From 4483d607c5621c503ca69f34208471adddfccb19 Mon Sep 17 00:00:00 2001 From: Igor Loginov Date: Wed, 12 Jun 2024 19:48:24 +0300 Subject: [PATCH] Done resection --- src/phg/sfm/resection.cpp | 187 +++++++++++++++++++++----------------- 1 file changed, 104 insertions(+), 83 deletions(-) diff --git a/src/phg/sfm/resection.cpp b/src/phg/sfm/resection.cpp index d2cf643..227bcd6 100644 --- a/src/phg/sfm/resection.cpp +++ b/src/phg/sfm/resection.cpp @@ -49,95 +49,116 @@ 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, + -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, + -x * X, -x * Y, -x * Z, -x * W; + } + + mat result(3, 4); + + Eigen::JacobiSVD svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV); + Eigen::VectorXd null_space = svd.matrixV().col(11); + + result.row(0) << null_space[0], null_space[1], null_space[2], null_space[3]; + result.row(1) << null_space[4], null_space[5], null_space[6], null_space[7]; + result.row(2) << null_space[8], null_space[9], null_space[10], null_space[11]; + + cv::Matx34d resultcv; + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 4; j++) { + resultcv(i, j) = result(i, j); + } + } + + return canonicalizeP(resultcv); } // По трехмерным точкам и их проекциям на изображении определяем положение камеры 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 double threshold_px = 3; + + const int n_samples = 6; + const double prob_ok = 0.42; + const int n_trials = std::round(1 + 4 * std::log(1 - 0.999) / std::log(1 - std::pow(prob_ok, n_samples))); + std::cout << n_trials << std::endl; + 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) { + cv::Vec3d px = calib.project(P * cv::Vec4d(X[i](0), X[i](1), X[i](2), 1)); + cv::Vec2d x1(px(0) / px(2), px(1) / px(2)); + if (cv::norm(x1 - 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; }