colmap · ahojnnes · Jul 12, 2023 · Jul 12, 2023 · Jul 12, 2023 · Jul 12, 2023
diff --git a/src/colmap/base/reconstruction.cc b/src/colmap/base/reconstruction.cc
@@ -32,11 +32,13 @@
 #include "colmap/base/reconstruction.h"
 
 #include "colmap/base/database_cache.h"
+#include "colmap/estimators/similarity_transform.h"
 #include "colmap/geometry/gps.h"
 #include "colmap/geometry/pose.h"
 #include "colmap/geometry/projection.h"
 #include "colmap/geometry/triangulation.h"
 #include "colmap/image/bitmap.h"
+#include "colmap/optim/loransac.h"
 #include "colmap/util/misc.h"
 #include "colmap/util/ply.h"
 
@@ -432,7 +434,7 @@ void Reconstruction::Transform(const SimilarityTransform3& tform) {
     tform.TransformPose(&image.second.Qvec(), &image.second.Tvec());
   }
   for (auto& point3D : points3D_) {
-    tform.TransformPoint(&point3D.second.XYZ());
+    point3D.second.XYZ() = tform * point3D.second.XYZ();
   }
 }
 
@@ -539,8 +541,7 @@ bool Reconstruction::Merge(const Reconstruction& reconstruction,
         (new_track.Length() + old_track.Length()) >= 2 &&
         old_point3D_ids.size() == 1;
     if (create_new_point || merge_new_and_old_point) {
-      Eigen::Vector3d xyz = point3D.second.XYZ();
-      alignment.TransformPoint(&xyz);
+      const Eigen::Vector3d xyz = alignment * point3D.second.XYZ();
       const auto point3D_id =
           AddPoint3D(xyz, new_track, point3D.second.Color());
       if (old_point3D_ids.size() == 1) {
@@ -1265,13 +1266,11 @@ void Reconstruction::ExportVRML(const std::string& images_path,
     images_file << " coord Coordinate {\n";
     images_file << " point [\n";
 
-    Eigen::Transform<double, 3, Eigen::Affine> transform;
-    transform.matrix().topLeftCorner<3, 4>() =
-        image.second.InverseProjectionMatrix();
-
     // Move camera base model to camera pose.
+    const Eigen::Matrix3x4d worldFromCam =
+        image.second.InverseProjectionMatrix();
     for (size_t i = 0; i < points.size(); i++) {
-      const Eigen::Vector3d point = transform * points[i];
+      const Eigen::Vector3d point = worldFromCam * points[i].homogeneous();
       images_file << point(0) << " " << point(1) << " " << point(2) << "\n";
     }
 
@@ -2174,4 +2173,113 @@ void Reconstruction::ResetTriObservations(const image_t image_id,
   }
 }
 
+bool Reconstruction::Align(const std::vector<std::string>& image_names,
+                           const std::vector<Eigen::Vector3d>& locations,
+                           const int min_common_images,
+                           SimilarityTransform3* tform) {
+  CHECK_GE(min_common_images, 3);
+  CHECK_EQ(image_names.size(), 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 = FindImageWithName(image_names[i]);
+    if (image == nullptr) {
+      continue;
+    }
+
+    if (!IsImageRegistered(image->ImageId())) {
+      continue;
+    }
+
+    // Ignore duplicate images.
+    if (common_image_ids.count(image->ImageId()) > 0) {
+      continue;
+    }
+
+    common_image_ids.insert(image->ImageId());
+    src.push_back(image->ProjectionCenter());
+    dst.push_back(locations[i]);
+  }
+
+  // Only compute the alignment if there are enough correspondences.
+  if (common_image_ids.size() < static_cast<size_t>(min_common_images)) {
+    return false;
+  }
+
+  SimilarityTransform3 transform;
+  if (!transform.Estimate(src, dst)) {
+    return false;
+  }
+
+  Transform(transform);
+
+  if (tform != nullptr) {
+    *tform = transform;
+  }
+
+  return true;
+}
+
+bool Reconstruction::AlignRobust(const std::vector<std::string>& image_names,
+                                 const std::vector<Eigen::Vector3d>& locations,
+                                 const int min_common_images,
+                                 const RANSACOptions& ransac_options,
+                                 SimilarityTransform3* tform) {
+  CHECK_GE(min_common_images, 3);
+  CHECK_EQ(image_names.size(), 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 = FindImageWithName(image_names[i]);
+    if (image == nullptr) {
+      continue;
+    }
+
+    if (!IsImageRegistered(image->ImageId())) {
+      continue;
+    }
+
+    // Ignore duplicate images.
+    if (common_image_ids.count(image->ImageId()) > 0) {
+      continue;
+    }
+
+    common_image_ids.insert(image->ImageId());
+    src.push_back(image->ProjectionCenter());
+    dst.push_back(locations[i]);
+  }
+
+  // Only compute the alignment if there are enough correspondences.
+  if (common_image_ids.size() < static_cast<size_t>(min_common_images)) {
+    return false;
+  }
+
+  LORANSAC<SimilarityTransformEstimator<3, true>,
+           SimilarityTransformEstimator<3, true>>
+      ransac(ransac_options);
+
+  const auto report = ransac.Estimate(src, dst);
+
+  if (report.support.num_inliers < static_cast<size_t>(min_common_images)) {
+    return false;
+  }
+
+  SimilarityTransform3 transform = SimilarityTransform3(report.model);
+  Transform(transform);
+
+  if (tform != nullptr) {
+    *tform = transform;
+  }
+
+  return true;
+}
+
 }  // namespace colmap
diff --git a/src/colmap/base/reconstruction.h b/src/colmap/base/reconstruction.h
@@ -37,9 +37,7 @@
 #include "colmap/base/point2d.h"
 #include "colmap/base/point3d.h"
 #include "colmap/base/track.h"
-#include "colmap/estimators/similarity_transform.h"
 #include "colmap/geometry/similarity_transform.h"
-#include "colmap/optim/loransac.h"
 #include "colmap/util/types.h"
 
 #include <memory>
@@ -208,14 +206,12 @@ class Reconstruction {
   // Align the given reconstruction with a set of pre-defined camera positions.
   // Assuming that locations[i] gives the 3D coordinates of the center
   // of projection of the image with name image_names[i].
-  template <bool kEstimateScale = true>
   bool Align(const std::vector<std::string>& image_names,
              const std::vector<Eigen::Vector3d>& locations,
              int min_common_images,
              SimilarityTransform3* tform = nullptr);
 
   // Robust alignment using RANSAC.
-  template <bool kEstimateScale = true>
   bool AlignRobust(const std::vector<std::string>& image_names,
                    const std::vector<Eigen::Vector3d>& locations,
                    int min_common_images,
@@ -536,115 +532,4 @@ bool Reconstruction::IsImageRegistered(const image_t image_id) const {
   return Image(image_id).IsRegistered();
 }
 
-template <bool kEstimateScale>
-bool Reconstruction::Align(const std::vector<std::string>& image_names,
-                           const std::vector<Eigen::Vector3d>& locations,
-                           const int min_common_images,
-                           SimilarityTransform3* tform) {
-  CHECK_GE(min_common_images, 3);
-  CHECK_EQ(image_names.size(), 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 = FindImageWithName(image_names[i]);
-    if (image == nullptr) {
-      continue;
-    }
-
-    if (!IsImageRegistered(image->ImageId())) {
-      continue;
-    }
-
-    // Ignore duplicate images.
-    if (common_image_ids.count(image->ImageId()) > 0) {
-      continue;
-    }
-
-    common_image_ids.insert(image->ImageId());
-    src.push_back(image->ProjectionCenter());
-    dst.push_back(locations[i]);
-  }
-
-  // Only compute the alignment if there are enough correspondences.
-  if (common_image_ids.size() < static_cast<size_t>(min_common_images)) {
-    return false;
-  }
-
-  SimilarityTransform3 transform;
-  if (!transform.Estimate<kEstimateScale>(src, dst)) {
-    return false;
-  }
-
-  Transform(transform);
-
-  if (tform != nullptr) {
-    *tform = transform;
-  }
-
-  return true;
-}
-
-template <bool kEstimateScale>
-bool Reconstruction::AlignRobust(const std::vector<std::string>& image_names,
-                                 const std::vector<Eigen::Vector3d>& locations,
-                                 const int min_common_images,
-                                 const RANSACOptions& ransac_options,
-                                 SimilarityTransform3* tform) {
-  CHECK_GE(min_common_images, 3);
-  CHECK_EQ(image_names.size(), 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 = FindImageWithName(image_names[i]);
-    if (image == nullptr) {
-      continue;
-    }
-
-    if (!IsImageRegistered(image->ImageId())) {
-      continue;
-    }
-
-    // Ignore duplicate images.
-    if (common_image_ids.count(image->ImageId()) > 0) {
-      continue;
-    }
-
-    common_image_ids.insert(image->ImageId());
-    src.push_back(image->ProjectionCenter());
-    dst.push_back(locations[i]);
-  }
-
-  // Only compute the alignment if there are enough correspondences.
-  if (common_image_ids.size() < static_cast<size_t>(min_common_images)) {
-    return false;
-  }
-
-  LORANSAC<SimilarityTransformEstimator<3, kEstimateScale>,
-           SimilarityTransformEstimator<3, kEstimateScale>>
-      ransac(ransac_options);
-
-  const auto report = ransac.Estimate(src, dst);
-
-  if (report.support.num_inliers < static_cast<size_t>(min_common_images)) {
-    return false;
-  }
-
-  SimilarityTransform3 transform = SimilarityTransform3(report.model);
-  Transform(transform);
-
-  if (tform != nullptr) {
-    *tform = transform;
-  }
-
-  return true;
-}
-
 }  // namespace colmap
diff --git a/src/colmap/base/synthetic.cc b/src/colmap/base/synthetic.cc
@@ -33,6 +33,7 @@
 
 #include "colmap/geometry/essential_matrix.h"
 #include "colmap/geometry/pose.h"
+#include "colmap/geometry/projection.h"
 #include "colmap/math/random.h"
 
 #include <Eigen/Geometry>

diff --git a/src/colmap/estimators/absolute_pose_test.cc b/src/colmap/estimators/absolute_pose_test.cc
@@ -70,9 +70,7 @@ TEST(AbsolutePose, P3P) {
       // Project points to camera coordinate system.
       std::vector<Eigen::Vector2d> points2D;
       for (size_t i = 0; i < points3D.size(); ++i) {
-        Eigen::Vector3d point3D_camera = points3D[i];
-        orig_tform.TransformPoint(&point3D_camera);
-        points2D.push_back(point3D_camera.hnormalized());
+        points2D.push_back((orig_tform * points3D[i]).hnormalized());
       }
 
       RANSACOptions options;
@@ -132,9 +130,7 @@ TEST(AbsolutePose, EPNP) {
       // Project points to camera coordinate system.
       std::vector<Eigen::Vector2d> points2D;
       for (size_t i = 0; i < points3D.size(); ++i) {
-        Eigen::Vector3d point3D_camera = points3D[i];
-        orig_tform.TransformPoint(&point3D_camera);
-        points2D.push_back(point3D_camera.hnormalized());
+        points2D.push_back((orig_tform * points3D[i]).hnormalized());
       }
 
       RANSACOptions options;

diff --git a/src/colmap/estimators/coordinate_frame_test.cc b/src/colmap/estimators/coordinate_frame_test.cc
@@ -102,11 +102,11 @@ TEST(CoordinateFrame, AlignToPrincipalPlane) {
                 .norm(),
             1e-6);
   // Verify that transform matrix does shuffling of axes
-  Eigen::Matrix4d mat;
+  Eigen::Matrix3x4d mat;
   if (inverted) {
-    mat << 0, -1, 0, 0, 0, 0, -1, 0, 1, 0, 0, 0, 0, 0, 0, 1;
+    mat << 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, 0, 0, 0, 1;
+    mat << 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);

diff --git a/src/colmap/estimators/generalized_absolute_pose_test.cc b/src/colmap/estimators/generalized_absolute_pose_test.cc
@@ -94,11 +94,10 @@ TEST(GeneralizedAbsolutePose, Estimate) {
       // Project points to camera coordinate system.
       std::vector<GP3PEstimator::X_t> points2D;
       for (size_t i = 0; i < points3D.size(); ++i) {
-        Eigen::Vector3d point3D_camera = points3D[i];
-        orig_tforms[i % kNumTforms].TransformPoint(&point3D_camera);
         points2D.emplace_back();
         points2D.back().rel_tform = rel_tforms[i % kNumTforms];
-        points2D.back().xy = point3D_camera.hnormalized();
+        points2D.back().xy =
+            (orig_tforms[i % kNumTforms] * points3D[i]).hnormalized();
       }
 
       RANSACOptions options;

diff --git a/src/colmap/estimators/generalized_relative_pose_test.cc b/src/colmap/estimators/generalized_relative_pose_test.cc
@@ -92,9 +92,8 @@ TEST(GeneralizedRelativePose, Estimate) {
       for (size_t i = 0; i < points3D.size(); ++i) {
         const Eigen::Vector3d point3D_camera1 =
             rel_tforms[i % kNumTforms] * points3D[i].homogeneous();
-        Eigen::Vector3d point3D_camera2 = points3D[i];
-        orig_tforms[(i + 1) % kNumTforms].TransformPoint(&point3D_camera2);
-
+        const Eigen::Vector3d point3D_camera2 =
+            orig_tforms[(i + 1) % kNumTforms] * points3D[i];
         if (point3D_camera1.z() < 0 || point3D_camera2.z() < 0) {
           continue;
         }