colmap · ahojnnes · Aug 24, 2021 · Jul 12, 2021 · Jul 12, 2021 · Jul 12, 2021
diff --git a/src/estimators/generalized_absolute_pose.cc b/src/estimators/generalized_absolute_pose.cc
@@ -314,12 +314,21 @@ void GP3PEstimator::Residuals(const std::vector<X_t>& 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 (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 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<double>::max();
     }

diff --git a/src/estimators/generalized_absolute_pose.h b/src/estimators/generalized_absolute_pose.h
@@ -69,6 +69,17 @@ 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.
+  ResidualType residual_type = ResidualType::CosineDistance;
+
   // Estimate the most probable solution of the GP3P problem from a set of
   // three 2D-3D point correspondences.
   static std::vector<M_t> Estimate(const std::vector<X_t>& points2D,
@@ -77,9 +88,9 @@ 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<X_t>& points2D,
-                        const std::vector<Y_t>& points3D,
-                        const M_t& proj_matrix, std::vector<double>* residuals);
+  void Residuals(const std::vector<X_t>& points2D,
+                 const std::vector<Y_t>& points3D,
+                 const M_t& proj_matrix, std::vector<double>* residuals);
 };
 
 }  // namespace colmap

diff --git 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<double> 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);
       }