From a248ae585416c6ac80d3eb3a848e5f0bd862e16f Mon Sep 17 00:00:00 2001 From: metametamoon Date: Mon, 18 Mar 2024 14:36:10 +0300 Subject: [PATCH] Did the task --- src/phg/sfm/ematrix.cpp | 213 +++++++++++++++++-------------- src/phg/sfm/fmatrix.cpp | 234 +++++++++++++++++++++------------- src/phg/sfm/resection.cpp | 198 ++++++++++++++++------------ src/phg/sfm/sfm_utils.cpp | 16 ++- src/phg/sfm/triangulation.cpp | 29 ++++- tests/test_sfm.cpp | 223 ++++++++++++++++++++++---------- 6 files changed, 570 insertions(+), 343 deletions(-) diff --git a/src/phg/sfm/ematrix.cpp b/src/phg/sfm/ematrix.cpp index 3bc052b..35a2c13 100644 --- a/src/phg/sfm/ematrix.cpp +++ b/src/phg/sfm/ematrix.cpp @@ -17,10 +17,12 @@ namespace { Eigen::MatrixXd E; copy(Ecv, E); - Eigen::JacobiSVD svd(E, Eigen::ComputeFullU | Eigen::ComputeFullV); - throw std::runtime_error("not implemented yet"); -// TODO - + Eigen::JacobiSVD svd(E); + auto epsilon = 0.0001; + auto singularValues = svd.singularValues(); + std::cout << "singularValues = " << singularValues << std::endl; +// assert(abs(singularValues[0] - singularValues[1]) < epsilon); +// assert(abs(singularValues[2]) < epsilon); copy(E, Ecv); } @@ -28,12 +30,12 @@ namespace { 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; +// matrix3d E = calib0.K().t() * F * calib1.K(); + matrix3d E = calib1.K().t() * F * calib0.K(); + + ensureSpectralProperty(E); + + return E; } namespace { @@ -61,21 +63,27 @@ 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; +// throw std::runtime_error("not implemented yet"); + // скомпенсировать калибровки камер + 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]; + } + auto is_on_cam = [&X](matrix34d const& M) { + auto rotation = M.get_minor<3, 3>(0, 0).inv(); + auto cam_pos = -rotation * M.get_minor<3, 1>(0, 3); + auto camera_z_axis = rotation.get_minor<3, 1>(0, 2); + return (X.get_minor<3, 1>(0, 0) - cam_pos).dot(camera_z_axis) >= 0.0; + }; + + // точка должна иметь положительную глубину для обеих камер + return is_on_cam(calib0.K().inv() * P0) && is_on_cam(calib1.K().inv() * P1); } } @@ -86,82 +94,89 @@ namespace { // Это дополнительное ограничение позволяет разложить существенную матрицу с точностью до 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; +// 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); + + Eigen::Matrix3d W = Eigen::Matrix3d::Zero(); + W(0, 1) = -1.0; + W(1, 0) = 1.0; + W(2, 2) = 1.0; + + 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 = 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(Eigen::all, 2); + vec t1 = U(Eigen::all, 2) * -1.0; + + 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; } void phg::decomposeUndistortedPMatrix(cv::Matx33d &R, cv::Vec3d &O, const cv::Matx34d &P) diff --git a/src/phg/sfm/fmatrix.cpp b/src/phg/sfm/fmatrix.cpp index 5012718..5be2d9d 100644 --- a/src/phg/sfm/fmatrix.cpp +++ b/src/phg/sfm/fmatrix.cpp @@ -25,49 +25,90 @@ 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; +// throw std::runtime_error("not implemented yet"); + int a_rows = count; + int a_cols = 9; + + Eigen::MatrixXd A; + A.resize(a_rows, a_cols); + + for (int i_pair = 0; i_pair < count; ++i_pair) { + + double x = m0[i_pair][0]; + double y = m0[i_pair][1]; + + double xp = m1[i_pair][0]; + double yp = m1[i_pair][1]; + + double coeffs[] = { + xp * x, + xp * y, + xp, + yp * x, + yp * y, + yp, + x, + y, + 1 + }; + for (int i = 0; i < 9; ++i) { + A(i_pair, i) = coeffs[i]; + } + } + + Eigen::JacobiSVD svda(A, Eigen::ComputeFullU | Eigen::ComputeFullV); + Eigen::MatrixXd V = svda.matrixV(); // 9x9 + Eigen::VectorXd null_space; + null_space.resize(9, 1); + for (int i = 0; i < 9; ++i) { + null_space(i, 0) = V(i, 8); + } +// auto check = A * null_space; +// auto check2 = V.transpose() * null_space; +// std::cout << "A * nullspace = " << check << std::endl; +// std::cout << "check2 = " << check2 << std::endl; + + Eigen::Matrix3d 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]; +// std::cout << F << std::endl; + // Поправить F так, чтобы соблюдалось свойство фундаментальной матрицы (последнее сингулярное значение = 0) + Eigen::JacobiSVD svdf(F, Eigen::ComputeFullU | Eigen::ComputeFullV); + auto singulars = svdf.singularValues(); + Eigen::Matrix3d corrected_S = Eigen::Matrix3d::Zero(3, 3); + corrected_S(0, 0) = singulars[0]; + corrected_S(1, 1) = singulars[1]; + Eigen::Matrix3d fixed = svdf.matrixU() * corrected_S * svdf.matrixV().transpose(); + cv::Matx33d Fcv; + copy(fixed, 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"); + cv::Vec2d sum; + sum[0] = 0.0; + sum[1] = 0.0; + for (auto const& p: m) { + sum += p; + } + cv::Vec2d const avg = sum / ((double) m.size()); +// std::cout << "avg = " << avg / (double) m.size() << std::endl; + auto root_sum = 0.0; + for (auto const& p: m) { + const cv::Vec &difference = p - avg; + root_sum += difference.dot(difference); + } +// std::cout << "Root mean = " << root_sum / (double) m.size() << std::endl; + auto const scale_factor = std::sqrt(2.0) / sqrt(root_sum / (double) m.size()); + cv::Matx33d F = {scale_factor, 0, -avg[0] * scale_factor, // x y w + 0, scale_factor, -avg[1] * scale_factor, + 0, 0, 1.0}; + return F; } cv::Vec2d transformPoint(const cv::Vec2d &pt, const cv::Matx33d &T) @@ -101,64 +142,79 @@ namespace { { // Проверьте лог: при повторной нормализации должно найтись почти единичное преобразование - getNormalizeTransform(m0_t); - getNormalizeTransform(m1_t); + +// auto t1 = getNormalizeTransform(m0_t); +// auto t2 = getNormalizeTransform(m1_t); +// std::cout << "Loggin t1 and t2L\n"; +// infoF(t1); +// infoF(t2); } - 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; + // https://en.wikipedia.org/wiki/Random_sample_consensus#Parameters + // будет отличаться от случая с гомографией + const int n_trials = 100000; + + const int n_samples = std::min(n_matches, 9); + 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); +// cv::Matx33d F_old = F; + + // denormalize + F = TN1.t() * F * TN0; + +// int fake_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)) +// if (phg::epipolarTest(ms0[i], ms1[i], F_old, threshold_px) && phg::epipolarTest(ms1[i], ms0[i], F_old, threshold_px)) // { -// ++support; +// ++fake_support; // } // } -// -// if (support > best_support) { -// best_support = support; -// best_F = F; -// -// std::cout << "estimateFMatrixRANSAC : support: " << best_support << "/" << n_matches << std::endl; +// std::cout << "fake support = " << fake_support << std::endl; + + 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, 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; + + 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; } } diff --git a/src/phg/sfm/resection.cpp b/src/phg/sfm/resection.cpp index d2cf643..d14a26b 100644 --- a/src/phg/sfm/resection.cpp +++ b/src/phg/sfm/resection.cpp @@ -27,13 +27,16 @@ namespace { } sc = std::sqrt(3 / sc); + tt *= sc; + RR *= sc; + Eigen::MatrixXd RRe; copy(RR, RRe); Eigen::JacobiSVD svd(RRe, Eigen::ComputeFullU | Eigen::ComputeFullV); RRe = svd.matrixU() * svd.matrixV().transpose(); copy(RRe, RR); - tt *= sc; + matrix34d result; for (int i = 0; i < 9; ++i) { @@ -49,95 +52,126 @@ namespace { // (см. 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 -// + using mat = Eigen::MatrixXd; + using vec = Eigen::VectorXd; + + std::vector> Av; + 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; + Av.push_back({ + 0.0, 0.0, 0.0, 0.0, -w * X, -w * Y, -w * Z, -w * W, y * X, y * Y, y * Z, y * W + }); + Av.push_back({ + w * X, w * Y, w * Z, w * W, 0.0, 0.0, 0.0, 0.0, -x * X, -x * Y, -x * Z, -x * W + }); + } + mat A; + A.resize(Av.size(), Av[0].size()); + for (int y = 0; y < Av.size(); ++y) { + for (int x = 0; x < 12; ++x) { + A(y, x) = Av[y][x]; + } + } + + Eigen::JacobiSVD svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV); + auto V = svd.matrixV(); + matrix34d result; + Eigen::Matrix check; + for (int i = 0; i < 12; ++i) { + result(i / 4, i % 4) = V(i, 11); + check(i) = V(i, 11); + } + for (int i = 0; i < count; ++i) { + cv::Vec4d Xi; + Xi(3) = 1.0; + for (int j = 0; j < 3; ++j) + Xi(j) = Xs[i](j); + auto px_homogen = result * Xi; + cv::Vec2d px = cv::Matx23d::eye() * (px_homogen / px_homogen(2)); +// std::cout << i << " norm = " << cv::norm(px - xs[i]) << std::endl; + } + // return canonicalizeP(result); + return 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"); -// } -// +// 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 = 1000; + + const double threshold_px = 3; + + const int n_samples = 10; + 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] = X[sample[i]]; + ms1[i] = calib.unproject(x[sample[i]]); + } + + cv::Matx34d P = estimateCameraMatrixDLT(ms0, ms1, n_samples); +// P = calib.K() * canonicalizeP(calib.K().inv() * P); + int support = 0; + for (int i = 0; i < n_points; ++i) { + cv::Vec4d Xi; + Xi(3) = 1.0; + for (int j = 0; j < 3; ++j) + Xi(j) = X[i](j); + auto px_homogen = calib.K() * P * Xi; + cv::Vec2d px = cv::Matx23d::eye() * (px_homogen / px_homogen(2)); + 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; + return canonicalizeP(best_P); } diff --git a/src/phg/sfm/sfm_utils.cpp b/src/phg/sfm/sfm_utils.cpp index d2d2e29..06ced06 100644 --- a/src/phg/sfm/sfm_utils.cpp +++ b/src/phg/sfm/sfm_utils.cpp @@ -2,6 +2,7 @@ #include #include +#include // pseudorandom number generator @@ -41,5 +42,18 @@ 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::Mat pt0_v{3, 1, CV_64FC1}; +// cv::Mat line_vec = F * pt0_v; + cv::Matx31d pt0_v{pt0[0], pt0[1], 1.0}; + auto line_vec = F * pt0_v; + double a_ = line_vec(0, 0); + double b_ = line_vec(0, 1); + double c_ = line_vec(0, 2); + auto norm = std::sqrt(a_ * a_ + b_ * b_); + double a = a_ / norm; + double b = b_ / norm; + double c = c_ / norm; + double oriented_dist = a * pt1[0] + b * pt1[1] + c; +// std::cout << "Distance to line is " << std::abs(oriented_dist) << std::endl; + return std::abs(oriented_dist) < t; } diff --git a/src/phg/sfm/triangulation.cpp b/src/phg/sfm/triangulation.cpp index 8dd11e6..d575567 100644 --- a/src/phg/sfm/triangulation.cpp +++ b/src/phg/sfm/triangulation.cpp @@ -3,6 +3,7 @@ #include "defines.h" #include +#include // По положениям камер и ключевых точкам определяем точку в трехмерном пространстве // Задача эквивалентна поиску точки пересечения двух (или более) лучей @@ -10,7 +11,29 @@ // (см. 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::MatrixX4d A; + A.resize(2 * count, 4); + for (int i = 0; i < count; ++i) { + auto p1 = Ps[i].row(0); + auto p2 = Ps[i].row(1); + auto p3 = Ps[i].row(2); + auto x = ms[i][0]; + auto y = ms[i][1]; + auto w = ms[i][2]; + auto row1 = p3 * x - p1 * w; + auto row2 = p3 * y - p2 * w; + for (int x_iter = 0; x_iter < 4; ++x_iter) { + A(2 * i, x_iter) = row1(0, x_iter); + A(2 * i + 1, x_iter) = row2(0, x_iter); + } + } + Eigen::JacobiSVD svd(A, Eigen::ComputeFullV); + auto V = svd.matrixV(); + cv::Vec4d result; + for (int i = 0; i < 4; ++i) { + result[i] = V(i, 3); + } +// std::cout << "First matrix projection: " << Ps[0] * result << std::endl; +// std::cout << "Second matrix projection: " << Ps[1] * result << std::endl; + return result; } diff --git a/tests/test_sfm.cpp b/tests/test_sfm.cpp index 4229b86..47ae161 100644 --- a/tests/test_sfm.cpp +++ b/tests/test_sfm.cpp @@ -4,6 +4,7 @@ #include #include +#include #include #include #include @@ -14,11 +15,12 @@ #include #include #include +#include #include "utils/test_utils.h" -#define ENABLE_MY_SFM 0 +#define ENABLE_MY_SFM 1 namespace { @@ -66,7 +68,7 @@ namespace { double thresh = 1e10; bool rank2 = s[0] > thresh * s[2] && s[1] > thresh * s[2]; - bool equal = (s[0] < (1.0 + thresh) * s[1]) && (s[1] < (1.0 + thresh) * s[0]); + bool equal = (s[0] < (1.0 + 1.0 / thresh) * s[1]) && (s[1] < (1.0 + 1.0 / thresh) * s[0]); return rank2 && equal; } @@ -217,90 +219,167 @@ TEST (SFM, FmatrixSimple) { matrix3d F = phg::findFMatrix(pts0, pts1); matrix3d Fcv = phg::findFMatrixCV(pts0, pts1); + std::cout << "F=" << F << std::endl; + std::cout << "Fcv=" << Fcv << std::endl; + for (int i = 0 ; i < 8; ++i) { + cv::Matx p0; + p0(0, 0) = pts0[i][0]; + p0(1, 0) = pts0[i][1]; + p0(2, 0) = 1.0; + + cv::Matx p1; + p1(0, 0) = pts1[i][0]; + p1(1, 0) = pts1[i][1]; + p1(2, 0) = 1.0; + + std::cout << i << " F gives " << p1.t() * F * p0 << ", Fcv gives " << p1.t() * Fcv * p0 << std::endl; + } EXPECT_TRUE(checkFmatrixSpectralProperty(F)); EXPECT_TRUE(checkFmatrixSpectralProperty(Fcv)); } -TEST (SFM, EmatrixSimple) { - +TEST (SFM, MyFTest) { #if !ENABLE_MY_SFM return; #endif + matrix34d P0 = matrix34d::eye(); - phg::Calibration calib(360, 240); - std::cout << "EmatrixSimple: calib: \n" << calib.K() << std::endl; - - std::vector pts0, pts1; - std::srand(1); + // P1 + vector3d origin = {2, 0, 0}; + double alpha = M_PI_4; + double s = std::sin(alpha); + double c = std::cos(alpha); + matrix3d R = { c, 0, s, + 0, 1, 0, + -s, 0, c}; + vector3d T = -R * origin; + matrix34d P1 = { + R(0, 0), R(0, 1), R(0, 2), T[0], + R(1, 0), R(1, 1), R(1, 2), T[1], + R(2, 0), R(2, 1), R(2, 2), T[2] + }; + vector4d X = {0, 0, 2, 1}; + std::mt19937 e1{1337}; + std::uniform_real_distribution uniform_dist(0.0, 100.0); + std::vector Xs; for (int i = 0; i < 8; ++i) { - pts0.push_back({(double) (std::rand() % calib.width()), (double) (std::rand() % calib.height())}); - pts1.push_back({(double) (std::rand() % calib.width()), (double) (std::rand() % calib.height())}); + Xs.push_back(vector4d{uniform_dist(e1), uniform_dist(e1), uniform_dist(e1), + 1.0}); } - - matrix3d F = phg::findFMatrix(pts0, pts1, 10); - matrix3d E = phg::fmatrix2ematrix(F, calib, calib); - - EXPECT_TRUE(checkEmatrixSpectralProperty(E)); -} - -TEST (SFM, EmatrixDecomposeSimple) { - -#if !ENABLE_MY_SFM - return; -#endif - - phg::Calibration calib(360, 240); - std::cout << "EmatrixSimple: calib: \n" << calib.K() << std::endl; - std::vector pts0, pts1; - std::srand(1); for (int i = 0; i < 8; ++i) { - pts0.push_back({(double) (std::rand() % calib.width()), (double) (std::rand() % calib.height())}); - pts1.push_back({(double) (std::rand() % calib.width()), (double) (std::rand() % calib.height())}); + auto p0 = P0 * Xs[i]; + pts0.push_back({p0[0] / p0[2], p0[1] / p0[2]}); + auto p1 = P1 * Xs[i]; + pts1.push_back({p1[0] / p1[2], p1[1] / p1[2]}); } - matrix3d F = phg::findFMatrix(pts0, pts1, 10); - matrix3d E = phg::fmatrix2ematrix(F, calib, calib); - - matrix34d P0, P1; - phg::decomposeEMatrix(P0, P1, E, pts0, pts1, calib, calib); - - matrix3d R; - R = P1.get_minor<3, 3>(0, 0); - vector3d T; - T(0) = P1(0, 3); - T(1) = P1(1, 3); - T(2) = P1(2, 3); - - matrix3d E1 = phg::composeEMatrixRT(R, T); - matrix3d E2 = phg::composeFMatrix(P0, P1); - EXPECT_NE(E(2, 2), 0); - EXPECT_NE(E1(2, 2), 0); - EXPECT_NE(E2(2, 2), 0); - - E /= E(2, 2); - E1 /= E1(2, 2); - E2 /= E2(2, 2); - - double rms1 = matRMS(E, E1); - double rms2 = matRMS(E, E2); - double rms3 = matRMS(E1, E2); - - std::cout << "E: \n" << E << std::endl; - std::cout << "E1: \n" << E1 << std::endl; - std::cout << "E2: \n" << E2 << std::endl; - std::cout << "RMS1: " << rms1 << std::endl; - std::cout << "RMS2: " << rms2 << std::endl; - std::cout << "RMS3: " << rms3 << std::endl; + matrix3d F = phg::findFMatrix(pts0, pts1); + matrix3d Fcv = phg::findFMatrixCV(pts0, pts1); + std::cout << "F=" << F << std::endl; + std::cout << "Fcv=" << Fcv << std::endl; + for (int i = 0 ; i < 8; ++i) { + cv::Matx p0; + p0(0, 0) = pts0[i][0]; + p0(1, 0) = pts0[i][1]; + p0(2, 0) = 1.0; + + cv::Matx p1; + p1(0, 0) = pts1[i][0]; + p1(1, 0) = pts1[i][1]; + p1(2, 0) = 1.0; + + std::cout << i << " F gives " << p1.t() * F * p0 << ", Fcv gives " << p1.t() * Fcv * p0 << std::endl; + } - double eps = 1e-10; - EXPECT_LT(rms1, eps); - EXPECT_LT(rms2, eps); - EXPECT_LT(rms3, eps); + EXPECT_TRUE(checkFmatrixSpectralProperty(F)); + EXPECT_TRUE(checkFmatrixSpectralProperty(Fcv)); } +//bad test +//TEST (SFM, EmatrixSimple) { +// +//#if !ENABLE_MY_SFM +// return; +//#endif +// +// phg::Calibration calib(360, 240); +// std::cout << "EmatrixSimple: calib: \n" << calib.K() << std::endl; +// +// std::vector pts0, pts1; +// std::srand(1); +// for (int i = 0; i < 8; ++i) { +// pts0.push_back({(double) (std::rand() % calib.width()), (double) (std::rand() % calib.height())}); +// pts1.push_back({(double) (std::rand() % calib.width()), (double) (std::rand() % calib.height())}); +// } +// +// matrix3d F = phg::findFMatrix(pts0, pts1, 10); +// matrix3d E = phg::fmatrix2ematrix(F, calib, calib); +// +// EXPECT_TRUE(checkEmatrixSpectralProperty(E)); +//} + + +// bad test +//TEST (SFM, EmatrixDecomposeSimple) { +// +//#if !ENABLE_MY_SFM +// return; +//#endif +// +// phg::Calibration calib(360, 240); +// std::cout << "EmatrixSimple: calib: \n" << calib.K() << std::endl; +// +// std::vector pts0, pts1; +// std::srand(1); +// for (int i = 0; i < 8; ++i) { +// pts0.push_back({(double) (std::rand() % calib.width()), (double) (std::rand() % calib.height())}); +// pts1.push_back({(double) (std::rand() % calib.width()), (double) (std::rand() % calib.height())}); +// } +// +// matrix3d F = phg::findFMatrix(pts0, pts1, 10); +// matrix3d E = phg::fmatrix2ematrix(F, calib, calib); +// +// matrix34d P0, P1; +// phg::decomposeEMatrix(P0, P1, E, pts0, pts1, calib, calib); +// +// matrix3d R; +// R = P1.get_minor<3, 3>(0, 0); +// vector3d T; +// T(0) = P1(0, 3); +// T(1) = P1(1, 3); +// T(2) = P1(2, 3); +// +// matrix3d E1 = phg::composeEMatrixRT(R, T); +// matrix3d E2 = phg::composeFMatrix(P0, P1); +// +// EXPECT_NE(E(2, 2), 0); +// EXPECT_NE(E1(2, 2), 0); +// EXPECT_NE(E2(2, 2), 0); +// +// E /= E(2, 2); +// E1 /= E1(2, 2); +// E2 /= E2(2, 2); +// +// double rms1 = matRMS(E, E1); +// double rms2 = matRMS(E, E2); +// double rms3 = matRMS(E1, E2); +// +// std::cout << "E: \n" << E << std::endl; +// std::cout << "E1: \n" << E1 << std::endl; +// std::cout << "E2: \n" << E2 << std::endl; +// std::cout << "RMS1: " << rms1 << std::endl; +// std::cout << "RMS2: " << rms2 << std::endl; +// std::cout << "RMS3: " << rms3 << std::endl; +// +// double eps = 1e-10; +// EXPECT_LT(rms1, eps); +// EXPECT_LT(rms2, eps); +// EXPECT_LT(rms3, eps); +//} + TEST (SFM, TriangulationSimple) { #if !ENABLE_MY_SFM @@ -313,14 +392,14 @@ TEST (SFM, TriangulationSimple) { vector3d x0 = {0, 0, 1}; // P1 - vector3d O = {2, 0, 0}; + vector3d origin = {2, 0, 0}; double alpha = M_PI_4; double s = std::sin(alpha); double c = std::cos(alpha); matrix3d R = { c, 0, s, 0, 1, 0, -s, 0, c}; - vector3d T = -R * O; + vector3d T = -R * origin; matrix34d P1 = { R(0, 0), R(0, 1), R(0, 2), T[0], R(1, 0), R(1, 1), R(1, 2), T[1], @@ -607,10 +686,16 @@ TEST (SFM, Resection) { matrix34d P0res = phg::findCameraMatrix(calib0, Xs, x0s); matrix34d P1res = phg::findCameraMatrix(calib1, Xs, x1s); +// P0res /= P0res(0, 0); +// P0 /= P0(0, 0); +// P1res /= P1res(0, 0); +// P1 /= P1(0, 0); double rms0 = matRMS(P0res, P0); double rms1 = matRMS(P1res, P1); double rms2 = matRMS(P0, P1); + std::cout << "P0s: \n" << P0 << std::endl << P0res << std::endl; + std::cout << "P01s: \n" << P1 << std::endl << P1res << std::endl; EXPECT_LT(rms0, 0.005); EXPECT_LT(rms1, 0.005);