diff --git a/libs/3rdparty/libgtest/googletest/src/gtest-death-test.cc b/libs/3rdparty/libgtest/googletest/src/gtest-death-test.cc index 43cbd78a..e366f2a2 100755 --- a/libs/3rdparty/libgtest/googletest/src/gtest-death-test.cc +++ b/libs/3rdparty/libgtest/googletest/src/gtest-death-test.cc @@ -46,6 +46,7 @@ # include # include # include +# include # if GTEST_OS_LINUX # include diff --git a/src/phg/sfm/ematrix.cpp b/src/phg/sfm/ematrix.cpp index 3bc052b0..22196733 100644 --- a/src/phg/sfm/ematrix.cpp +++ b/src/phg/sfm/ematrix.cpp @@ -18,26 +18,15 @@ namespace { copy(Ecv, E); Eigen::JacobiSVD svd(E, Eigen::ComputeFullU | Eigen::ComputeFullV); - throw std::runtime_error("not implemented yet"); -// TODO + Eigen::VectorXd s = svd.singularValues(); + double avg = (s[0] + s[1]) / 2.0; + Eigen::VectorXd s_new(3); + s_new << avg, avg, 0; + E = svd.matrixU() * s_new.asDiagonal() * svd.matrixV().transpose(); copy(E, Ecv); } -} - -cv::Matx33d phg::fmatrix2ematrix(const cv::Matx33d &F, const phg::Calibration &calib0, const phg::Calibration &calib1) -{ - throw std::runtime_error("not implemented yet"); -// matrix3d E = TODO; -// -// ensureSpectralProperty(E); -// -// return E; -} - -namespace { - matrix34d composeP(const Eigen::MatrixXd &R, const Eigen::VectorXd &t) { matrix34d result; @@ -61,107 +50,108 @@ namespace { bool depthTest(const vector2d &m0, const vector2d &m1, const phg::Calibration &calib0, const phg::Calibration &calib1, const matrix34d &P0, const matrix34d &P1) { - throw std::runtime_error("not implemented yet"); -// // скомпенсировать калибровки камер -// vector3d p0 = TODO; -// vector3d p1 = TODO; -// -// vector3d ps[2] = {p0, p1}; -// matrix34d Ps[2] = {P0, P1}; -// -// vector4d X = phg::triangulatePoint(Ps, ps, 2); -// if (X[3] != 0) { -// X /= X[3]; -// } -// -// // точка должна иметь положительную глубину для обеих камер -// return TODO && TODO; + vector3d p0 = calib0.unproject(m0); + vector3d p1 = calib1.unproject(m1); + + vector3d ps[2] = {p0, p1}; + matrix34d Ps[2] = {P0, P1}; + + vector4d X = phg::triangulatePoint(Ps, ps, 2); + if (X[3] != 0) { + X /= X[3]; + } + + vector3d x0_proj = P0 * X; + vector3d x1_proj = P1 * X; + + return x0_proj[2] > 0 && x1_proj[2] > 0; } + +} + +cv::Matx33d phg::fmatrix2ematrix(const cv::Matx33d &F, const phg::Calibration &calib0, const phg::Calibration &calib1) +{ + matrix3d E = calib1.K().t() * F * calib0.K(); + ensureSpectralProperty(E); + return E; } -// Матрицы камер для фундаментальной матрицы определены с точностью до проективного преобразования -// То есть, можно исказить трехмерный мир (применив 4-мерную однородную матрицу), и одновременно поменять матрицы P0, P1 так, что проекции в пикселях не изменятся -// Если мы знаем калибровки камер (матрицы K0, K1 в структуре матриц P0, P1), то можем наложить дополнительные ограничения, в частности, известно, что -// существенная матрица (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) { - 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); + + Eigen::JacobiSVD svd(E, Eigen::ComputeFullU | Eigen::ComputeFullV); + + mat U = svd.matrixU(); + vec s = svd.singularValues(); + mat V = svd.matrixV(); + + 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 W(3, 3); + W << 0, -1, 0, + 1, 0, 0, + 0, 0, 1; + + mat R0 = U * W * V.transpose(); + mat R1 = U * W.transpose() * V.transpose(); + + std::cout << "R0:\n" << R0 << std::endl; + std::cout << "R1:\n" << R1 << std::endl; + + vec t0 = U.col(2); + vec t1 = -U.col(2); + + std::cout << "t0:\n" << t0 << std::endl; + + P0 = matrix34d::eye(); + + 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}; + + 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; } void phg::decomposeUndistortedPMatrix(cv::Matx33d &R, cv::Vec3d &O, const cv::Matx34d &P) @@ -174,7 +164,7 @@ void phg::decomposeUndistortedPMatrix(cv::Matx33d &R, cv::Vec3d &O, const cv::Ma O(2) = O_mat(2); if (cv::determinant(R) < 0) { - R *= -1; + R *= -1; } } diff --git a/src/phg/sfm/fmatrix.cpp b/src/phg/sfm/fmatrix.cpp index 50127189..193af1b0 100644 --- a/src/phg/sfm/fmatrix.cpp +++ b/src/phg/sfm/fmatrix.cpp @@ -3,6 +3,7 @@ #include "defines.h" #include +#include #include #include @@ -25,49 +26,55 @@ namespace { // (см. Hartley & Zisserman p.279) cv::Matx33d estimateFMatrixDLT(const cv::Vec2d *m0, const cv::Vec2d *m1, int count) { - throw std::runtime_error("not implemented yet"); -// int a_rows = TODO; -// int a_cols = TODO; -// -// Eigen::MatrixXd A(a_rows, a_cols); -// -// for (int i_pair = 0; i_pair < count; ++i_pair) { -// -// double x0 = m0[i_pair][0]; -// double y0 = m0[i_pair][1]; -// -// double x1 = m1[i_pair][0]; -// double y1 = m1[i_pair][1]; -// -//// std::cout << "(" << x0 << ", " << y0 << "), (" << x1 << ", " << y1 << ")" << std::endl; -// -// TODO -// } -// -// Eigen::JacobiSVD svda(A, Eigen::ComputeFullU | Eigen::ComputeFullV); -// Eigen::VectorXd null_space = TODO -// -// Eigen::MatrixXd F(3, 3); -// F.row(0) << null_space[0], null_space[1], null_space[2]; -// F.row(1) << null_space[3], null_space[4], null_space[5]; -// F.row(2) << null_space[6], null_space[7], null_space[8]; -// -//// Поправить F так, чтобы соблюдалось свойство фундаментальной матрицы (последнее сингулярное значение = 0) -// Eigen::JacobiSVD svdf(F, Eigen::ComputeFullU | Eigen::ComputeFullV); -// -// TODO -// -// cv::Matx33d Fcv; -// copy(F, Fcv); -// -// return Fcv; + Eigen::MatrixXd A(count, 9); + + for (int i = 0; i < count; ++i) { + double x0 = m0[i][0], y0 = m0[i][1]; + double x1 = m1[i][0], y1 = m1[i][1]; + A.row(i) << x1 * x0, x1 * y0, x1, y1 * x0, y1 * y0, y1, x0, y0, 1; + } + + Eigen::JacobiSVD svda(A, Eigen::ComputeFullU | Eigen::ComputeFullV); + Eigen::VectorXd f = svda.matrixV().col(8); + + Eigen::MatrixXd F(3, 3); + F.row(0) << f[0], f[1], f[2]; + F.row(1) << f[3], f[4], f[5]; + F.row(2) << f[6], f[7], f[8]; + + Eigen::JacobiSVD svdf(F, Eigen::ComputeFullU | Eigen::ComputeFullV); + Eigen::VectorXd s = svdf.singularValues(); + s[2] = 0; + F = svdf.matrixU() * s.asDiagonal() * svdf.matrixV().transpose(); + + cv::Matx33d Fcv; + copy(F, Fcv); + return Fcv; } - // Нужно создать матрицу преобразования, которая сдвинет переданное множество точек так, что центр масс перейдет в ноль, а Root Mean Square расстояние до него станет sqrt(2) // (см. Hartley & Zisserman p.107 Why is normalization essential?) cv::Matx33d getNormalizeTransform(const std::vector &m) { - throw std::runtime_error("not implemented yet"); + double cx = 0, cy = 0; + for (const auto &p : m) { + cx += p[0]; + cy += p[1]; + } + cx /= m.size(); + cy /= m.size(); + + double rms = 0; + for (const auto &p : m) { + double dx = p[0] - cx, dy = p[1] - cy; + rms += dx * dx + dy * dy; + } + rms = std::sqrt(rms / m.size()); + + double scale = std::sqrt(2.0) / rms; + + return cv::Matx33d(scale, 0, -scale * cx, + 0, scale, -scale * cy, + 0, 0, 1); } cv::Vec2d transformPoint(const cv::Vec2d &pt, const cv::Matx33d &T) @@ -100,70 +107,66 @@ namespace { } { -// Проверьте лог: при повторной нормализации должно найтись почти единичное преобразование - getNormalizeTransform(m0_t); - getNormalizeTransform(m1_t); + cv::Matx33d TN0_check = getNormalizeTransform(m0_t); + cv::Matx33d TN1_check = getNormalizeTransform(m1_t); + (void)TN0_check; + (void)TN1_check; + } + + const int n_samples = 8; + const int n_trials = 100000; + uint64_t seed = 1; + + int best_support = 0; + cv::Matx33d best_F; + + std::vector sample; + for (int i_trial = 0; i_trial < n_trials; ++i_trial) { + phg::randomSample(sample, n_matches, n_samples, &seed); + + cv::Vec2d ms0[n_samples]; + cv::Vec2d ms1[n_samples]; + for (int i = 0; i < n_samples; ++i) { + ms0[i] = m0_t[sample[i]]; + ms1[i] = m1_t[sample[i]]; + } + + cv::Matx33d Fn = estimateFMatrixDLT(ms0, ms1, n_samples); + + cv::Matx33d F = TN1.t() * Fn * TN0; + + int support = 0; + for (int i = 0; i < n_matches; ++i) { + if (phg::epipolarTest(m0[i], m1[i], F, threshold_px) && phg::epipolarTest(m1[i], m0[i], F.t(), threshold_px)) { + ++support; + } + } + + if (support > best_support) { + best_support = support; + best_F = F; + + std::cout << "estimateFMatrixRANSAC : support: " << best_support << "/" << n_matches << std::endl; + infoF(F); + + if (best_support == n_matches) { + break; + } + } + } + + std::cout << "estimateFMatrixRANSAC : best support: " << best_support << "/" << n_matches << std::endl; + + if (best_support == 0) { + throw std::runtime_error("estimateFMatrixRANSAC : failed to estimate fundamental matrix"); } - throw std::runtime_error("not implemented yet"); -// // https://en.wikipedia.org/wiki/Random_sample_consensus#Parameters -// // будет отличаться от случая с гомографией -// const int n_trials = TODO; -// -// const int n_samples = TODO; -// uint64_t seed = 1; -// -// int best_support = 0; -// cv::Matx33d best_F; -// -// std::vector sample; -// for (int i_trial = 0; i_trial < n_trials; ++i_trial) { -// phg::randomSample(sample, n_matches, n_samples, &seed); -// -// cv::Vec2d ms0[n_samples]; -// cv::Vec2d ms1[n_samples]; -// for (int i = 0; i < n_samples; ++i) { -// ms0[i] = m0_t[sample[i]]; -// ms1[i] = m1_t[sample[i]]; -// } -// -// cv::Matx33d F = estimateFMatrixDLT(ms0, ms1, n_samples); -// -// // denormalize -// F = TODO -// -// int support = 0; -// for (int i = 0; i < n_matches; ++i) { -// if (phg::epipolarTest(m0[i], m1[i], todo, threshold_px) && phg::epipolarTest(m1[i], m0[i], todo, threshold_px)) -// { -// ++support; -// } -// } -// -// if (support > best_support) { -// best_support = support; -// best_F = F; -// -// std::cout << "estimateFMatrixRANSAC : support: " << best_support << "/" << n_matches << std::endl; -// infoF(F); -// -// if (best_support == n_matches) { -// break; -// } -// } -// } -// -// std::cout << "estimateFMatrixRANSAC : best support: " << best_support << "/" << n_matches << std::endl; -// -// if (best_support == 0) { -// throw std::runtime_error("estimateFMatrixRANSAC : failed to estimate fundamental matrix"); -// } -// -// return best_F; + + return best_F; } } -cv::Matx33d phg::findFMatrix(const std::vector &m0, const std::vector &m1, double threshold_px) { +cv::Matx33d phg::findFMatrix(const std::vector &m0, const std::vector &m1, double threshold_px) { return estimateFMatrixRANSAC(m0, m1, threshold_px); } @@ -175,7 +178,7 @@ cv::Matx33d phg::composeFMatrix(const cv::Matx34d &P0, const cv::Matx34d &P1) { // compute fundamental matrix from general cameras // Hartley & Zisserman (17.3 - p412) - + cv::Matx33d F; #define det4(a, b, c, d) \ @@ -199,6 +202,6 @@ cv::Matx33d phg::composeFMatrix(const cv::Matx34d &P0, const cv::Matx34d &P1) } #undef det4 - + return F; } diff --git a/src/phg/sfm/resection.cpp b/src/phg/sfm/resection.cpp index d2cf6433..2c3e3c9b 100644 --- a/src/phg/sfm/resection.cpp +++ b/src/phg/sfm/resection.cpp @@ -2,6 +2,7 @@ #include #include +#include #include "sfm_utils.h" #include "defines.h" @@ -46,103 +47,119 @@ namespace { return result; } - // (см. Hartley & Zisserman p.178) cv::Matx34d estimateCameraMatrixDLT(const cv::Vec3d *Xs, const cv::Vec3d *xs, int count) { - throw std::runtime_error("not implemented yet"); -// using mat = Eigen::MatrixXd; -// using vec = Eigen::VectorXd; -// -// mat A(TODO); -// -// for (int i = 0; i < count; ++i) { -// -// double x = xs[i][0]; -// double y = xs[i][1]; -// double w = xs[i][2]; -// -// double X = Xs[i][0]; -// double Y = Xs[i][1]; -// double Z = Xs[i][2]; -// double W = 1.0; -// -// TODO -// } -// -// matrix34d result; -// TODO -// -// return canonicalizeP(result); - } + using mat = Eigen::MatrixXd; + using vec = Eigen::VectorXd; + + mat A(2 * count, 12); + A.setZero(); + + for (int i = 0; i < count; ++i) { + double x = xs[i][0], y = xs[i][1], w = xs[i][2]; + double X = Xs[i][0], Y = Xs[i][1], Z = Xs[i][2]; + double W = 1.0; + + A(2 * i, 4) = -w * X; A(2 * i, 5) = -w * Y; A(2 * i, 6) = -w * Z; A(2 * i, 7) = -w * W; + A(2 * i, 8) = y * X; A(2 * i, 9) = y * Y; A(2 * i, 10) = y * Z; A(2 * i, 11) = y * W; + A(2 * i + 1, 0) = w * X; A(2 * i + 1, 1) = w * Y; A(2 * i + 1, 2) = w * Z; A(2 * i + 1, 3) = w * W; + A(2 * i + 1, 8) = -x * X; A(2 * i + 1, 9) = -x * Y; A(2 * i + 1, 10) = -x * Z; A(2 * i + 1, 11) = -x * W; + } + + Eigen::JacobiSVD svd(A, Eigen::ComputeFullV); + vec p = svd.matrixV().col(11); + + matrix34d result; + for (int i = 0; i < 12; ++i) { + result(i / 4, i % 4) = p(i); + } + return canonicalizeP(result); + } // По трехмерным точкам и их проекциям на изображении определяем положение камеры cv::Matx34d estimateCameraMatrixRANSAC(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()"); -// } -// -// const int n_points = X.size(); -// -// // https://en.wikipedia.org/wiki/Random_sample_consensus#Parameters -// // будет отличаться от случая с гомографией -// const int n_trials = TODO; -// -// const double threshold_px = 3; -// -// const int n_samples = TODO; -// uint64_t seed = 1; -// -// int best_support = 0; -// cv::Matx34d best_P; -// -// std::vector sample; -// for (int i_trial = 0; i_trial < n_trials; ++i_trial) { -// phg::randomSample(sample, n_points, n_samples, &seed); -// -// cv::Vec3d ms0[n_samples]; -// cv::Vec3d ms1[n_samples]; -// for (int i = 0; i < n_samples; ++i) { -// ms0[i] = TODO; -// ms1[i] = TODO; -// } -// -// cv::Matx34d P = estimateCameraMatrixDLT(ms0, ms1, n_samples); -// -// int support = 0; -// for (int i = 0; i < n_points; ++i) { -// cv::Vec2d px = TODO спроецировать 3Д точку в пиксель с использованием P и calib; -// if (cv::norm(px - x[i]) < threshold_px) { -// ++support; -// } -// } -// -// if (support > best_support) { -// best_support = support; -// best_P = P; -// -// std::cout << "estimateCameraMatrixRANSAC : support: " << best_support << "/" << n_points << std::endl; -// -// if (best_support == n_points) { -// break; -// } -// } -// } -// -// std::cout << "estimateCameraMatrixRANSAC : best support: " << best_support << "/" << n_points << std::endl; -// -// if (best_support == 0) { -// throw std::runtime_error("estimateCameraMatrixRANSAC : failed to estimate camera matrix"); -// } -// -// return best_P; - } + if (X.size() != x.size()) { + throw std::runtime_error("estimateCameraMatrixRANSAC: X.size() != x.size()"); + } + + const int n_points = X.size(); + const int n_trials = 10000; + const double threshold_px = 5; + const int n_samples = 6; + uint64_t seed = 1; + + auto countSupport = [&](const cv::Matx34d &P) -> int { + int s = 0; + for (int i = 0; i < n_points; ++i) { + cv::Vec3d px_h = calib.project(P * cv::Vec4d(X[i][0], X[i][1], X[i][2], 1)); + if (px_h[2] == 0) continue; + cv::Vec2d px(px_h[0] / px_h[2], px_h[1] / px_h[2]); + if (cv::norm(px - x[i]) < threshold_px) ++s; + } + return s; + }; + int best_support = 0; + cv::Matx34d best_P; + + std::vector sample; + for (int i_trial = 0; i_trial < n_trials; ++i_trial) { + phg::randomSample(sample, n_points, n_samples, &seed); + + cv::Vec3d ms0[n_samples]; + cv::Vec3d ms1[n_samples]; + for (int i = 0; i < n_samples; ++i) { + ms0[i] = X[sample[i]]; + ms1[i] = calib.unproject(x[sample[i]]); + } + + cv::Matx34d P = estimateCameraMatrixDLT(ms0, ms1, n_samples); + int support = countSupport(P); + + if (support > best_support) { + best_support = support; + best_P = P; + std::cout << "estimateCameraMatrixRANSAC : support: " << best_support << "/" << n_points << std::endl; + if (best_support == n_points) break; + } + } + + if (best_support == 0) { + throw std::runtime_error("estimateCameraMatrixRANSAC : failed to estimate camera matrix"); + } + + const double wide_threshold = 2.0 * threshold_px; + for (int refine_iter = 0; refine_iter < 10; ++refine_iter) { + std::vector Xs_in; + std::vector xs_in; + for (int i = 0; i < n_points; ++i) { + cv::Vec3d px_h = calib.project(best_P * cv::Vec4d(X[i][0], X[i][1], X[i][2], 1)); + if (px_h[2] == 0) continue; + cv::Vec2d px(px_h[0] / px_h[2], px_h[1] / px_h[2]); + if (cv::norm(px - x[i]) < wide_threshold) { + Xs_in.push_back(X[i]); + xs_in.push_back(calib.unproject(x[i])); + } + } + if ((int)Xs_in.size() <= n_samples) break; + + cv::Matx34d P_refit = estimateCameraMatrixDLT(Xs_in.data(), xs_in.data(), (int)Xs_in.size()); + int new_support = countSupport(P_refit); + if (new_support <= best_support) break; + + best_support = new_support; + best_P = P_refit; + std::cout << "estimateCameraMatrixRANSAC refine: support: " << best_support << "/" << n_points << std::endl; + } + + std::cout << "estimateCameraMatrixRANSAC : best support: " << best_support << "/" << n_points << std::endl; + return best_P; + } } -cv::Matx34d phg::findCameraMatrix(const Calibration &calib, const std::vector &X, const std::vector &x) { +cv::Matx34d phg::findCameraMatrix(const Calibration &calib, const std::vector &X, const std::vector &x) { return estimateCameraMatrixRANSAC(calib, X, x); } diff --git a/src/phg/sfm/sfm_utils.cpp b/src/phg/sfm/sfm_utils.cpp index d2d2e294..cc071499 100644 --- a/src/phg/sfm/sfm_utils.cpp +++ b/src/phg/sfm/sfm_utils.cpp @@ -41,5 +41,10 @@ void phg::randomSample(std::vector &dst, int max_id, int sample_size, uint6 // проверяет, что расстояние от точки до линии меньше порога bool phg::epipolarTest(const cv::Vec2d &pt0, const cv::Vec2d &pt1, const cv::Matx33d &F, double t) { - throw std::runtime_error("not implemented yet"); + cv::Vec3d l = F * cv::Vec3d(pt0[0], pt0[1], 1.0); + double denom = std::sqrt(l[0] * l[0] + l[1] * l[1]); + if (denom < 1e-10) + return false; + double dist = std::abs(pt1[0] * l[0] + pt1[1] * l[1] + l[2]) / denom; + return dist < t; } diff --git a/src/phg/sfm/triangulation.cpp b/src/phg/sfm/triangulation.cpp index 8dd11e69..df8a34c6 100644 --- a/src/phg/sfm/triangulation.cpp +++ b/src/phg/sfm/triangulation.cpp @@ -4,13 +4,20 @@ #include -// По положениям камер и ключевых точкам определяем точку в трехмерном пространстве -// Задача эквивалентна поиску точки пересечения двух (или более) лучей -// Используем DLT метод, составляем систему уравнений. Система похожа на систему для гомографии, там пары уравнений получались из выражений вида x (cross) Hx = 0, а здесь будет x (cross) PX = 0 -// (см. Hartley & Zisserman p.312) cv::Vec4d phg::triangulatePoint(const cv::Matx34d *Ps, const cv::Vec3d *ms, int count) { - // составление однородной системы + SVD - // без подвохов - throw std::runtime_error("not implemented yet"); + Eigen::MatrixXd A(2 * count, 4); + + for (int i = 0; i < count; ++i) { + double mx = ms[i][0], my = ms[i][1], mw = ms[i][2]; + for (int col = 0; col < 4; ++col) { + A(2 * i, col) = mx * Ps[i](2, col) - mw * Ps[i](0, col); + A(2 * i + 1, col) = my * Ps[i](2, col) - mw * Ps[i](1, col); + } + } + + Eigen::JacobiSVD svd(A, Eigen::ComputeFullV); + Eigen::VectorXd X = svd.matrixV().col(3); + + return cv::Vec4d(X[0], X[1], X[2], X[3]); } diff --git a/tests/test_sfm.cpp b/tests/test_sfm.cpp index 4229b86b..48cc1588 100644 --- a/tests/test_sfm.cpp +++ b/tests/test_sfm.cpp @@ -18,7 +18,7 @@ #include "utils/test_utils.h" -#define ENABLE_MY_SFM 0 +#define ENABLE_MY_SFM 1 namespace {