diff --git a/include/theia/theia.h b/include/theia/theia.h index e3b429883..9f3b843c5 100644 --- a/include/theia/theia.h +++ b/include/theia/theia.h @@ -181,6 +181,8 @@ #include "theia/sfm/pose/relative_pose_from_two_points_with_known_rotation.h" #include "theia/sfm/pose/seven_point_fundamental_matrix.h" #include "theia/sfm/pose/sim_transform_partial_rotation.h" +#include "theia/sfm/pose/ten_point_radial_distortion_fundamental_matrix.h" +#include "theia/sfm/pose/ten_point_radial_distortion_fundamental_matrix_helper.h" #include "theia/sfm/pose/test_util.h" #include "theia/sfm/pose/three_point_relative_pose_partial_rotation.h" #include "theia/sfm/pose/two_point_pose_partial_rotation.h" diff --git a/src/theia/CMakeLists.txt b/src/theia/CMakeLists.txt index dc68e475f..c77a171d1 100644 --- a/src/theia/CMakeLists.txt +++ b/src/theia/CMakeLists.txt @@ -166,6 +166,8 @@ set(THEIA_SRC sfm/pose/relative_pose_from_two_points_with_known_rotation.cc sfm/pose/seven_point_fundamental_matrix.cc sfm/pose/sim_transform_partial_rotation.cc + sfm/pose/ten_point_radial_distortion_fundamental_matrix.cc + sfm/pose/ten_point_radial_distortion_fundamental_matrix_helper.cc sfm/pose/test_util.cc sfm/pose/three_point_relative_pose_partial_rotation.cc sfm/pose/two_point_pose_partial_rotation.cc @@ -328,6 +330,7 @@ if (BUILD_TESTING) gtest(sfm/pose/relative_pose_from_two_points_with_known_rotation) gtest(sfm/pose/seven_point_fundamental_matrix) gtest(sfm/pose/sim_transform_partial_rotation) + gtest(sfm/pose/ten_point_radial_distortion_fundamental_matrix) gtest(sfm/pose/three_point_relative_pose_partial_rotation) gtest(sfm/pose/two_point_pose_partial_rotation) gtest(sfm/reconstruction) diff --git a/src/theia/sfm/pose/ten_point_radial_distortion_fundamental_matrix.cc b/src/theia/sfm/pose/ten_point_radial_distortion_fundamental_matrix.cc new file mode 100644 index 000000000..bf3e5841a --- /dev/null +++ b/src/theia/sfm/pose/ten_point_radial_distortion_fundamental_matrix.cc @@ -0,0 +1,150 @@ +// Copyright (C) 2019 The Regents of the University of California (Regents). +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// +// * Neither the name of The Regents or University of California nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Please contact the author of this library if you have any questions. +// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu) + +// This file was created by Steffen Urban (urbste@googlemail.com) or +// company address (steffen.urban@zeiss.com) +// January 2019 + +#include "theia/sfm/pose/ten_point_radial_distortion_fundamental_matrix.h" +#include "theia/sfm/pose/ten_point_radial_distortion_fundamental_matrix_helper.h" + +#include +#include +#include + +namespace theia { + +using Eigen::Matrix; +using Eigen::MatrixXd; +using Eigen::Vector3d; +using Eigen::Vector2d; +using Eigen::Matrix3d; +using Eigen::Vector4d; +using Vector6d = Eigen::Matrix; +using Vector10d = Eigen::Matrix; +using Matrix102d = Eigen::Matrix; +using Matrix210d = Eigen::Matrix; + +bool TenPointRadialDistortionFundamentalMatrix( + const std::vector& normalized_feature_points_left, + const std::vector& normalized_feature_points_right, + std::vector* results, double lmin, + double lmax) { + Matrix102d X; + Matrix102d U; + for (int i = 0; i < 10; ++i) { + X.row(i) = normalized_feature_points_left[i]; + U.row(i) = normalized_feature_points_right[i]; + } + + Vector10d Z1; + Vector10d Z2; + Matrix A; + Z1.array() = + X.col(0).array() * X.col(0).array() + X.col(1).array() * X.col(1).array(); + Z2.array() = + U.col(0).array() * U.col(0).array() + U.col(1).array() * U.col(1).array(); + + A.col(0).array() = X.col(0).array() * U.col(0).array(); + A.col(1).array() = X.col(0).array() * U.col(1).array(); + A.col(2).array() = X.col(1).array() * U.col(0).array(); + A.col(3).array() = X.col(1).array() * U.col(1).array(); + A.col(4).array() = U.col(0).array() * Z1.array(); + A.col(5).array() = U.col(0).array(); + A.col(6).array() = U.col(1).array() * Z1.array(); + A.col(7).array() = U.col(1).array(); + A.col(8).array() = X.col(0).array() * Z2.array(); + A.col(9).array() = X.col(0).array(); + A.col(10).array() = X.col(1).array() * Z2.array(); + A.col(11).array() = X.col(1).array(); + A.col(12).array() = Z1.array() * Z2.array(); + A.col(13).array() = Z1.array(); + A.col(14).array() = Z2.array(); + A.col(15).fill(1.0); + + const Matrix Mr = + A.block<10, 10>(0, 0).lu().solve(A.block<10, 6>(0, 10)); + + Matrix params; + + params << Mr(5, 0), Mr(5, 1), -Mr(4, 0), -Mr(4, 1), Mr(5, 2), Mr(5, 3), + Mr(5, 4) - Mr(4, 2), Mr(5, 5) - Mr(4, 3), -Mr(4, 4), -Mr(4, 5), Mr(7, 0), + Mr(7, 1), -Mr(6, 0), -Mr(6, 1), Mr(7, 2), Mr(7, 3), Mr(7, 4) - Mr(6, 2), + Mr(7, 5) - Mr(6, 3), -Mr(6, 4), -Mr(6, 5), Mr(9, 0), Mr(9, 1) - Mr(8, 0), + -Mr(8, 1), Mr(9, 2), Mr(9, 4), Mr(9, 3) - Mr(8, 2), -Mr(8, 3), + Mr(9, 5) - Mr(8, 4), -Mr(8, 5); + + Matrix210d Ls; + int nsols = TenPointRadialDistortionFundamentalMatrixHelper(params, Ls); + + if (nsols > 0) { + Vector4d m1; + Vector6d m2; + Vector6d m3; + Vector10d b; + + b << Mr(5, 0), Mr(5, 1), -Mr(4, 0), -Mr(4, 1), Mr(5, 2), Mr(5, 3), + Mr(5, 4) - Mr(4, 2), Mr(5, 5) - Mr(4, 3), -Mr(4, 4), -Mr(4, 5); + + for (int i = 0; i < nsols; i++) { + const double l1 = Ls(0, i); + const double l2 = Ls(1, i); + + if (l1 < lmin || l2 < lmin || l1 > lmax || l2 > lmax) { + continue; + } + + const double l1l1 = l1 * l1; + const double l1l2 = l1 * l2; + double f23; + + m1 << l1l2, l1, l2, 1; + m2 << l1l2 * l1, l1l1, l1l2, l1, l2, 1; + f23 = -b.block<6, 1>(4, 0).dot(m2) / b.block<4, 1>(0, 0).dot(m1); + m3 << l2 * f23, f23, l1l2, l1, l2, 1; + + RadialFundamentalMatrixResult res; + res.l1 = l1; + res.l2 = l2; + res.F << m3.dot(-Mr.row(0)), m3.dot(-Mr.row(1)), m3.dot(-Mr.row(9)), + m3.dot(-Mr.row(2)), m3.dot(-Mr.row(3)), f23, m3.dot(-Mr.row(5)), + m3.dot(-Mr.row(7)), 1.0; + results->push_back(res); + } + + return true; + } else + return false; +} +} // theia namespace diff --git a/src/theia/sfm/pose/ten_point_radial_distortion_fundamental_matrix.h b/src/theia/sfm/pose/ten_point_radial_distortion_fundamental_matrix.h new file mode 100644 index 000000000..40ca0f27d --- /dev/null +++ b/src/theia/sfm/pose/ten_point_radial_distortion_fundamental_matrix.h @@ -0,0 +1,79 @@ +// Copyright (C) 2019 The Regents of the University of California (Regents). +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// +// * Neither the name of The Regents or University of California nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Please contact the author of this library if you have any questions. +// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu) + +// This file was created by Steffen Urban (urbste@googlemail.com) or +// company address (steffen.urban@zeiss.com) +// January 2019 + +// This solver is taken from http://cmp.felk.cvut.cz/~hellej1/ and implements +// the ten point (two-sided) radial distortion fundamental matrix solver +// (H10_l1l2). +// The corresponding paper is: +// "Efficient Solution to the Epipolar Geometry for Radially Distorted Cameras", +// Zuzana Kukelova et al. ICCV 2015 + +#ifndef THEIA_SFM_POSE_TEN_POINT_RADIAL_DISTORTION_FUNDAMENTAL_MATRIX_H_ +#define THEIA_SFM_POSE_TEN_POINT_RADIAL_DISTORTION_FUNDAMENTAL_MATRIX_H_ + +#include +#include + +#include "theia/sfm/pose/ten_point_radial_distortion_fundamental_matrix.h" + +namespace theia { + +struct RadialFundamentalMatrixResult { + Eigen::Matrix3d F; + double l1; + double l2; +}; + +// Input: +// normalized_feature_points_left - ten normalized image positions (inv(K)*p) +// normalized_feature_points_right - ten normalized image positions (inv(K)*p) +// lmin - minimum radial distortion (can be used to speed up ransac loops) +// lmax - maximum radial distortion (can be used to speed up ransac loops) +// Output: bool - returns true if at least one solution was found +// Return: RadialFundamentalMatrixResult - struct that contains fundamental +// matrix and a radial +// distortion parameter for each image (i.e. two-sided, called: H10_l1l2 in the +// paper) +bool TenPointRadialDistortionFundamentalMatrix( + const std::vector& normalized_feature_points_left, + const std::vector& normalized_feature_points_right, + std::vector* results, double lmin = -5.0, + double lmax = 0.0); +} + +#endif diff --git a/src/theia/sfm/pose/ten_point_radial_distortion_fundamental_matrix_helper.cc b/src/theia/sfm/pose/ten_point_radial_distortion_fundamental_matrix_helper.cc new file mode 100644 index 000000000..a23590e65 --- /dev/null +++ b/src/theia/sfm/pose/ten_point_radial_distortion_fundamental_matrix_helper.cc @@ -0,0 +1,288 @@ +// Copyright (C) 2019 The Regents of the University of California (Regents). +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// +// * Neither the name of The Regents or University of California nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Please contact the author of this library if you have any questions. +// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu) + +// This file was created by Steffen Urban (urbste@googlemail.com) or +// company address (steffen.urban@zeiss.com) +// January 2019 + +#include +#include + +#include "theia/sfm/pose/ten_point_radial_distortion_fundamental_matrix_helper.h" + +namespace theia { + +using Eigen::Matrix; +using Matrix10d = Eigen::Matrix; + +int TenPointRadialDistortionFundamentalMatrixHelper( + Eigen::Matrix& pr, Eigen::Matrix& sols) { + Matrix c; + + c(0) = pr(0) * pr(14) - pr(4) * pr(10); + c(1) = pr(0) * pr(16) + pr(2) * pr(14) - pr(4) * pr(12) - pr(6) * pr(10); + c(2) = pr(2) * pr(18) - pr(8) * pr(12); + c(3) = pr(1) * pr(15) - pr(5) * pr(11); + c(4) = pr(1) * pr(17) + pr(3) * pr(15) - pr(5) * pr(13) - pr(7) * pr(11); + c(5) = pr(0) * pr(15) + pr(1) * pr(14) - pr(4) * pr(11) - pr(5) * pr(10); + c(6) = pr(0) * pr(17) + pr(1) * pr(16) + pr(2) * pr(15) + pr(3) * pr(14) - + pr(4) * pr(13) - pr(5) * pr(12) - pr(6) * pr(11) - pr(7) * pr(10); + c(7) = pr(0) * pr(18) + pr(2) * pr(16) - pr(6) * pr(12) - pr(8) * pr(10); + c(8) = pr(0) * pr(19) + pr(1) * pr(18) + pr(2) * pr(17) + pr(3) * pr(16) - + pr(6) * pr(13) - pr(7) * pr(12) - pr(8) * pr(11) - pr(9) * pr(10); + c(9) = pr(2) * pr(19) + pr(3) * pr(18) - pr(8) * pr(13) - pr(9) * pr(12); + c(10) = pr(1) * pr(19) + pr(3) * pr(17) - pr(7) * pr(13) - pr(9) * pr(11); + c(11) = pr(3) * pr(19) - pr(9) * pr(13); + c(12) = pr(0) * pr(23) - pr(4) * pr(20); + c(13) = pr(1) * pr(23) + pr(0) * pr(25) - pr(4) * pr(21) - pr(5) * pr(20); + c(14) = pr(2) * pr(24) - pr(8) * pr(20); + c(15) = pr(3) * pr(24) + pr(2) * pr(27) - pr(8) * pr(21) - pr(9) * pr(20); + c(16) = pr(1) * pr(26) - pr(5) * pr(22); + c(17) = pr(0) * pr(24) + pr(2) * pr(23) - pr(6) * pr(20); + c(18) = pr(0) * pr(26) + pr(1) * pr(25) - pr(4) * pr(22) - pr(5) * pr(21); + c(19) = pr(1) * pr(24) + pr(3) * pr(23) + pr(0) * pr(27) + pr(2) * pr(25) - + pr(6) * pr(21) - pr(7) * pr(20); + c(20) = pr(0) * pr(28) + pr(1) * pr(27) + pr(2) * pr(26) + pr(3) * pr(25) - + pr(6) * pr(22) - pr(7) * pr(21); + c(21) = pr(2) * pr(28) + pr(3) * pr(27) - pr(8) * pr(22) - pr(9) * pr(21); + c(22) = pr(1) * pr(28) + pr(3) * pr(26) - pr(7) * pr(22); + c(23) = pr(3) * pr(28) - pr(9) * pr(22); + c(24) = pr(10) * pr(23) - pr(14) * pr(20); + c(25) = pr(11) * pr(23) + pr(10) * pr(25) - pr(14) * pr(21) - pr(15) * pr(20); + c(26) = pr(12) * pr(24) - pr(18) * pr(20); + c(27) = pr(13) * pr(24) + pr(12) * pr(27) - pr(18) * pr(21) - pr(19) * pr(20); + c(28) = pr(11) * pr(26) - pr(15) * pr(22); + c(29) = pr(10) * pr(24) + pr(12) * pr(23) - pr(16) * pr(20); + c(30) = pr(10) * pr(26) + pr(11) * pr(25) - pr(14) * pr(22) - pr(15) * pr(21); + c(31) = pr(11) * pr(24) + pr(13) * pr(23) + pr(10) * pr(27) + + pr(12) * pr(25) - pr(16) * pr(21) - pr(17) * pr(20); + c(32) = pr(10) * pr(28) + pr(11) * pr(27) + pr(12) * pr(26) + + pr(13) * pr(25) - pr(16) * pr(22) - pr(17) * pr(21); + c(33) = pr(12) * pr(28) + pr(13) * pr(27) - pr(18) * pr(22) - pr(19) * pr(21); + c(34) = pr(11) * pr(28) + pr(13) * pr(26) - pr(17) * pr(22); + c(35) = pr(13) * pr(28) - pr(19) * pr(22); + + Matrix M; + M.fill(0.0); + + M(0) = c(0); + M(61) = c(0); + M(82) = c(0); + M(144) = c(0); + M(2) = c(1); + M(64) = c(1); + M(85) = c(1); + M(148) = c(1); + M(9) = c(2); + M(72) = c(2); + M(93) = c(2); + M(156) = c(2); + M(3) = c(3); + M(66) = c(3); + M(87) = c(3); + M(150) = c(3); + M(7) = c(4); + M(70) = c(4); + M(91) = c(4); + M(154) = c(4); + M(1) = c(5); + M(63) = c(5); + M(84) = c(5); + M(147) = c(5); + M(4) = c(6); + M(67) = c(6); + M(88) = c(6); + M(151) = c(6); + M(5) = c(7); + M(68) = c(7); + M(89) = c(7); + M(152) = c(7); + M(8) = c(8); + M(71) = c(8); + M(92) = c(8); + M(155) = c(8); + M(12) = c(9); + M(75) = c(9); + M(96) = c(9); + M(158) = c(9); + M(11) = c(10); + M(74) = c(10); + M(95) = c(10); + M(157) = c(10); + M(15) = c(11); + M(77) = c(11); + M(98) = c(11); + M(159) = c(11); + M(20) = c(12); + M(102) = c(12); + M(165) = c(12); + M(21) = c(13); + M(104) = c(13); + M(168) = c(13); + M(25) = c(14); + M(109) = c(14); + M(173) = c(14); + M(28) = c(15); + M(112) = c(15); + M(176) = c(15); + M(26) = c(16); + M(110) = c(16); + M(174) = c(16); + M(22) = c(17); + M(105) = c(17); + M(169) = c(17); + M(23) = c(18); + M(107) = c(18); + M(171) = c(18); + M(24) = c(19); + M(108) = c(19); + M(172) = c(19); + M(27) = c(20); + M(111) = c(20); + M(175) = c(20); + M(31) = c(21); + M(115) = c(21); + M(178) = c(21); + M(30) = c(22); + M(114) = c(22); + M(177) = c(22); + M(34) = c(23); + M(117) = c(23); + M(179) = c(23); + M(40) = c(24); + M(122) = c(24); + M(185) = c(24); + M(41) = c(25); + M(124) = c(25); + M(188) = c(25); + M(45) = c(26); + M(129) = c(26); + M(193) = c(26); + M(48) = c(27); + M(132) = c(27); + M(196) = c(27); + M(46) = c(28); + M(130) = c(28); + M(194) = c(28); + M(42) = c(29); + M(125) = c(29); + M(189) = c(29); + M(43) = c(30); + M(127) = c(30); + M(191) = c(30); + M(44) = c(31); + M(128) = c(31); + M(192) = c(31); + M(47) = c(32); + M(131) = c(32); + M(195) = c(32); + M(51) = c(33); + M(135) = c(33); + M(198) = c(33); + M(50) = c(34); + M(134) = c(34); + M(197) = c(34); + M(54) = c(35); + M(137) = c(35); + M(199) = c(35); + + colEchelonForm(M, 1e-12); + + Matrix10d A; + A.fill(0.0); + + A(0, 2) = 1.000000; + A(1, 4) = 1.000000; + A(2, 5) = 1.000000; + A(3, 7) = 1.000000; + A(4, 8) = 1.000000; + A(5, 9) = 1.000000; + A(6, 0) = -M(19, 9); + A(6, 1) = -M(18, 9); + A(6, 2) = -M(17, 9); + A(6, 3) = -M(16, 9); + A(6, 4) = -M(15, 9); + A(6, 5) = -M(14, 9); + A(6, 6) = -M(13, 9); + A(6, 7) = -M(12, 9); + A(6, 8) = -M(11, 9); + A(6, 9) = -M(10, 9); + A(7, 0) = -M(19, 8); + A(7, 1) = -M(18, 8); + A(7, 2) = -M(17, 8); + A(7, 3) = -M(16, 8); + A(7, 4) = -M(15, 8); + A(7, 5) = -M(14, 8); + A(7, 6) = -M(13, 8); + A(7, 7) = -M(12, 8); + A(7, 8) = -M(11, 8); + A(7, 9) = -M(10, 8); + A(8, 0) = -M(19, 7); + A(8, 1) = -M(18, 7); + A(8, 2) = -M(17, 7); + A(8, 3) = -M(16, 7); + A(8, 4) = -M(15, 7); + A(8, 5) = -M(14, 7); + A(8, 6) = -M(13, 7); + A(8, 7) = -M(12, 7); + A(8, 8) = -M(11, 7); + A(8, 9) = -M(10, 7); + A(9, 0) = -M(19, 6); + A(9, 1) = -M(18, 6); + A(9, 2) = -M(17, 6); + A(9, 3) = -M(16, 6); + A(9, 4) = -M(15, 6); + A(9, 5) = -M(14, 6); + A(9, 6) = -M(13, 6); + A(9, 7) = -M(12, 6); + A(9, 8) = -M(11, 6); + A(9, 9) = -M(10, 6); + + Eigen::EigenSolver eig(A); + Matrix, 10, 2> esols; + esols.col(0).array() = + eig.eigenvectors().row(2).array() / eig.eigenvectors().row(0).array(); + esols.col(1).array() = + eig.eigenvectors().row(1).array() / eig.eigenvectors().row(0).array(); + + int nsols = 0; + for (int i = 0; i < 10; i++) { + if (esols.row(i).imag().isZero(100.0 * + std::numeric_limits::epsilon())) + sols.col(nsols++) = esols.row(i).real(); + } + + return nsols; +} +} diff --git a/src/theia/sfm/pose/ten_point_radial_distortion_fundamental_matrix_helper.h b/src/theia/sfm/pose/ten_point_radial_distortion_fundamental_matrix_helper.h new file mode 100644 index 000000000..e55866d6e --- /dev/null +++ b/src/theia/sfm/pose/ten_point_radial_distortion_fundamental_matrix_helper.h @@ -0,0 +1,98 @@ +// Copyright (C) 2019 The Regents of the University of California (Regents). +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// +// * Neither the name of The Regents or University of California nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Please contact the author of this library if you have any questions. +// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu) + +// This file was created by Steffen Urban (urbste@googlemail.com) or +// company address (steffen.urban@zeiss.com) +// January 2019 + +#ifndef THEIA_SFM_POSE_TEN_POINT_RADIAL_DISTORTION_FUNDAMENTAL_MATRIX_HELPER_H_ +#define THEIA_SFM_POSE_TEN_POINT_RADIAL_DISTORTION_FUNDAMENTAL_MATRIX_HELPER_H_ + +#include +#include + +namespace theia { + +int TenPointRadialDistortionFundamentalMatrixHelper( + Eigen::Matrix& pr, Eigen::Matrix& sols); + + +template +inline void colEchelonForm(Eigen::MatrixBase& M, + double pivtol = 1e-12) { + typedef typename Derived::Scalar Scalar; + + int n = M.rows(); + int m = M.cols(); + int i = 0, j = 0, k = 0; + int col = 0; + Scalar p, tp; + + while ((i < m) && (j < n)) { + p = std::numeric_limits::min(); + col = i; + + for (k = i; k < m; k++) { + tp = std::abs(M(j, k)); + if (tp > p) { + p = tp; + col = k; + } + } + + if (p < Scalar(pivtol)) { + M.block(j, i, 1, m - i).setZero(); + j++; + } else { + if (col != i) + M.block(j, i, n - j, 1).swap(M.block(j, col, n - j, 1)); + + M.block(j + 1, i, n - j - 1, 1) /= M(j, i); + M(j, i) = 1.0; + + for (k = 0; k < m; k++) { + if (k == i) + continue; + + M.block(j, k, n - j, 1) -= M(j, k) * M.block(j, i, n - j, 1); + } + + i++; + j++; + } + } +} +} + +#endif diff --git a/src/theia/sfm/pose/ten_point_radial_distortion_fundamental_matrix_test.cc b/src/theia/sfm/pose/ten_point_radial_distortion_fundamental_matrix_test.cc new file mode 100644 index 000000000..ef7b56607 --- /dev/null +++ b/src/theia/sfm/pose/ten_point_radial_distortion_fundamental_matrix_test.cc @@ -0,0 +1,291 @@ +// Copyright (C) 2019 The Regents of the University of California (Regents). +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are +// met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above +// copyright notice, this list of conditions and the following +// disclaimer in the documentation and/or other materials provided +// with the distribution. +// +// * Neither the name of The Regents or University of California nor the +// names of its contributors may be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// +// Please contact the author of this library if you have any questions. +// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu) + +// This file was created by Steffen Urban (urbste@googlemail.com) or +// company address (steffen.urban@zeiss.com) +// January 2019 + +#include +#include +#include +#include "gtest/gtest.h" + +#include "theia/math/util.h" +#include "theia/sfm/pose/ten_point_radial_distortion_fundamental_matrix.h" +#include "theia/sfm/pose/test_util.h" +#include "theia/util/random.h" + +namespace theia { +namespace { +using Eigen::AngleAxisd; +using Eigen::Matrix3d; +using Eigen::Quaterniond; +using Eigen::Vector2d; +using Eigen::Vector3d; + +RandomNumberGenerator rng(53); + +void DistortPoint(const Eigen::Vector3d& point_in_camera, + const double focal_length, const double radial_distortion, + Eigen::Vector2d& distorted_point) { + Eigen::Vector2d point_in_cam_persp_div; + point_in_cam_persp_div[0] = + focal_length * point_in_camera[0] / point_in_camera[2]; + point_in_cam_persp_div[1] = + focal_length * point_in_camera[1] / point_in_camera[2]; + // see also division_undistortion_camera_model.h + const double r_u_sq = point_in_cam_persp_div[0] * point_in_cam_persp_div[0] + + point_in_cam_persp_div[1] * point_in_cam_persp_div[1]; + + const double denom = 2.0 * radial_distortion * r_u_sq; + const double inner_sqrt = 1.0 - 4.0 * radial_distortion * r_u_sq; + + // If the denominator is nearly zero then we can evaluate the distorted + // coordinates as k or r_u^2 goes to zero. Both evaluate to the identity. + if (std::abs(denom) < std::numeric_limits::epsilon() || + inner_sqrt < 0.0) { + distorted_point = point_in_cam_persp_div; + } else { + const double scale = (1.0 - std::sqrt(inner_sqrt)) / denom; + distorted_point = point_in_cam_persp_div * scale; + } +} + +void UndistortPoint(const Eigen::Vector2d& distorted_point, + const double focal_length, const double radial_distortion, + Eigen::Vector3d& undistorted_point) { + double px = distorted_point[0]; + double py = distorted_point[1]; + // The squared radius of the distorted image point. + const double r_d_sq = px * px + py * py; + + const double undistortion = 1.0 / (1.0 + radial_distortion * r_d_sq); + undistorted_point[0] = px * undistortion / focal_length; + undistorted_point[1] = py * undistortion / focal_length; + undistorted_point[2] = 1.0; +} + +// Creates a test scenario from ground truth 3D points and ground truth rotation +// and translation. Projection noise is optional (set to 0 for no +// noise). +void GenerateDistortedImagePoints( + const std::vector& points_3d, + const double projection_noise_std_dev, const Quaterniond& expected_rotation, + const Vector3d& expected_translation, const double focal_length1, + const double focal_length2, const double radial_distortion1, + const double radial_distortion2, + std::vector* image_1_points_normalized, + std::vector* image_2_points_normalized, + std::vector* image_1_points, + std::vector* image_2_points) { + image_1_points->reserve(points_3d.size()); + image_2_points->reserve(points_3d.size()); + + for (int i = 0; i < points_3d.size(); i++) { + Vector3d point3_cam1 = points_3d[i]; + Vector3d point3_cam2 = + (expected_rotation * points_3d[i] + expected_translation); + + Vector2d distorted_point1, distorted_point2; + DistortPoint(point3_cam1, focal_length1, radial_distortion1, + distorted_point1); + DistortPoint(point3_cam2, focal_length2, radial_distortion2, + distorted_point2); + + image_1_points->push_back(distorted_point1); + image_2_points->push_back(distorted_point2); + + if (projection_noise_std_dev) { + for (int i = 0; i < points_3d.size(); i++) { + AddNoiseToProjection(projection_noise_std_dev, &rng, + &((*image_1_points)[i])); + AddNoiseToProjection(projection_noise_std_dev, &rng, + &((*image_2_points)[i])); + } + } + + Vector3d points2d_1, points2d_2; + // normalize points with focal length (and principal point) estimate for + // estimation + UndistortPoint(distorted_point1, focal_length1, 0.0, points2d_1); + UndistortPoint(distorted_point2, focal_length2, 0.0, points2d_2); + + image_1_points_normalized->push_back(points2d_1.hnormalized()); + image_2_points_normalized->push_back(points2d_2.hnormalized()); + } +} + +Vector3d ProjectCameraToCameraF(const Matrix3d& fundamental_matrix, + const Vector3d& point_image_1, + const Vector3d& point_image_2, + Vector3d* output_vector) { + const Eigen::Vector3d ls = fundamental_matrix * point_image_1; + const double ay = ls[0] * point_image_2[1]; + const double bx = ls[1] * point_image_2[0]; + const double ac = ls[0] * ls[2]; + const double bc = ls[1] * ls[2]; + + const double dd = ls[0] * ls[0] + ls[1] * ls[1]; + + (*output_vector) << (ls[1] * (bx - ay) - ac) / dd, + (ls[0] * (ay - bx) - bc) / dd, 1.0; +} + +double CheckRadialSymmetricError( + const RadialFundamentalMatrixResult& radial_fundamental_matrix, + const Vector2d& pt_left, const Vector2d& pt_right, + const double focal_length1, const double focal_length2) { + Vector3d bearing_vector_left, bearing_vector_right; + UndistortPoint(pt_left, focal_length1, radial_fundamental_matrix.l1, + bearing_vector_left); + UndistortPoint(pt_right, focal_length2, radial_fundamental_matrix.l2, + bearing_vector_right); + + Eigen::Vector3d ray2_in_1, ray1_in_2; + ProjectCameraToCameraF(radial_fundamental_matrix.F, bearing_vector_right, + bearing_vector_left, &ray2_in_1); + ProjectCameraToCameraF(radial_fundamental_matrix.F.transpose(), + bearing_vector_left, bearing_vector_right, &ray1_in_2); + + Vector2d pt_left_from_right, pt_right_from_left; + DistortPoint(ray2_in_1, focal_length1, radial_fundamental_matrix.l1, + pt_left_from_right); + DistortPoint(ray1_in_2, focal_length2, radial_fundamental_matrix.l2, + pt_right_from_left); + + double dleft_x = pt_left(0) - pt_left_from_right(0); + double dleft_y = pt_left(1) - pt_left_from_right(1); + + double dright_x = pt_right(0) - pt_right_from_left(0); + double dright_y = pt_right(1) - pt_right_from_left(1); + + double sym_error = 0.5 * (dleft_x * dleft_x + dleft_y * dleft_y + + dright_x * dright_x + dright_y * dright_y); + + return sym_error; +} + +// Run a test for the homography with at least 4 points. +void SixPointHomographyWithNoiseTest( + const std::vector& points_3d, + const double projection_noise_std_dev, const Quaterniond& expected_rotation, + const Vector3d& expected_translation, const double kMaxSymmetricError, + const double focal_length1, const double focal_length2, + const double radial_distortion1, const double radial_distortion2) { + std::vector image_1_points, image_points_1_normalized; + std::vector image_2_points, image_points_2_normalized; + // generate distorted points for both cameras + GenerateDistortedImagePoints( + points_3d, projection_noise_std_dev, expected_rotation, + expected_translation, focal_length1, focal_length2, radial_distortion1, + radial_distortion2, &image_points_1_normalized, + &image_points_2_normalized, &image_1_points, &image_2_points); + // Compute two-sided radial distortion homography matrix. + std::vector radial_fundamental_matrix_result; + EXPECT_TRUE(TenPointRadialDistortionFundamentalMatrix( + image_points_1_normalized, image_points_2_normalized, + &radial_fundamental_matrix_result)); + bool one_correct_solution = false; + for (int i = 0; i < radial_fundamental_matrix_result.size(); ++i) { + // we need to scale the radial distortion values, + // since we used normalized image points for estimation + radial_fundamental_matrix_result[i].l1 /= (focal_length1 * focal_length1); + radial_fundamental_matrix_result[i].l2 /= (focal_length2 * focal_length2); + + double sym_error = 0.0; + for (int p = 0; p < image_1_points.size(); ++p) { + sym_error += CheckRadialSymmetricError( + radial_fundamental_matrix_result[i], image_1_points[p], + image_2_points[p], focal_length1, focal_length2); + } + sym_error /= (double)image_1_points.size(); + if (sym_error < kMaxSymmetricError) { + one_correct_solution = true; + } + } + EXPECT_TRUE(one_correct_solution); +} + +void BasicTest() { + const std::vector points_3d = { + Vector3d(-1.0, 3.0, 1.0), Vector3d(1.0, -1.0, 2.0), + Vector3d(-1.0, 1.0, 3.0), Vector3d(2.0, 1.0, 1.0), + Vector3d(3.0, 1.0, 2.0), Vector3d(2.0, 2.0, 3.0), + Vector3d(3.0, -1.0, 2.0), Vector3d(-2.0, 2.0, 3.0), + Vector3d(-3.0, 1.0, 4.0), Vector3d(2.0, 2.0, 4.0)}; + + const Quaterniond soln_rotation( + AngleAxisd(DegToRad(13.0), Vector3d(0.0, 0.0, 1.0))); + const Vector3d soln_translation(1.0, 1.0, 1.0); + const double kNoise = 0.0 / 512.0; + const double kMaxSymmetricError = 1e-4; + + const double focal_length1 = 1500.0; + const double focal_length2 = 1600.0; + const double radial_distortion1 = -1e-7; + const double radial_distortion2 = -2e-7; + + SixPointHomographyWithNoiseTest( + points_3d, kNoise, soln_rotation, soln_translation, kMaxSymmetricError, + focal_length1, focal_length2, radial_distortion1, radial_distortion2); +} + +TEST(SixPointRadialHomography, BasicTest) { BasicTest(); } + +TEST(SixPointRadialHomography, NoiseTest) { + const std::vector points_3d = { + Vector3d(-1.0, 3.0, 1.0), Vector3d(1.0, -1.0, 2.0), + Vector3d(-1.0, 1.0, 3.0), Vector3d(2.0, 1.0, 1.0), + Vector3d(3.0, 1.0, 2.0), Vector3d(2.0, 2.0, 3.0), + Vector3d(3.0, -1.0, 2.0), Vector3d(-2.0, 2.0, 3.0), + Vector3d(-3.0, 1.0, 4.0), Vector3d(2.0, 2.0, 4.0)}; + + const Quaterniond soln_rotation( + AngleAxisd(DegToRad(13.0), Vector3d(0.0, 0.0, 1.0))); + const Vector3d soln_translation(1.0, 1.0, 1.0); + const double kNoise = 0.5; + const double kMaxSymmetricError = 4.0; + + const double focal_length1 = 1500.0; + const double focal_length2 = 1600.0; + const double radial_distortion1 = -1e-7; + const double radial_distortion2 = -2e-7; + + SixPointHomographyWithNoiseTest( + points_3d, kNoise, soln_rotation, soln_translation, kMaxSymmetricError, + focal_length1, focal_length2, radial_distortion1, radial_distortion2); +} + +} // namespace +} // namespace theia