From 65406e4b5bf851d901fc52e069294a8a5cd3dd01 Mon Sep 17 00:00:00 2001 From: Paul-Edouard Sarlin Date: Mon, 12 Jul 2021 18:14:30 +0200 Subject: [PATCH 1/6] Add reprojection error to GP3P --- src/estimators/generalized_absolute_pose.cc | 16 ++++++++++++---- src/estimators/generalized_absolute_pose.h | 5 ++++- 2 files changed, 16 insertions(+), 5 deletions(-) diff --git a/src/estimators/generalized_absolute_pose.cc b/src/estimators/generalized_absolute_pose.cc index ee2f08e8cb..5d409adae5 100644 --- a/src/estimators/generalized_absolute_pose.cc +++ b/src/estimators/generalized_absolute_pose.cc @@ -314,11 +314,19 @@ void GP3PEstimator::Residuals(const std::vector& points2D, const double x_0 = points2D[i].xy(0); const double x_1 = points2D[i].xy(1); - const double inv_x_norm = 1 / std::sqrt(x_0 * x_0 + x_1 * x_1 + 1); - const double cosine_dist = - 1 - inv_pcx_norm * inv_x_norm * (pcx_0 * x_0 + pcx_1 * x_1 + pcx_2); - (*residuals)[i] = cosine_dist * cosine_dist; + if (do_cosine_similarity) { + const double inv_x_norm = 1 / std::sqrt(x_0 * x_0 + x_1 * x_1 + 1); + const double cosine_dist = + 1 - inv_pcx_norm * inv_x_norm * (pcx_0 * x_0 + pcx_1 * x_1 + pcx_2); + (*residuals)[i] = cosine_dist * cosine_dist; + } else { + const double inv_pcx_2 = 1.0 / pcx_2; + const double dx_0 = x_0 - pcx_0 * inv_pcx_2; + const double dx_1 = x_1 - pcx_1 * inv_pcx_2; + const double reproj_error = dx_0 * dx_0 + dx_1 * dx_1; + (*residuals)[i] = reproj_error; + } } else { (*residuals)[i] = std::numeric_limits::max(); diff --git a/src/estimators/generalized_absolute_pose.h b/src/estimators/generalized_absolute_pose.h index bc830efd02..9f04a29ee4 100644 --- a/src/estimators/generalized_absolute_pose.h +++ b/src/estimators/generalized_absolute_pose.h @@ -69,6 +69,9 @@ class GP3PEstimator { // The minimum number of samples needed to estimate a model. static const int kMinNumSamples = 3; + // Whether to compute the cosine similarity or the reprojection error. + bool do_cosine_similarity = true; + // Estimate the most probable solution of the GP3P problem from a set of // three 2D-3D point correspondences. static std::vector Estimate(const std::vector& points2D, @@ -77,7 +80,7 @@ class GP3PEstimator { // Calculate the squared cosine distance error between the rays given a set of // 2D-3D point correspondences and a projection matrix of the generalized // camera. - static void Residuals(const std::vector& points2D, + void Residuals(const std::vector& points2D, const std::vector& points3D, const M_t& proj_matrix, std::vector* residuals); }; From 69d612a058ed9d9127b8cd94012f2143ef2a06b7 Mon Sep 17 00:00:00 2001 From: Paul-Edouard Sarlin Date: Mon, 12 Jul 2021 18:48:14 +0200 Subject: [PATCH 2/6] Update tests --- src/estimators/generalized_absolute_pose_test.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/estimators/generalized_absolute_pose_test.cc b/src/estimators/generalized_absolute_pose_test.cc index 7f17601a9d..0e7ff607e3 100644 --- a/src/estimators/generalized_absolute_pose_test.cc +++ b/src/estimators/generalized_absolute_pose_test.cc @@ -113,14 +113,14 @@ BOOST_AUTO_TEST_CASE(Estimate) { // Test residuals of exact points. std::vector residuals; - GP3PEstimator::Residuals(points2D, points3D, report.model, &residuals); + ransac.estimator.Residuals(points2D, points3D, report.model, &residuals); for (size_t i = 0; i < residuals.size(); ++i) { BOOST_CHECK(residuals[i] < 1e-10); } // Test residuals of faulty points. - GP3PEstimator::Residuals(points2D, points3D_faulty, report.model, - &residuals); + ransac.estimator.Residuals(points2D, points3D_faulty, report.model, + &residuals); for (size_t i = 0; i < residuals.size(); ++i) { BOOST_CHECK(residuals[i] > 1e-10); } From 3cbbd832f9b7c1949a8a93f4b03c555e3f79040f Mon Sep 17 00:00:00 2001 From: Paul-Edouard Sarlin Date: Mon, 12 Jul 2021 18:49:50 +0200 Subject: [PATCH 3/6] Fix style --- src/estimators/generalized_absolute_pose.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/estimators/generalized_absolute_pose.h b/src/estimators/generalized_absolute_pose.h index 9f04a29ee4..4d1e08b68b 100644 --- a/src/estimators/generalized_absolute_pose.h +++ b/src/estimators/generalized_absolute_pose.h @@ -81,8 +81,8 @@ class GP3PEstimator { // 2D-3D point correspondences and a projection matrix of the generalized // camera. void Residuals(const std::vector& points2D, - const std::vector& points3D, - const M_t& proj_matrix, std::vector* residuals); + const std::vector& points3D, + const M_t& proj_matrix, std::vector* residuals); }; } // namespace colmap From 17b75ce56f1bef1e4bc39dcd86d52f7dc039a197 Mon Sep 17 00:00:00 2001 From: Paul-Edouard Sarlin Date: Sun, 8 Aug 2021 15:37:05 +0200 Subject: [PATCH 4/6] Add warning --- src/estimators/generalized_absolute_pose.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/estimators/generalized_absolute_pose.h b/src/estimators/generalized_absolute_pose.h index 4d1e08b68b..acc1baba87 100644 --- a/src/estimators/generalized_absolute_pose.h +++ b/src/estimators/generalized_absolute_pose.h @@ -70,6 +70,9 @@ class GP3PEstimator { static const int kMinNumSamples = 3; // Whether to compute the cosine similarity or the reprojection error. + // [WARNING] The reprojection error being in normalized coordinates, + // the unique error threshold of RANSAC corresponds to different pixel values + // in the different cameras of the rig if they have different intrinsics. bool do_cosine_similarity = true; // Estimate the most probable solution of the GP3P problem from a set of From e1e6061619fd541d9cacc60c26dd184523ef8ab7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Johannes=20Sch=C3=B6nberger?= Date: Tue, 24 Aug 2021 14:05:08 +0200 Subject: [PATCH 5/6] Update generalized_absolute_pose.h --- src/estimators/generalized_absolute_pose.h | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/estimators/generalized_absolute_pose.h b/src/estimators/generalized_absolute_pose.h index acc1baba87..0265e38f5a 100644 --- a/src/estimators/generalized_absolute_pose.h +++ b/src/estimators/generalized_absolute_pose.h @@ -69,11 +69,16 @@ class GP3PEstimator { // The minimum number of samples needed to estimate a model. static const int kMinNumSamples = 3; + enum class ResidualType { + CosineDistance, + ReprojectionError, + }; + // Whether to compute the cosine similarity or the reprojection error. // [WARNING] The reprojection error being in normalized coordinates, // the unique error threshold of RANSAC corresponds to different pixel values // in the different cameras of the rig if they have different intrinsics. - bool do_cosine_similarity = true; + ResidualType residual_type = ResidualType::CosineDistance; // Estimate the most probable solution of the GP3P problem from a set of // three 2D-3D point correspondences. From 4c6a0cbfb6d742b44fcc193374909d1ad7fe7320 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Johannes=20Sch=C3=B6nberger?= Date: Tue, 24 Aug 2021 14:07:13 +0200 Subject: [PATCH 6/6] Update generalized_absolute_pose.cc --- src/estimators/generalized_absolute_pose.cc | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/estimators/generalized_absolute_pose.cc b/src/estimators/generalized_absolute_pose.cc index 5d409adae5..29f829ddad 100644 --- a/src/estimators/generalized_absolute_pose.cc +++ b/src/estimators/generalized_absolute_pose.cc @@ -315,19 +315,20 @@ void GP3PEstimator::Residuals(const std::vector& points2D, const double x_0 = points2D[i].xy(0); const double x_1 = points2D[i].xy(1); - if (do_cosine_similarity) { + if (residual_type == ResidualType::CosineDistance) { const double inv_x_norm = 1 / std::sqrt(x_0 * x_0 + x_1 * x_1 + 1); const double cosine_dist = 1 - inv_pcx_norm * inv_x_norm * (pcx_0 * x_0 + pcx_1 * x_1 + pcx_2); (*residuals)[i] = cosine_dist * cosine_dist; - } else { + } else if (residual_type == ResidualType::ReprojectionError) { const double inv_pcx_2 = 1.0 / pcx_2; const double dx_0 = x_0 - pcx_0 * inv_pcx_2; const double dx_1 = x_1 - pcx_1 * inv_pcx_2; const double reproj_error = dx_0 * dx_0 + dx_1 * dx_1; (*residuals)[i] = reproj_error; - } - + } else { + LOG(FATAL) << "Invalid residual type"; + } } else { (*residuals)[i] = std::numeric_limits::max(); }