diff --git a/src/aliceVision/multiview/essentialF10Solver.cpp b/src/aliceVision/multiview/essentialF10Solver.cpp index 3a1739f055..4a45cef9f0 100644 --- a/src/aliceVision/multiview/essentialF10Solver.cpp +++ b/src/aliceVision/multiview/essentialF10Solver.cpp @@ -8,13 +8,198 @@ #include "essentialF10Solver.hpp" #include +#include namespace aliceVision { -int F10RelativePose(const Mat& X, const Mat& U, std::vector* F, std::vector* L) +/** + * @brief Computes column echelon form of a matrix + * @author Jan Heller, adapted to aliceVision by Michal Polic + */ +template +void colEchelonForm(Eigen::MatrixBase& M, double pivtol = 1e-12) { - eigen_assert((X.rows() == 10 && X.cols() == 2) && "The first parameter (x) must be a 10x2 matrix"); - eigen_assert((U.rows() == 10 && U.cols() == 2) && "The second parameter (u) must be a 10x2 matrix"); + using Scalar = typename Derived::Scalar; + + const int n = M.rows(); + const int m = M.cols(); + + Eigen::Index i = 0; + Eigen::Index j = 0; + Eigen::Index k = 0; + Eigen::Index 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; + } + } +} + +template +int f10e_gb(const Eigen::Matrix& params, Eigen::MatrixBase& sols, double pivtol = 1e-16) +{ + Eigen::Matrix c; + + c(0) = params(0) * params(14) - params(4) * params(10); + c(1) = params(0) * params(16) + params(2) * params(14) - params(4) * params(12) - params(6) * params(10); + c(2) = params(2) * params(18) - params(8) * params(12); + c(3) = params(1) * params(15) - params(5) * params(11); + c(4) = params(1) * params(17) + params(3) * params(15) - params(5) * params(13) - params(7) * params(11); + c(5) = params(0) * params(15) + params(1) * params(14) - params(4) * params(11) - params(5) * params(10); + c(6) = params(0) * params(17) + params(1) * params(16) + params(2) * params(15) + params(3) * params(14) - params(4) * params(13) - params(5) * params(12) - params(6) * params(11) - params(7) * params(10); + c(7) = params(0) * params(18) + params(2) * params(16) - params(6) * params(12) - params(8) * params(10); + c(8) = params(0) * params(19) + params(1) * params(18) + params(2) * params(17) + params(3) * params(16) - params(6) * params(13) - params(7) * params(12) - params(8) * params(11) - params(9) * params(10); + c(9) = params(2) * params(19) + params(3) * params(18) - params(8) * params(13) - params(9) * params(12); + c(10) = params(1) * params(19) + params(3) * params(17) - params(7) * params(13) - params(9) * params(11); + c(11) = params(3) * params(19) - params(9) * params(13); + c(12) = params(0) * params(23) - params(4) * params(20); + c(13) = params(1) * params(23) + params(0) * params(25) - params(4) * params(21) - params(5) * params(20); + c(14) = params(2) * params(24) - params(8) * params(20); + c(15) = params(3) * params(24) + params(2) * params(27) - params(8) * params(21) - params(9) * params(20); + c(16) = params(1) * params(26) - params(5) * params(22); + c(17) = params(0) * params(24) + params(2) * params(23) - params(6) * params(20); + c(18) = params(0) * params(26) + params(1) * params(25) - params(4) * params(22) - params(5) * params(21); + c(19) = params(1) * params(24) + params(3) * params(23) + params(0) * params(27) + params(2) * params(25) - params(6) * params(21) - params(7) * params(20); + c(20) = params(0) * params(28) + params(1) * params(27) + params(2) * params(26) + params(3) * params(25) - params(6) * params(22) - params(7) * params(21); + c(21) = params(2) * params(28) + params(3) * params(27) - params(8) * params(22) - params(9) * params(21); + c(22) = params(1) * params(28) + params(3) * params(26) - params(7) * params(22); + c(23) = params(3) * params(28) - params(9) * params(22); + c(24) = params(10) * params(23) - params(14) * params(20); + c(25) = params(11) * params(23) + params(10) * params(25) - params(14) * params(21) - params(15) * params(20); + c(26) = params(12) * params(24) - params(18) * params(20); + c(27) = params(13) * params(24) + params(12) * params(27) - params(18) * params(21) - params(19) * params(20); + c(28) = params(11) * params(26) - params(15) * params(22); + c(29) = params(10) * params(24) + params(12) * params(23) - params(16) * params(20); + c(30) = params(10) * params(26) + params(11) * params(25) - params(14) * params(22) - params(15) * params(21); + c(31) = params(11) * params(24) + params(13) * params(23) + params(10) * params(27) + params(12) * params(25) - params(16) * params(21) - params(17) * params(20); + c(32) = params(10) * params(28) + params(11) * params(27) + params(12) * params(26) + params(13) * params(25) - params(16) * params(22) - params(17) * params(21); + c(33) = params(12) * params(28) + params(13) * params(27) - params(18) * params(22) - params(19) * params(21); + c(34) = params(11) * params(28) + params(13) * params(26) - params(17) * params(22); + c(35) = params(13) * params(28) - params(19) * params(22); + + Eigen::Matrix M; + M.setZero(); + + 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, pivtol); + + Eigen::Matrix A; + A.setZero(); + + 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); + + const Eigen::EigenSolver > eig(A); + Eigen::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(Eigen::Index 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; +} + +int F10RelativePose(const Mat& X, const Mat& U, std::vector& F, std::vector& L) +{ + assert((X.rows() == 10 && X.cols() == 2) && "The first parameter (x) must be a 10x2 matrix"); + assert((U.rows() == 10 && U.cols() == 2) && "The second parameter (u) must be a 10x2 matrix"); Eigen::Matrix Z1; Eigen::Matrix Z2; @@ -40,62 +225,49 @@ int F10RelativePose(const Mat& X, const Mat& U, std::vector* F, std::vecto A.col(14).array() = Z2.array(); A.col(15).fill(1.0); - Eigen::Matrix Mr = A.block<10, 10>(0, 0).lu().solve(A.block<10, 6>(0, 10)); + const Eigen::Matrix mr = A.block<10, 10>(0, 0).lu().solve(A.block<10, 6>(0, 10)); Eigen::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); + 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); Mat Ls(2, 10); - int nsols = f10e_gb(params, Ls); + const int nsols = f10e_gb(params, Ls); + + if(nsols == 0) + return 0; + + Eigen::Matrix b; - if(nsols > 0) + 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); + + F.resize(nsols); + L.resize(nsols); + + for(Eigen::Index i = 0; i < nsols; ++i) { - Eigen::Matrix m1; - Eigen::Matrix m2; - Eigen::Matrix m3; - Eigen::Matrix b; + const double l1 = Ls(0, i); + const double l2 = Ls(1, i); + const double l1l1 = l1 * l1; + const double l1l2 = l1 * l2; - 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); + const Eigen::Matrix m1 = (Eigen::Matrix() << l1l2, l1, l2, 1).finished(); + const Eigen::Matrix m2 = (Eigen::Matrix() << l1l2 * l1, l1l1, l1l2, l1, l2, 1).finished(); - F->resize(nsols); - L->resize(nsols); + const double f23 = -b.block<6, 1>(4, 0).dot(m2) / b.block<4, 1>(0, 0).dot(m1); - for(int i = 0; i < nsols; ++i) - { - const double l1 = Ls(0, i); - const double l2 = Ls(1, i); - 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; - - (*L)[i] << l1, l2; - (*F)[i] << 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; - - //Fs(0, i) = m3.dot(-Mr.row(0)); - //Fs(1, i) = m3.dot(-Mr.row(2)); - //Fs(2, i) = m3.dot(-Mr.row(5)); - //Fs(3, i) = m3.dot(-Mr.row(1)); - //Fs(4, i) = m3.dot(-Mr.row(3)); - //Fs(5, i) = m3.dot(-Mr.row(7)); - //Fs(6, i) = m3.dot(-Mr.row(9)); - //Fs(7, i) = f23; - //Fs(8, i) = 1; - } + const Eigen::Matrix m3 = (Eigen::Matrix() << l2 * f23, f23, l1l2, l1, l2, 1).finished(); + + L.at(i) << l1, l2; + F.at(i) << 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; } return nsols; diff --git a/src/aliceVision/multiview/essentialF10Solver.hpp b/src/aliceVision/multiview/essentialF10Solver.hpp index c29d2e16d8..0057685a8b 100644 --- a/src/aliceVision/multiview/essentialF10Solver.hpp +++ b/src/aliceVision/multiview/essentialF10Solver.hpp @@ -19,205 +19,14 @@ namespace aliceVision { using Mat21 = Eigen::Matrix; -template -bool isZero(Scalar val) -{ - return (+val < (100.0 * std::numeric_limits::epsilon())) && - (-val < (100.0 * std::numeric_limits::epsilon())); -} - -/** - * @brief Computes column echelon form of a matrix - * @author Jan Heller, adapted to aliceVision by Michal Polic - */ -template -inline void colEchelonForm(Eigen::MatrixBase& M, double pivtol = 1e-12) -{ - using Scalar = typename Derived::Scalar; - - const int n = M.rows(); - const int m = M.cols(); - - int i = 0; - int j = 0; - int 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; - } - } -} - -template -int f10e_gb(Eigen::Matrix& pr, Eigen::MatrixBase& sols, double pivtol = 1e-16) -{ - Eigen::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); - - Eigen::Matrix M; - M.setZero(); - - 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, pivtol); - - Eigen::Matrix A; - A.setZero(); - - 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); - Eigen::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; -} - /** * @brief Computes the relative pose and two radial disortion coefficients for two cameras from 10 correspondences * Epipolar geometry F + l1 + l2 solver F10e: - * + * @code{.unparsed} * [F11 F12 F13] [u] * [x, y, 1+l1*(x^2+y^2)] * [F21 F22 F23] [v] = 0 * [F31 F32 F33] [1+l2*(u^2+v^2)] - * + * @endcode * @param[in] X 10x2 matrix of 2D distorted measurements (X == [x y]') * @param[in] U 10x2 matrix of 2D distorted measurements (U == [u v]') * @param[out] F list of candidate fundamental matrices solutions @@ -230,6 +39,6 @@ int f10e_gb(Eigen::Matrix& pr, Eigen::MatrixBase& sols, * The IEEE International Conference on Computer Vision (ICCV), * December, 2015, Santiago, Chile. */ -int F10RelativePose(const Mat& X, const Mat& U, std::vector* F, std::vector* L); +int F10RelativePose(const Mat& X, const Mat& U, std::vector& F, std::vector& L); } // namespace aliceVision diff --git a/src/aliceVision/multiview/essentialF10Solver_test.cpp b/src/aliceVision/multiview/essentialF10Solver_test.cpp index a42abc1489..1e337610c7 100644 --- a/src/aliceVision/multiview/essentialF10Solver_test.cpp +++ b/src/aliceVision/multiview/essentialF10Solver_test.cpp @@ -4,7 +4,7 @@ // v. 2.0. If a copy of the MPL was not distributed with this file, // You can obtain one at https://mozilla.org/MPL/2.0/. -#define BOOST_TEST_MODULE F10Solver +#define BOOST_TEST_MODULE essentialF10Solver #include @@ -13,20 +13,9 @@ #include - using namespace aliceVision; -bool isEqual(const Mat first, const Mat second) -{ - const double eps = std::max(first.cwiseAbs().maxCoeff(), second.cwiseAbs().maxCoeff()) * 1e-1; // there are very numericaly senzitive solutions as so as the stable ones - //std::cout << first << "\n\n"; - //std::cout << second << "\n\n"; - //std::cout << first - second << "\n\n"; - //std::cout << (first - second).cwiseAbs().maxCoeff() << " < " << eps << "\n\n\n"; - return ((first - second).cwiseAbs().maxCoeff() < eps); -} - -BOOST_AUTO_TEST_CASE(RelativePoseF10_8solutions) +BOOST_AUTO_TEST_CASE(RelativePoseF10_8_solutions) { // input data Mat X = Mat(10, 2); @@ -70,23 +59,23 @@ BOOST_AUTO_TEST_CASE(RelativePoseF10_8solutions) Mat2X resL4(2, 1); resL4 << -3.004792949356860e+01, 1.442424098845748e+02; resL.push_back(resL4); Mat2X resL5(2, 1); resL5 << 5.619131137084262e+00, 1.528997232119745e+01; resL.push_back(resL5); Mat2X resL6(2, 1); resL6 << -1.359819306123174e+00, -3.194231629423156e+02; resL.push_back(resL6); - Mat2X resL7(2, 1); resL7 << -7.668121663596985e+00, -1.015843563423398e+01; resL.push_back(resL7); - Mat2X resL8(2, 1); resL8 << -8.478427431999348e+00, -4.469547181112368e+00; resL.push_back(resL8); + Mat2X resL7(2, 1); resL7 << -7.668121663596985e+00, -1.015843563423398e+01; resL.push_back(resL7); + Mat2X resL8(2, 1); resL8 << -8.478427431999348e+00, -4.469547181112368e+00; resL.push_back(resL8); // process std::vector F; std::vector L; - F10RelativePose(X, U, &F, &L); + F10RelativePose(X, U, F, L); // test results if(resF.size() != F.size()) BOOST_CHECK(false); - for(int i = 0; i < resF.size(); ++i) - BOOST_CHECK(isEqual(resF.at(i), F.at(i)) && isEqual(resL.at(i), L.at(i))); + for(Eigen::Index i = 0; i < resF.size(); ++i) + BOOST_CHECK(resF.at(i).isApprox(F.at(i), 1e-1) && resL.at(i).isApprox(L.at(i), 1e-1)); } -BOOST_AUTO_TEST_CASE(RelativePoseF10_2solutions) +BOOST_AUTO_TEST_CASE(RelativePoseF10_2_solutions) { // input data Mat X = Mat(10, 2); @@ -122,14 +111,14 @@ BOOST_AUTO_TEST_CASE(RelativePoseF10_2solutions) Mat21 resL2; resL2 << 1.258548483732838e+01, -2.918230091342369e+03; resL.push_back(resL2); // process - std::vector F; + std::vector F; std::vector L; - F10RelativePose(X, U, &F, &L); + F10RelativePose(X, U, F, L); // test results if(resF.size() != F.size()) BOOST_CHECK(false); - for(int i = 0; i < resF.size(); ++i) - BOOST_CHECK(isEqual(resF.at(i), F.at(i)) && isEqual(resL.at(i), L.at(i))); + for(Eigen::Index i = 0; i < resF.size(); ++i) + BOOST_CHECK(resF.at(i).isApprox(F.at(i), 1e-1) && resL.at(i).isApprox(L.at(i), 1e-1)); }