From e998e2be6273b09c2cbd23f9a3047c84dffcd5ec Mon Sep 17 00:00:00 2001 From: metametamoon Date: Mon, 18 Mar 2024 15:17:40 +0300 Subject: [PATCH] Added nocalib marks --- src/phg/sfm/ematrix.cpp | 158 +++++++++++++++++++------------------- src/phg/sfm/ematrix.h | 4 +- src/phg/sfm/resection.cpp | 17 ++-- src/phg/sfm/resection.h | 2 +- tests/test_sfm.cpp | 24 +++--- 5 files changed, 103 insertions(+), 102 deletions(-) diff --git a/src/phg/sfm/ematrix.cpp b/src/phg/sfm/ematrix.cpp index 3bc052b..6b1a1a6 100644 --- a/src/phg/sfm/ematrix.cpp +++ b/src/phg/sfm/ematrix.cpp @@ -80,95 +80,95 @@ namespace { } // Матрицы камер для фундаментальной матрицы определены с точностью до проективного преобразования -// То есть, можно исказить трехмерный мир (применив 4-мерную однородную матрицу), и одновременно поменять матрицы P0, P1 так, что проекции в пикселях не изменятся -// Если мы знаем калибровки камер (матрицы K0, K1 в структуре матриц P0, P1), то можем наложить дополнительные ограничения, в частности, известно, что +// То есть, можно исказить трехмерный мир (применив 4-мерную однородную матрицу), и одновременно поменять матрицы P0_nocalib, P1_nocalib так, что проекции в пикселях не изменятся +// Если мы знаем калибровки камер (матрицы K0, K1 в структуре матриц P0_nocalib, P1_nocalib), то можем наложить дополнительные ограничения, в частности, известно, что // существенная матрица (Essential matrix = K1t * F * K0) имеет ровно два совпадающих ненулевых сингулярных значения, тогда как для фундаментальной матрицы они могут различаться // Это дополнительное ограничение позволяет разложить существенную матрицу с точностью до 4 решений, вместо произвольного проективного преобразования (см. Hartley & Zisserman p.258) // Обычно мы можем использовать одну общую калибровку, более менее верную для большого количества реальных камер и с ее помощью выполнить // первичное разложение существенной матрицы (а из него, взаимное расположение камер) для последующего уточнения методом нелинейной оптимизации -void phg::decomposeEMatrix(cv::Matx34d &P0, cv::Matx34d &P1, const cv::Matx33d &Ecv, const std::vector &m0, const std::vector &m1, const Calibration &calib0, const Calibration &calib1) +void phg::decomposeEMatrix(cv::Matx34d &P0_nocalib, cv::Matx34d &P1_nocalib, const cv::Matx33d &Ecv, const std::vector &m0, const std::vector &m1, const Calibration &calib0, const Calibration &calib1) { throw std::runtime_error("not implemented yet"); -// if (m0.size() != m1.size()) { -// throw std::runtime_error("decomposeEMatrix : m0.size() != m1.size()"); -// } -// -// using mat = Eigen::MatrixXd; -// using vec = Eigen::VectorXd; -// -// mat E; -// copy(Ecv, E); -// -// // (см. Hartley & Zisserman p.258) -// -// Eigen::JacobiSVD svd(E, Eigen::ComputeFullU | Eigen::ComputeFullV); -// -// mat U = svd.matrixU(); -// vec s = svd.singularValues(); -// mat V = svd.matrixV(); -// -// // U, V must be rotation matrices, not just orthogonal -// if (U.determinant() < 0) U = -U; -// if (V.determinant() < 0) V = -V; -// -// std::cout << "U:\n" << U << std::endl; -// std::cout << "s:\n" << s << std::endl; -// std::cout << "V:\n" << V << std::endl; -// -// -// mat R0 = TODO; -// mat R1 = TODO; -// -// std::cout << "R0:\n" << R0 << std::endl; -// std::cout << "R1:\n" << R1 << std::endl; -// -// vec t0 = TODO; -// vec t1 = TODO; -// -// std::cout << "t0:\n" << t0 << std::endl; -// -// P0 = matrix34d::eye(); -// -// // 4 possible solutions -// matrix34d P10 = composeP(R0, t0); -// matrix34d P11 = composeP(R0, t1); -// matrix34d P12 = composeP(R1, t0); -// matrix34d P13 = composeP(R1, t1); -// matrix34d P1s[4] = {P10, P11, P12, P13}; -// -// // need to select best of 4 solutions: 3d points should be in front of cameras (positive depths) -// int best_count = 0; -// int best_idx = -1; -// for (int i = 0; i < 4; ++i) { -// int count = 0; -// for (int j = 0; j < (int) m0.size(); ++j) { -// if (depthTest(m0[j], m1[j], calib0, calib1, P0, P1s[i])) { -// ++count; -// } -// } -// std::cout << "decomposeEMatrix: count: " << count << std::endl; -// if (count > best_count) { -// best_count = count; -// best_idx = i; -// } -// } -// -// if (best_count == 0) { -// throw std::runtime_error("decomposeEMatrix : can't decompose ematrix"); -// } -// -// P1 = P1s[best_idx]; -// -// std::cout << "best idx: " << best_idx << std::endl; -// std::cout << "P0: \n" << P0 << std::endl; -// std::cout << "P1: \n" << P1 << std::endl; + if (m0.size() != m1.size()) { + throw std::runtime_error("decomposeEMatrix : m0.size() != m1.size()"); + } + + using mat = Eigen::MatrixXd; + using vec = Eigen::VectorXd; + + mat E; + copy(Ecv, E); + + // (см. Hartley & Zisserman p.258) + + Eigen::JacobiSVD svd(E, Eigen::ComputeFullU | Eigen::ComputeFullV); + + mat U = svd.matrixU(); + vec s = svd.singularValues(); + mat V = svd.matrixV(); + + // U, V must be rotation matrices, not just orthogonal + if (U.determinant() < 0) U = -U; + if (V.determinant() < 0) V = -V; + + std::cout << "U:\n" << U << std::endl; + std::cout << "s:\n" << s << std::endl; + std::cout << "V:\n" << V << std::endl; + + + mat R0 = TODO; + mat R1 = TODO; + + std::cout << "R0:\n" << R0 << std::endl; + std::cout << "R1:\n" << R1 << std::endl; + + vec t0 = TODO; + vec t1 = TODO; + + std::cout << "t0:\n" << t0 << std::endl; + + P0_nocalib = matrix34d::eye(); + + // 4 possible solutions + matrix34d P10 = composeP(R0, t0); + matrix34d P11 = composeP(R0, t1); + matrix34d P12 = composeP(R1, t0); + matrix34d P13 = composeP(R1, t1); + matrix34d P1s[4] = {P10, P11, P12, P13}; + + // need to select best of 4 solutions: 3d points should be in front of cameras (positive depths) + int best_count = 0; + int best_idx = -1; + for (int i = 0; i < 4; ++i) { + int count = 0; + for (int j = 0; j < (int) m0.size(); ++j) { + if (depthTest(m0[j], m1[j], calib0, calib1, P0_nocalib, P1s[i])) { + ++count; + } + } + std::cout << "decomposeEMatrix: count: " << count << std::endl; + if (count > best_count) { + best_count = count; + best_idx = i; + } + } + + if (best_count == 0) { + throw std::runtime_error("decomposeEMatrix : can't decompose ematrix"); + } + + P1_nocalib = P1s[best_idx]; + + std::cout << "best idx: " << best_idx << std::endl; + std::cout << "P0_nocalib: \n" << P0_nocalib << std::endl; + std::cout << "P1_nocalib: \n" << P1_nocalib << std::endl; } -void phg::decomposeUndistortedPMatrix(cv::Matx33d &R, cv::Vec3d &O, const cv::Matx34d &P) +void phg::decomposeUndistortedPMatrix(cv::Matx33d &R, cv::Vec3d &O, const cv::Matx34d &P_nocalib) { - R = P.get_minor<3, 3>(0, 0); + R = P_nocalib.get_minor<3, 3>(0, 0); - cv::Matx31d O_mat = -R.t() * P.get_minor<3, 1>(0, 3); + cv::Matx31d O_mat = -R.t() * P_nocalib.get_minor<3, 1>(0, 3); O(0) = O_mat(0); O(1) = O_mat(1); O(2) = O_mat(2); diff --git a/src/phg/sfm/ematrix.h b/src/phg/sfm/ematrix.h index 67f0abc..25dbd96 100644 --- a/src/phg/sfm/ematrix.h +++ b/src/phg/sfm/ematrix.h @@ -7,9 +7,9 @@ namespace phg { cv::Matx33d fmatrix2ematrix(const cv::Matx33d &F, const Calibration &calib0, const Calibration &calib1); - void decomposeEMatrix(cv::Matx34d &P0, cv::Matx34d &P1, const cv::Matx33d &E, const std::vector &m0, const std::vector &m1, const Calibration &calib0, const Calibration &calib1); + void decomposeEMatrix(cv::Matx34d &P0_nocalib, cv::Matx34d &P1_nocalib, const cv::Matx33d &E, const std::vector &m0, const std::vector &m1, const Calibration &calib0, const Calibration &calib1); - void decomposeUndistortedPMatrix(cv::Matx33d &R, cv::Vec3d &O, const cv::Matx34d &P); + void decomposeUndistortedPMatrix(cv::Matx33d &R, cv::Vec3d &O, const cv::Matx34d &P_nocalib); cv::Matx34d composeCameraMatrixRO(const cv::Matx33d &R, const cv::Vec3d &O); cv::Matx33d composeEMatrixRT(const cv::Matx33d &R, const cv::Vec3d &T); diff --git a/src/phg/sfm/resection.cpp b/src/phg/sfm/resection.cpp index b52fd3f..f6e4469 100644 --- a/src/phg/sfm/resection.cpp +++ b/src/phg/sfm/resection.cpp @@ -78,12 +78,13 @@ namespace { } - // По трехмерным точкам и их проекциям на изображении определяем положение камеры - cv::Matx34d estimateCameraMatrixRANSAC(const phg::Calibration &calib, const std::vector &X, const std::vector &x) + // По трехмерным точкам и их проекциям на изображении определяем положение камеры без калибровки + // (то есть функция возвращает матрицу (R|t); см. также функцию canonicalizeP + cv::Matx34d estimateCameraMatrixNocalibRANSAC(const phg::Calibration &calib, const std::vector &X, const std::vector &x) { throw std::runtime_error("not implemented yet"); // if (X.size() != x.size()) { -// throw std::runtime_error("estimateCameraMatrixRANSAC: X.size() != x.size()"); +// throw std::runtime_error("estimateCameraMatrixNocalibRANSAC: X.size() != x.size()"); // } // // const int n_points = X.size(); @@ -125,7 +126,7 @@ namespace { // best_support = support; // best_P = P; // -// std::cout << "estimateCameraMatrixRANSAC : support: " << best_support << "/" << n_points << std::endl; +// std::cout << "estimateCameraMatrixNocalibRANSAC : support: " << best_support << "/" << n_points << std::endl; // // if (best_support == n_points) { // break; @@ -133,10 +134,10 @@ namespace { // } // } // -// std::cout << "estimateCameraMatrixRANSAC : best support: " << best_support << "/" << n_points << std::endl; +// std::cout << "estimateCameraMatrixNocalibRANSAC : best support: " << best_support << "/" << n_points << std::endl; // // if (best_support == 0) { -// throw std::runtime_error("estimateCameraMatrixRANSAC : failed to estimate camera matrix"); +// throw std::runtime_error("estimateCameraMatrixNocalibRANSAC : failed to estimate camera matrix"); // } // // return best_P; @@ -145,6 +146,6 @@ namespace { } -cv::Matx34d phg::findCameraMatrix(const Calibration &calib, const std::vector &X, const std::vector &x) { - return estimateCameraMatrixRANSAC(calib, X, x); +cv::Matx34d phg::findCameraMatrixNocalib(const Calibration &calib, const std::vector &X, const std::vector &x) { + return estimateCameraMatrixNocalibRANSAC(calib, X, x); } diff --git a/src/phg/sfm/resection.h b/src/phg/sfm/resection.h index d956f1e..72fe5ef 100644 --- a/src/phg/sfm/resection.h +++ b/src/phg/sfm/resection.h @@ -5,6 +5,6 @@ namespace phg { - cv::Matx34d findCameraMatrix(const Calibration &calib, const std::vector &X, const std::vector &x); + cv::Matx34d findCameraMatrixNocalib(const Calibration &calib, const std::vector &X, const std::vector &x); } diff --git a/tests/test_sfm.cpp b/tests/test_sfm.cpp index 8e70dcc..f449a99 100644 --- a/tests/test_sfm.cpp +++ b/tests/test_sfm.cpp @@ -19,7 +19,7 @@ #include -#define ENABLE_MY_SFM 0 +#define ENABLE_MY_SFM 1 namespace { @@ -550,13 +550,13 @@ TEST (SFM, Resection) { matrix3d F = phg::findFMatrix(points1, points2); matrix3d E = phg::fmatrix2ematrix(F, calib0, calib1); - matrix34d P0, P1; - phg::decomposeEMatrix(P0, P1, E, points1, points2, calib0, calib1); + matrix34d P0_nocalib, P1_nocalib; + phg::decomposeEMatrix(P0_nocalib, P1_nocalib, E, points1, points2, calib0, calib1); matrix3d R0, R1; vector3d O0, O1; - phg::decomposeUndistortedPMatrix(R0, O0, P0); - phg::decomposeUndistortedPMatrix(R1, O1, P1); + phg::decomposeUndistortedPMatrix(R0, O0, P0_nocalib); + phg::decomposeUndistortedPMatrix(R1, O1, P1_nocalib); std::cout << "Camera positions: " << std::endl; std::cout << "R0:\n" << R0 << std::endl; @@ -568,7 +568,7 @@ TEST (SFM, Resection) { std::vector x0s; std::vector x1s; - matrix34d Ps[2] = {P0, P1}; + matrix34d Ps[2] = {P0_nocalib, P1_nocalib}; for (int i = 0; i < (int) good_matches_gms.size(); ++i) { vector3d ms[2] = {calib0.unproject(points1[i]), calib1.unproject(points2[i])}; vector4d X = phg::triangulatePoint(Ps, ms, 2); @@ -583,12 +583,12 @@ TEST (SFM, Resection) { x1s.push_back(points2[i]); } - matrix34d P0res = phg::findCameraMatrix(calib0, Xs, x0s); - matrix34d P1res = phg::findCameraMatrix(calib1, Xs, x1s); + matrix34d P0res = phg::findCameraMatrixNocalib(calib0, Xs, x0s); + matrix34d P1res = phg::findCameraMatrixNocalib(calib1, Xs, x1s); - double rms0 = matRMS(P0res, P0); - double rms1 = matRMS(P1res, P1); - double rms2 = matRMS(P0, P1); + double rms0 = matRMS(P0res, P0_nocalib); + double rms1 = matRMS(P1res, P1_nocalib); + double rms2 = matRMS(P0_nocalib, P1_nocalib); EXPECT_LT(rms0, 0.005); EXPECT_LT(rms1, 0.005); @@ -742,7 +742,7 @@ TEST (SFM, ReconstructNViews) { } } - matrix34d P = phg::findCameraMatrix(calib0, Xs, xs); + matrix34d P = phg::findCameraMatrixNocalib(calib0, Xs, xs); cameras[i_camera] = P; aligned[i_camera] = true;