colmap · ahojnnes · Jul 14, 2023 · Jul 12, 2023 · Jul 12, 2023 · Jul 12, 2023
diff --git a/src/colmap/base/alignment.cc b/src/colmap/base/alignment.cc
@@ -44,7 +44,7 @@ struct ReconstructionAlignmentEstimator {
 
   typedef const Image* X_t;
   typedef const Image* Y_t;
-  typedef Eigen::Matrix3x4d M_t;
+  typedef Sim3d M_t;
 
   void SetMaxReprojError(const double max_reproj_error) {
     max_squared_reproj_error_ = max_reproj_error * max_reproj_error;
@@ -75,7 +75,7 @@ struct ReconstructionAlignmentEstimator {
 
     Sim3d tgtFromSrc;
     if (tgtFromSrc.Estimate(proj_centers1, proj_centers2)) {
-      return {tgtFromSrc.Matrix()};
+      return {tgtFromSrc};
     }
 
     return {};
@@ -94,7 +94,7 @@ struct ReconstructionAlignmentEstimator {
     CHECK_NOTNULL(src_reconstruction_);
     CHECK_NOTNULL(tgt_reconstruction_);
 
-    const Eigen::Matrix3x4d srcFromTgt = Sim3d(tgtFromSrc).Inverse().Matrix();
+    const Sim3d srcFromTgt = tgtFromSrc.Inverse();
 
     residuals->resize(src_images.size());
 
@@ -134,9 +134,8 @@ struct ReconstructionAlignmentEstimator {
         num_common_points += 1;
 
         const Eigen::Vector3d src_point_in_tgt =
-            tgtFromSrc * src_reconstruction_->Point3D(src_point2D.Point3DId())
-                             .XYZ()
-                             .homogeneous();
+            tgtFromSrc *
+            src_reconstruction_->Point3D(src_point2D.Point3DId()).XYZ();
         if (CalculateSquaredReprojectionError(tgt_point2D.XY(),
                                               src_point_in_tgt,
                                               src_proj_matrix,
@@ -146,9 +145,8 @@ struct ReconstructionAlignmentEstimator {
         }
 
         const Eigen::Vector3d tgt_point_in_src =
-            srcFromTgt * tgt_reconstruction_->Point3D(tgt_point2D.Point3DId())
-                             .XYZ()
-                             .homogeneous();
+            srcFromTgt *
+            tgt_reconstruction_->Point3D(tgt_point2D.Point3DId()).XYZ();
         if (CalculateSquaredReprojectionError(src_point2D.XY(),
                                               tgt_point_in_src,
                                               tgt_proj_matrix,
@@ -180,38 +178,39 @@ struct ReconstructionAlignmentEstimator {
 }  // namespace
 
 bool AlignReconstructionToLocations(
-    const Reconstruction& reconstruction,
-    const std::vector<std::string>& image_names,
-    const std::vector<Eigen::Vector3d>& locations,
+    const Reconstruction& src_reconstruction,
+    const std::vector<std::string>& tgt_image_names,
+    const std::vector<Eigen::Vector3d>& tgt_image_locations,
     const int min_common_images,
     const RANSACOptions& ransac_options,
-    Sim3d* tform) {
+    Sim3d* tgtFromSrc) {
   CHECK_GE(min_common_images, 3);
-  CHECK_EQ(image_names.size(), locations.size());
+  CHECK_EQ(tgt_image_names.size(), tgt_image_locations.size());
 
   // Find out which images are contained in the reconstruction and get the
   // positions of their camera centers.
   std::unordered_set<image_t> common_image_ids;
   std::vector<Eigen::Vector3d> src;
   std::vector<Eigen::Vector3d> dst;
-  for (size_t i = 0; i < image_names.size(); ++i) {
-    const class Image* image = reconstruction.FindImageWithName(image_names[i]);
-    if (image == nullptr) {
+  for (size_t i = 0; i < tgt_image_names.size(); ++i) {
+    const class Image* src_image =
+        src_reconstruction.FindImageWithName(tgt_image_names[i]);
+    if (src_image == nullptr) {
       continue;
     }
 
-    if (!reconstruction.IsImageRegistered(image->ImageId())) {
+    if (!src_reconstruction.IsImageRegistered(src_image->ImageId())) {
       continue;
     }
 
     // Ignore duplicate images.
-    if (common_image_ids.count(image->ImageId()) > 0) {
+    if (common_image_ids.count(src_image->ImageId()) > 0) {
       continue;
     }
 
-    common_image_ids.insert(image->ImageId());
-    src.push_back(image->ProjectionCenter());
-    dst.push_back(locations[i]);
+    common_image_ids.insert(src_image->ImageId());
+    src.push_back(src_image->ProjectionCenter());
+    dst.push_back(tgt_image_locations[i]);
   }
 
   // Only compute the alignment if there are enough correspondences.
@@ -229,8 +228,10 @@ bool AlignReconstructionToLocations(
     return false;
   }
 
-  if (tform != nullptr) {
-    *tform = Sim3d(report.model);
+  if (tgtFromSrc != nullptr) {
+    tgtFromSrc->scale = report.model.col(0).norm();
+    tgtFromSrc->rotation = report.model.leftCols<3>() / tgtFromSrc->scale;
+    tgtFromSrc->translation = report.model.col(3);
   }
 
   return true;
@@ -273,7 +274,7 @@ bool AlignReconstructions(const Reconstruction& src_reconstruction,
   const auto report = ransac.Estimate(src_images, tgt_images);
 
   if (report.success) {
-    *tgtFromSrc = Sim3d(report.model);
+    *tgtFromSrc = report.model;
   }
 
   return report.success;

diff --git a/src/colmap/base/reconstruction.cc b/src/colmap/base/reconstruction.cc
@@ -345,7 +345,8 @@ void Reconstruction::Normalize(const double extent,
     scale = extent / old_extent;
   }
 
-  Sim3d tform(scale, ComposeIdentityQuaternion(), -scale * std::get<2>(bound));
+  Sim3d tform(
+      scale, Eigen::Quaterniond::Identity(), -scale * std::get<2>(bound));
   Transform(tform);
 }
 

diff --git a/src/colmap/base/reconstruction_test.cc b/src/colmap/base/reconstruction_test.cc
@@ -360,7 +360,7 @@ TEST(Reconstruction, Transform) {
   reconstruction.AddObservation(point3D_id, TrackElement(1, 1));
   reconstruction.AddObservation(point3D_id, TrackElement(2, 1));
   reconstruction.Transform(
-      Sim3d(2, ComposeIdentityQuaternion(), Eigen::Vector3d(0, 1, 2)));
+      Sim3d(2, Eigen::Quaterniond::Identity(), Eigen::Vector3d(0, 1, 2)));
   EXPECT_EQ(reconstruction.Image(1).ProjectionCenter(),
             Eigen::Vector3d(0, 1, 2));
   EXPECT_EQ(reconstruction.Point3D(point3D_id).XYZ(), Eigen::Vector3d(2, 3, 4));

diff --git a/src/colmap/controllers/hierarchical_mapper_test.cc b/src/colmap/controllers/hierarchical_mapper_test.cc
@@ -94,7 +94,7 @@ TEST(HierarchicalMapperController, WithoutNoise) {
                              /*num_obs_tolerance=*/0);
 }
 
-TEST(IncrementalMapperController, MultiReconstruction) {
+TEST(HierarchicalMapperController, MultiReconstruction) {
   const std::string database_path = CreateTestDir() + "/database.db";
 
   Database database(database_path);

diff --git a/src/colmap/estimators/absolute_pose_test.cc b/src/colmap/estimators/absolute_pose_test.cc
@@ -33,7 +33,7 @@
 
 #include "colmap/estimators/essential_matrix.h"
 #include "colmap/geometry/pose.h"
-#include "colmap/geometry/sim3.h"
+#include "colmap/geometry/rigid3.h"
 #include "colmap/math/random.h"
 #include "colmap/optim/ransac.h"
 
@@ -43,17 +43,16 @@
 namespace colmap {
 
 TEST(AbsolutePose, P3P) {
-  SetPRNGSeed(0);
-
-  std::vector<Eigen::Vector3d> points3D;
-  points3D.emplace_back(1, 1, 1);
-  points3D.emplace_back(0, 1, 1);
-  points3D.emplace_back(3, 1.0, 4);
-  points3D.emplace_back(3, 1.1, 4);
-  points3D.emplace_back(3, 1.2, 4);
-  points3D.emplace_back(3, 1.3, 4);
-  points3D.emplace_back(3, 1.4, 4);
-  points3D.emplace_back(2, 1, 7);
+  const std::vector<Eigen::Vector3d> points3D = {
+      Eigen::Vector3d(1, 1, 1),
+      Eigen::Vector3d(0, 1, 1),
+      Eigen::Vector3d(3, 1.0, 4),
+      Eigen::Vector3d(3, 1.1, 4),
+      Eigen::Vector3d(3, 1.2, 4),
+      Eigen::Vector3d(3, 1.3, 4),
+      Eigen::Vector3d(3, 1.4, 4),
+      Eigen::Vector3d(2, 1, 7),
+  };
 
   auto points3D_faulty = points3D;
   for (size_t i = 0; i < points3D.size(); ++i) {
@@ -64,13 +63,14 @@ TEST(AbsolutePose, P3P) {
   for (double qx = 0; qx < 1; qx += 0.2) {
     // NOLINTNEXTLINE(clang-analyzer-security.FloatLoopCounter)
     for (double tx = 0; tx < 1; tx += 0.1) {
-      const Sim3d orig_tform(
-          1, Eigen::Vector4d(1, qx, 0, 0), Eigen::Vector3d(tx, 0, 0));
+      const Rigid3d expectedCamFromWorld(
+          Eigen::Quaterniond(1, qx, 0, 0).normalized(),
+          Eigen::Vector3d(tx, 0, 0));
 
       // Project points to camera coordinate system.
       std::vector<Eigen::Vector2d> points2D;
       for (size_t i = 0; i < points3D.size(); ++i) {
-        points2D.push_back((orig_tform * points3D[i]).hnormalized());
+        points2D.push_back((expectedCamFromWorld * points3D[i]).hnormalized());
       }
 
       RANSACOptions options;
@@ -79,11 +79,7 @@ TEST(AbsolutePose, P3P) {
       const auto report = ransac.Estimate(points2D, points3D);
 
       EXPECT_TRUE(report.success);
-
-      // Test if correct transformation has been determined.
-      const double matrix_diff =
-          (orig_tform.Matrix().topLeftCorner<3, 4>() - report.model).norm();
-      EXPECT_TRUE(matrix_diff < 1e-2);
+      EXPECT_LT((expectedCamFromWorld.Matrix() - report.model).norm(), 1e-2);
 
       // Test residuals of exact points.
       std::vector<double> residuals;
@@ -103,17 +99,16 @@ TEST(AbsolutePose, P3P) {
 }
 
 TEST(AbsolutePose, EPNP) {
-  SetPRNGSeed(0);
-
-  std::vector<Eigen::Vector3d> points3D;
-  points3D.emplace_back(1, 1, 1);
-  points3D.emplace_back(0, 1, 1);
-  points3D.emplace_back(3, 1.0, 4);
-  points3D.emplace_back(3, 1.1, 4);
-  points3D.emplace_back(3, 1.2, 4);
-  points3D.emplace_back(3, 1.3, 4);
-  points3D.emplace_back(3, 1.4, 4);
-  points3D.emplace_back(2, 1, 7);
+  const std::vector<Eigen::Vector3d> points3D = {
+      Eigen::Vector3d(1, 1, 1),
+      Eigen::Vector3d(0, 1, 1),
+      Eigen::Vector3d(3, 1.0, 4),
+      Eigen::Vector3d(3, 1.1, 4),
+      Eigen::Vector3d(3, 1.2, 4),
+      Eigen::Vector3d(3, 1.3, 4),
+      Eigen::Vector3d(3, 1.4, 4),
+      Eigen::Vector3d(2, 1, 7),
+  };
 
   auto points3D_faulty = points3D;
   for (size_t i = 0; i < points3D.size(); ++i) {
@@ -124,13 +119,14 @@ TEST(AbsolutePose, EPNP) {
   for (double qx = 0; qx < 1; qx += 0.2) {
     // NOLINTNEXTLINE(clang-analyzer-security.FloatLoopCounter)
     for (double tx = 0; tx < 1; tx += 0.1) {
-      const Sim3d orig_tform(
-          1, Eigen::Vector4d(1, qx, 0, 0), Eigen::Vector3d(tx, 0, 0));
+      const Rigid3d expectedCamFromWorld(
+          Eigen::Quaterniond(1, qx, 0, 0).normalized(),
+          Eigen::Vector3d(tx, 0, 0));
 
       // Project points to camera coordinate system.
       std::vector<Eigen::Vector2d> points2D;
       for (size_t i = 0; i < points3D.size(); ++i) {
-        points2D.push_back((orig_tform * points3D[i]).hnormalized());
+        points2D.push_back((expectedCamFromWorld * points3D[i]).hnormalized());
       }
 
       RANSACOptions options;
@@ -139,11 +135,7 @@ TEST(AbsolutePose, EPNP) {
       const auto report = ransac.Estimate(points2D, points3D);
 
       EXPECT_TRUE(report.success);
-
-      // Test if correct transformation has been determined.
-      const double matrix_diff =
-          (orig_tform.Matrix().topLeftCorner<3, 4>() - report.model).norm();
-      EXPECT_TRUE(matrix_diff < 1e-3);
+      EXPECT_LT((expectedCamFromWorld.Matrix() - report.model).norm(), 1e-4);
 
       // Test residuals of exact points.
       std::vector<double> residuals;

diff --git a/src/colmap/estimators/coordinate_frame.cc b/src/colmap/estimators/coordinate_frame.cc
@@ -320,7 +320,7 @@ void AlignToPrincipalPlane(Reconstruction* recon, Sim3d* tform) {
   rot_mat << basis.col(0), basis.col(1), basis.col(0).cross(basis.col(1));
   rot_mat.transposeInPlace();
 
-  *tform = Sim3d(1.0, RotationMatrixToQuaternion(rot_mat), -rot_mat * centroid);
+  *tform = Sim3d(1.0, Eigen::Quaterniond(rot_mat), -rot_mat * centroid);
 
   // if camera plane ends up below ground then flip basis vectors and create new
   // transform
@@ -329,8 +329,7 @@ void AlignToPrincipalPlane(Reconstruction* recon, Sim3d* tform) {
   if (test_img.ProjectionCenter().z() < 0.0) {
     rot_mat << basis.col(0), -basis.col(1), basis.col(0).cross(-basis.col(1));
     rot_mat.transposeInPlace();
-    *tform =
-        Sim3d(1.0, RotationMatrixToQuaternion(rot_mat), -rot_mat * centroid);
+    *tform = Sim3d(1.0, Eigen::Quaterniond(rot_mat), -rot_mat * centroid);
   }
 
   recon->Transform(*tform);
@@ -352,10 +351,9 @@ void AlignToENUPlane(Reconstruction* recon, Sim3d* tform, bool unscaled) {
   rot_mat << -sin_lon, cos_lon, 0, -cos_lon * sin_lat, -sin_lon * sin_lat,
       cos_lat, cos_lon * cos_lat, sin_lon * cos_lat, sin_lat;
 
-  const double scale = unscaled ? 1.0 / tform->Scale() : 1.0;
-  *tform = Sim3d(scale,
-                 RotationMatrixToQuaternion(rot_mat),
-                 -(scale * rot_mat) * centroid);
+  const double scale = unscaled ? 1.0 / tform->scale : 1.0;
+  *tform =
+      Sim3d(scale, Eigen::Quaterniond(rot_mat), -(scale * rot_mat) * centroid);
   recon->Transform(*tform);
 }
 

diff --git a/src/colmap/estimators/coordinate_frame_test.cc b/src/colmap/estimators/coordinate_frame_test.cc
@@ -32,6 +32,7 @@
 #include "colmap/estimators/coordinate_frame.h"
 
 #include "colmap/geometry/gps.h"
+#include "colmap/geometry/projection.h"
 
 #include <gtest/gtest.h>
 
@@ -66,18 +67,18 @@ TEST(CoordinateFrame, AlignToPrincipalPlane) {
   image.Tvec() = Eigen::Vector3d(-1.0, 0.0, 0.0);
   reconstruction.AddImage(image);
   // Setup 4 points on the Y-Z plane
-  point3D_t p1 =
+  const point3D_t p1 =
       reconstruction.AddPoint3D(Eigen::Vector3d(0.0, -1.0, 0.0), Track());
-  point3D_t p2 =
+  const point3D_t p2 =
       reconstruction.AddPoint3D(Eigen::Vector3d(0.0, 1.0, 0.0), Track());
-  point3D_t p3 =
+  const point3D_t p3 =
       reconstruction.AddPoint3D(Eigen::Vector3d(0.0, 0.0, -1.0), Track());
-  point3D_t p4 =
+  const point3D_t p4 =
       reconstruction.AddPoint3D(Eigen::Vector3d(0.0, 0.0, 1.0), Track());
   AlignToPrincipalPlane(&reconstruction, &tform);
   // Note that the final X and Y axes may be inverted after alignment, so we
   // need to account for both cases when checking for correctness
-  const bool inverted = tform.Rotation()(2) < 0;
+  const bool inverted = tform.rotation.y() < 0;
 
   // Verify that points lie on the correct locations of the X-Y plane
   EXPECT_LE((reconstruction.Point3D(p1).XYZ() -
@@ -102,14 +103,13 @@ TEST(CoordinateFrame, AlignToPrincipalPlane) {
                 .norm(),
             1e-6);
   // Verify that transform matrix does shuffling of axes
-  Eigen::Matrix3x4d mat;
+  Eigen::Matrix3x4d expected;
   if (inverted) {
-    mat << 0, -1, 0, 0, 0, 0, -1, 0, 1, 0, 0, 0;
+    expected << 0, -1, 0, 0, 0, 0, -1, 0, 1, 0, 0, 0;
   } else {
-    mat << 0, 1, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0;
+    expected << 0, 1, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0;
   }
-  std::cout << tform.Matrix() << std::endl;
-  EXPECT_LE((tform.Matrix() - mat).norm(), 1e-6);
+  EXPECT_LT((tform.Matrix() - expected).norm(), 1e-6);
 }
 
 TEST(CoordinateFrame, AlignToENUPlane) {

diff --git a/src/colmap/estimators/generalized_absolute_pose.h b/src/colmap/estimators/generalized_absolute_pose.h
@@ -53,6 +53,7 @@ class GP3PEstimator {
   struct X_t {
     // The relative transformation from the generalized camera to the camera
     // frame of the observation.
+    // TODO(coord-convention)
     Eigen::Matrix3x4d rel_tform;
     // The 2D image feature observation.
     Eigen::Vector2d xy;