Share SolveEpipolarConstraintMatrix between eight-point F and E

src/colmap/estimators/solvers/CMakeLists.txt

@@ -20,6 +20,7 @@ COLMAP_ADD_LIBRARY(
         relpose_shared_focal.h relpose_shared_focal.cc
         similarity_transform.h similarity_transform.cc
         translation_transform.h
+        utils.h utils.cc
     PUBLIC_LINK_LIBS
         colmap_util
         colmap_math
@@ -77,6 +78,11 @@ COLMAP_ADD_TEST(
     SRCS translation_transform_test.cc
     LINK_LIBS colmap_estimators_solvers
 )
+COLMAP_ADD_TEST(
+    NAME utils_test
+    SRCS utils_test.cc
+    LINK_LIBS colmap_estimators_solvers
+)
 COLMAP_ADD_TEST(
     NAME poselib_utils_test
     SRCS poselib_utils_test.cc

src/colmap/estimators/solvers/essential_matrix.cc

@@ -4,6 +4,7 @@
 
 #include "colmap/estimators/cost_functions/tiny_manifold.h"
 #include "colmap/estimators/cost_functions/tiny_sampson_error.h"
+#include "colmap/estimators/solvers/utils.h"
 #include "colmap/geometry/essential_matrix.h"
 #include "colmap/geometry/rigid3.h"
 #include "colmap/math/polynomial.h"
@@ -163,58 +164,18 @@ void EssentialMatrixEightPointEstimator::Estimate(
         cam_rays2[i].z() * cam_rays1[i].transpose();
   }
 
-  // Solve for the nullspace of the constraint matrix.
-  Eigen::Matrix3d Q;
-  if (cam_rays1.size() == 8) {
-    Eigen::Matrix<double, 9, 9> QQ =
-        A.transpose().householderQr().householderQ();
-    Q = Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor>>(
-        QQ.col(8).data());
-  } else {
-    Eigen::JacobiSVD<Eigen::Matrix<double, Eigen::Dynamic, 9>> svd(
-        A, Eigen::ComputeFullV);
-    Q = Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor>>(
-        svd.matrixV().col(8).data());
-  }
-
-  // Enforcing the internal constraint that two singular values must be non-zero
-  // and one must be zero.
-  Eigen::JacobiSVD<Eigen::Matrix3d> svd(
-      Q, Eigen::ComputeFullU | Eigen::ComputeFullV);
-  Eigen::Vector3d singular_values = svd.singularValues();
-  singular_values(2) = 0.0;
-  const Eigen::Matrix3d E =
-      svd.matrixU() * singular_values.asDiagonal() * svd.matrixV().transpose();
-
   models->resize(1);
-  (*models)[0] = E;
-}
-
-namespace {
-
-// Extract the bearings into the contiguous array the five-point solver expects.
-// The Jacobians play no part in estimation. They only affect scoring.
-std::vector<Eigen::Vector3d> UnpackCamRaysWithJac(
-    const std::vector<CamRayWithJac>& cam_rays_with_jac) {
-  std::vector<Eigen::Vector3d> rays;
-  rays.reserve(cam_rays_with_jac.size());
-  for (const CamRayWithJac& cam_ray_with_jac : cam_rays_with_jac) {
-    rays.push_back(cam_ray_with_jac.ray);
-  }
-  return rays;
+  (*models)[0] = SolveEpipolarConstraintMatrix(A);
 }
 
-}  // namespace
-
 void EssentialMatrixTangentSampsonEstimator::Estimate(
     const std::vector<X_t>& cam_rays1_with_jac,
     const std::vector<Y_t>& cam_rays2_with_jac,
     std::vector<M_t>* models) {
-  const std::vector<Eigen::Vector3d> rays1 =
-      UnpackCamRaysWithJac(cam_rays1_with_jac);
-  const std::vector<Eigen::Vector3d> rays2 =
-      UnpackCamRaysWithJac(cam_rays2_with_jac);
-  EssentialMatrixFivePointEstimator::Estimate(rays1, rays2, models);
+  EssentialMatrixFivePointEstimator::Estimate(
+      RaysFromCamRaysWithJac(cam_rays1_with_jac),
+      RaysFromCamRaysWithJac(cam_rays2_with_jac),
+      models);
 }
 
 bool EssentialMatrixTangentSampsonEstimator::Refine(
@@ -227,13 +188,13 @@ bool EssentialMatrixTangentSampsonEstimator::Refine(
 
   // Decompose the initial E into a relative pose (resolving the four-fold
   // ambiguity via cheirality over the bearings).
-  const std::vector<Eigen::Vector3d> rays1 =
-      UnpackCamRaysWithJac(cam_rays1_with_jac);
-  const std::vector<Eigen::Vector3d> rays2 =
-      UnpackCamRaysWithJac(cam_rays2_with_jac);
   Rigid3d cam2_from_cam1;
   std::vector<int> valid_indices;
-  PoseFromEssentialMatrix(*E, rays1, rays2, &cam2_from_cam1, &valid_indices);
+  PoseFromEssentialMatrix(*E,
+                          RaysFromCamRaysWithJac(cam_rays1_with_jac),
+                          RaysFromCamRaysWithJac(cam_rays2_with_jac),
+                          &cam2_from_cam1,
+                          &valid_indices);
   if (valid_indices.empty()) {
     return false;
   }

src/colmap/estimators/solvers/fundamental_matrix.cc

@@ -4,6 +4,7 @@
 
 #include "colmap/estimators/cost_functions/tiny_manifold.h"
 #include "colmap/estimators/cost_functions/tiny_sampson_error.h"
+#include "colmap/estimators/solvers/utils.h"
 #include "colmap/geometry/essential_matrix.h"
 #include "colmap/geometry/normalization.h"
 #include "colmap/math/polynomial.h"
@@ -179,28 +180,7 @@ void FundamentalMatrixEightPointEstimator::Estimate(
         normed_points1[i].transpose().homogeneous();
   }
 
-  // Solve for the nullspace of the constraint matrix.
-  Eigen::Matrix3d Q;
-  if (points1.size() == 8) {
-    Eigen::Matrix<double, 9, 9> QQ =
-        A.transpose().householderQr().householderQ();
-    Q = Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor>>(
-        QQ.col(8).data());
-  } else {
-    Eigen::JacobiSVD<Eigen::Matrix<double, Eigen::Dynamic, 9>> svd(
-        A, Eigen::ComputeFullV);
-    Q = Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor>>(
-        svd.matrixV().col(8).data());
-  }
-
-  // Enforcing the internal constraint that two singular values must non-zero
-  // and one must be zero.
-  Eigen::JacobiSVD<Eigen::Matrix3d> svd(
-      Q, Eigen::ComputeFullU | Eigen::ComputeFullV);
-  Eigen::Vector3d singular_values = svd.singularValues();
-  singular_values(2) = 0.0;
-  const Eigen::Matrix3d F =
-      svd.matrixU() * singular_values.asDiagonal() * svd.matrixV().transpose();
+  const Eigen::Matrix3d F = SolveEpipolarConstraintMatrix(A);
 
   models->resize(1);
   (*models)[0] = normed_from_orig2.transpose() * F * normed_from_orig1;

src/colmap/estimators/solvers/utils.cc

@@ -0,0 +1,48 @@
+// SPDX-License-Identifier: BSD-3-Clause
+
+#include "colmap/estimators/solvers/utils.h"
+
+#include "colmap/util/logging.h"
+
+#include <Eigen/QR>
+#include <Eigen/SVD>
+
+namespace colmap {
+
+Eigen::Matrix3d SolveEpipolarConstraintMatrix(
+    const Eigen::Matrix<double, Eigen::Dynamic, 9>& A) {
+  THROW_CHECK_GE(A.rows(), 8);
+
+  // Solve for the nullspace of the constraint matrix.
+  Eigen::Matrix3d Q;
+  if (A.rows() == 8) {
+    Eigen::Matrix<double, 9, 9> QQ =
+        A.transpose().householderQr().householderQ();
+    Q = Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor>>(
+        QQ.col(8).data());
+  } else {
+    Eigen::JacobiSVD<Eigen::Matrix<double, Eigen::Dynamic, 9>> svd(
+        A, Eigen::ComputeFullV);
+    Q = Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor>>(
+        svd.matrixV().col(8).data());
+  }
+
+  // Enforce rank at most 2.
+  Eigen::JacobiSVD<Eigen::Matrix3d> svd(
+      Q, Eigen::ComputeFullU | Eigen::ComputeFullV);
+  Eigen::Vector3d singular_values = svd.singularValues();
+  singular_values(2) = 0.0;
+  return svd.matrixU() * singular_values.asDiagonal() *
+         svd.matrixV().transpose();
+}
+
+std::vector<Eigen::Vector3d> RaysFromCamRaysWithJac(
+    const std::vector<CamRayWithJac>& cam_rays_with_jac) {
+  std::vector<Eigen::Vector3d> rays(cam_rays_with_jac.size());
+  for (size_t i = 0; i < cam_rays_with_jac.size(); ++i) {
+    rays[i] = cam_rays_with_jac[i].ray;
+  }
+  return rays;
+}
+
+}  // namespace colmap

src/colmap/estimators/solvers/utils.h

@@ -0,0 +1,24 @@
+// SPDX-License-Identifier: BSD-3-Clause
+
+#pragma once
+
+#include "colmap/geometry/pose.h"
+#include "colmap/util/eigen_alignment.h"
+
+#include <vector>
+
+#include <Eigen/Core>
+
+namespace colmap {
+
+// Solve an Nx9 (N >= 8) epipolar system and enforce rank 2. The minimal
+// N == 8 case takes the exact QR nullspace, while N > 8 takes the least-squares
+// SVD nullspace.
+Eigen::Matrix3d SolveEpipolarConstraintMatrix(
+    const Eigen::Matrix<double, Eigen::Dynamic, 9>& A);
+
+// Extract rays while discarding their measurement Jacobians.
+std::vector<Eigen::Vector3d> RaysFromCamRaysWithJac(
+    const std::vector<CamRayWithJac>& cam_rays_with_jac);
+
+}  // namespace colmap

src/colmap/estimators/solvers/utils_test.cc

@@ -0,0 +1,61 @@
+// SPDX-License-Identifier: BSD-3-Clause
+
+#include "colmap/estimators/solvers/utils.h"
+
+#include "colmap/geometry/essential_matrix.h"
+#include "colmap/geometry/rigid3.h"
+#include "colmap/util/eigen_matchers.h"
+
+#include <vector>
+
+#include <Eigen/SVD>
+#include <gtest/gtest.h>
+
+namespace colmap {
+namespace {
+
+TEST(SolveEpipolarConstraintMatrix, ExactRecoveryIsRankTwo) {
+  const Rigid3d cam2_from_cam1(Eigen::Quaterniond::Identity(),
+                               Eigen::Vector3d(1, 0, 0));
+  const Eigen::Matrix3d E_expected = EssentialMatrixFromPose(cam2_from_cam1);
+
+  for (const int num_points : {8, 64}) {
+    Eigen::Matrix<double, Eigen::Dynamic, 9> A(num_points, 9);
+    std::vector<Eigen::Vector3d> rays1(num_points);
+    std::vector<Eigen::Vector3d> rays2(num_points);
+    for (int i = 0; i < num_points; ++i) {
+      // Deterministic non-coplanar spread in front of both cameras (a planar
+      // point set would leave the epipolar matrix under-determined).
+      const Eigen::Vector3d point_in_cam1(
+          (i % 8) - 3.5, ((i * 3) % 8) - 3.5, 4.0 + (i % 5) * 0.5);
+      rays1[i] = point_in_cam1.normalized();
+      rays2[i] = (cam2_from_cam1 * point_in_cam1).normalized();
+      A.row(i) << rays2[i].x() * rays1[i].transpose(),
+          rays2[i].y() * rays1[i].transpose(),
+          rays2[i].z() * rays1[i].transpose();
+    }
+
+    const Eigen::Matrix3d E = SolveEpipolarConstraintMatrix(A);
+    EXPECT_LT(std::min((E.normalized() - E_expected.normalized()).norm(),
+                       (E.normalized() + E_expected.normalized()).norm()),
+              1e-6);
+    EXPECT_LT(Eigen::JacobiSVD<Eigen::Matrix3d>(E).singularValues()(2), 1e-12);
+    for (int i = 0; i < num_points; ++i) {
+      EXPECT_LT(std::abs(rays2[i].dot(E * rays1[i])), 1e-10);
+    }
+  }
+}
+
+TEST(RaysFromCamRaysWithJac, Nominal) {
+  const std::vector<CamRayWithJac> rays_with_jac = {
+      {Eigen::Vector3d(1, 0, 0), Eigen::Matrix3x2d::Ones()},
+      {Eigen::Vector3d(0, 1, 0), Eigen::Matrix3x2d::Zero()}};
+  const std::vector<Eigen::Vector3d> rays =
+      RaysFromCamRaysWithJac(rays_with_jac);
+  ASSERT_EQ(rays.size(), 2);
+  EXPECT_EQ(rays[0], Eigen::Vector3d(1, 0, 0));
+  EXPECT_EQ(rays[1], Eigen::Vector3d(0, 1, 0));
+}
+
+}  // namespace
+}  // namespace colmap