diff --git a/src/phg/sfm/ematrix.cpp b/src/phg/sfm/ematrix.cpp index 3bc052b0..c51f9900 100644 --- a/src/phg/sfm/ematrix.cpp +++ b/src/phg/sfm/ematrix.cpp @@ -18,8 +18,12 @@ 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 a = 0.5 * (s[0] + s[1]); + Eigen::MatrixXd S = Eigen::MatrixXd::Zero(3, 3); + S(0, 0) = a; + S(1, 1) = a; + E = svd.matrixU() * S * svd.matrixV().transpose(); copy(E, Ecv); } @@ -28,12 +32,11 @@ 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 = calib1.K().t() * F * calib0.K(); + + ensureSpectralProperty(E); + + return E; } namespace { @@ -61,21 +64,23 @@ 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 = P0 * X; + vector3d x1 = P1 * X; + + // точка должна иметь положительную глубину для обеих камер + return x0[2] > 0 && x1[2] > 0; } } @@ -88,80 +93,84 @@ namespace { // первичное разложение существенной матрицы (а из него, взаимное расположение камер) для последующего уточнения методом нелинейной оптимизации 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); + + // (см. 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 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(); + + // 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 50127189..a16917a9 100644 --- a/src/phg/sfm/fmatrix.cpp +++ b/src/phg/sfm/fmatrix.cpp @@ -2,6 +2,7 @@ #include "sfm_utils.h" #include "defines.h" +#include #include #include #include @@ -25,49 +26,70 @@ 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; + int a_rows = count; + int a_cols = 9; + + 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]; + + A.row(i_pair) << x1 * x0, x1 * y0, x1, y1 * x0, y1 * y0, y1, x0, y0, 1.0; + } + + Eigen::JacobiSVD svda(A, Eigen::ComputeFullU | Eigen::ComputeFullV); + Eigen::VectorXd null_space = svda.matrixV().col(a_cols - 1); + + 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); + + Eigen::VectorXd s = svdf.singularValues(); + s[2] = 0.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"); + if (m.empty()) { + throw std::runtime_error("getNormalizeTransform: empty points"); + } + + cv::Vec2d center(0.0, 0.0); + for (const cv::Vec2d &pt : m) { + center += pt; + } + center *= 1.0 / m.size(); + + double rms = 0.0; + for (const cv::Vec2d &pt : m) { + cv::Vec2d d = pt - center; + rms += d.dot(d); + } + rms = std::sqrt(rms / m.size()); + + if (rms == 0.0) { + throw std::runtime_error("getNormalizeTransform: zero rms"); + } + + double scale = std::sqrt(2.0) / rms; + return cv::Matx33d(scale, 0.0, -scale * center[0], 0.0, scale, -scale * center[1], 0.0, 0.0, 1.0); } cv::Vec2d transformPoint(const cv::Vec2d &pt, const cv::Matx33d &T) @@ -104,61 +126,60 @@ namespace { getNormalizeTransform(m0_t); getNormalizeTransform(m1_t); } - 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; + // https://en.wikipedia.org/wiki/Random_sample_consensus#Parameters + // будет отличаться от случая с гомографией + const int n_trials = n_matches > 10000 ? 100000 : 2000; + + const int n_samples = 8; + 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 = TN1.t() * F * 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"); + } + + return best_F; } } diff --git a/src/phg/sfm/resection.cpp b/src/phg/sfm/resection.cpp index d2cf6433..39f74f0c 100644 --- a/src/phg/sfm/resection.cpp +++ b/src/phg/sfm/resection.cpp @@ -49,95 +49,133 @@ 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 -// -// return canonicalizeP(result); + using mat = Eigen::MatrixXd; + using vec = Eigen::VectorXd; + + mat A(2 * count, 12); + + 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; + + A.row(2 * i) << 0.0, 0.0, 0.0, 0.0, -w * X, -w * Y, -w * Z, -w * W, y * X, y * Y, y * Z, y * W; + A.row(2 * i + 1) << w * X, w * Y, w * Z, w * W, 0.0, 0.0, 0.0, 0.0, -x * X, -x * Y, -x * Z, -x * W; + } + + Eigen::JacobiSVD svd(A, Eigen::ComputeFullU | 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(); + + // https://en.wikipedia.org/wiki/Random_sample_consensus#Parameters + // будет отличаться от случая с гомографией + const int n_trials = 100000; + + const double threshold_px = 3; + + const int n_samples = 6; + 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); + + int support = 0; + for (int i = 0; i < n_points; ++i) { + vector4d Xh(X[i][0], X[i][1], X[i][2], 1.0); + vector3d p = P * Xh; + if (p[2] == 0) { + continue; + } + p /= p[2]; + cv::Vec3d pxh = calib.project(p); + if (pxh[2] == 0) { + continue; + } + cv::Vec2d px(pxh[0] / pxh[2], pxh[1] / pxh[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"); + } + + std::vector inliers_X; + std::vector inliers_x; + for (int i = 0; i < n_points; ++i) { + vector4d Xh(X[i][0], X[i][1], X[i][2], 1.0); + vector3d p = best_P * Xh; + if (p[2] == 0) { + continue; + } + p /= p[2]; + cv::Vec3d pxh = calib.project(p); + if (pxh[2] == 0) { + continue; + } + cv::Vec2d px(pxh[0] / pxh[2], pxh[1] / pxh[2]); + if (cv::norm(px - x[i]) < threshold_px) { + inliers_X.push_back(X[i]); + inliers_x.push_back(calib.unproject(x[i])); + } + } + + if ((int) inliers_X.size() >= n_samples) { + best_P = estimateCameraMatrixDLT(inliers_X.data(), inliers_x.data(), inliers_X.size()); + } + + return best_P; } diff --git a/src/phg/sfm/sfm_utils.cpp b/src/phg/sfm/sfm_utils.cpp index d2d2e294..c67e8180 100644 --- a/src/phg/sfm/sfm_utils.cpp +++ b/src/phg/sfm/sfm_utils.cpp @@ -41,5 +41,15 @@ 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"); + const cv::Vec3d x0(pt0[0], pt0[1], 1.0); + const cv::Vec3d x1(pt1[0], pt1[1], 1.0); + const cv::Vec3d line = F * x0; + + const double norm = std::sqrt(line[0] * line[0] + line[1] * line[1]); + if (norm == 0) { + return false; + } + + const double dist = std::abs(line.dot(x1)) / norm; + return dist < t; } diff --git a/src/phg/sfm/triangulation.cpp b/src/phg/sfm/triangulation.cpp index 8dd11e69..407a3caa 100644 --- a/src/phg/sfm/triangulation.cpp +++ b/src/phg/sfm/triangulation.cpp @@ -12,5 +12,24 @@ cv::Vec4d phg::triangulatePoint(const cv::Matx34d *Ps, const cv::Vec3d *ms, int { // составление однородной системы + SVD // без подвохов - throw std::runtime_error("not implemented yet"); + Eigen::MatrixXd A(2 * count, 4); + + for (int i = 0; i < count; ++i) { + const cv::Matx34d &P = Ps[i]; + const cv::Vec3d &m = ms[i]; + + A.row(2 * i) << m[0] * P(2, 0) - m[2] * P(0, 0), + m[0] * P(2, 1) - m[2] * P(0, 1), + m[0] * P(2, 2) - m[2] * P(0, 2), + m[0] * P(2, 3) - m[2] * P(0, 3); + A.row(2 * i + 1) << m[1] * P(2, 0) - m[2] * P(1, 0), + m[1] * P(2, 1) - m[2] * P(1, 1), + m[1] * P(2, 2) - m[2] * P(1, 2), + m[1] * P(2, 3) - m[2] * P(1, 3); + } + + Eigen::JacobiSVD svd(A, Eigen::ComputeFullU | 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 {