Update model_aligner functionality (colmap#1177)

* New functionality in model_aligner

* Remove unused constructor

* Address PR comments

* Remove duplicate comments

* Fix unit test to work on all platforms

* Update similarity_transform.cc

Co-authored-by: Antonios Matakos <anmatako@dss.microsoft.us>
Co-authored-by: Antonios Matakos <anmatako@microsoft.com>
Co-authored-by: Johannes Schönberger <jsch@demuc.de>

src/base/similarity_transform.cc

@@ -37,6 +37,8 @@
 #include "estimators/similarity_transform.h"
 #include "optim/loransac.h"
 
+#include <fstream>
+
 namespace colmap {
 namespace {
 
@@ -194,6 +196,14 @@ SimilarityTransform3::SimilarityTransform3(const double scale,
   transform_.matrix() = matrix;
 }
 
+void SimilarityTransform3::Write(const std::string& path) {
+  std::ofstream file(path, std::ios::trunc);
+  CHECK(file.is_open()) << path;
+  // Ensure that we don't loose any precision by storing in text.
+  file.precision(17);
+  file << transform_.matrix() << std::endl;
+}
+
 SimilarityTransform3 SimilarityTransform3::Inverse() const {
   return SimilarityTransform3(transform_.inverse());
 }

src/base/similarity_transform.h

@@ -61,6 +61,8 @@ class SimilarityTransform3 {
   SimilarityTransform3(const double scale, const Eigen::Vector4d& qvec,
                        const Eigen::Vector3d& tvec);
 
+  void Write(const std::string& path);
+
   template <bool kEstimateScale = true>
   bool Estimate(const std::vector<Eigen::Vector3d>& src,
                 const std::vector<Eigen::Vector3d>& dst);

src/estimators/coordinate_frame.cc

@@ -31,6 +31,7 @@
 
 #include "estimators/coordinate_frame.h"
 
+#include "base/gps.h"
 #include "base/line.h"
 #include "base/pose.h"
 #include "base/undistortion.h"
@@ -298,4 +299,58 @@ Eigen::Matrix3d EstimateManhattanWorldFrame(
   return frame;
 }
 
+void AlignToPrincipalPlane(Reconstruction* recon, SimilarityTransform3* tform) {
+  // Perform SVD on the 3D points to estimate the ground plane basis
+  const Eigen::Vector3d centroid = recon->ComputeCentroid(0.0, 1.0);
+  Eigen::MatrixXd points(3, recon->NumPoints3D());
+  int pidx = 0;
+  for (const auto& point : recon->Points3D()) {
+    points.col(pidx++) = point.second.XYZ() - centroid;
+  }
+  const Eigen::Matrix3d basis =
+      points.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).matrixU();
+  Eigen::Matrix3d rot_mat;
+  rot_mat << basis.col(0), basis.col(1), basis.col(0).cross(basis.col(1));
+  rot_mat.transposeInPlace();
+
+  *tform = SimilarityTransform3(1.0, RotationMatrixToQuaternion(rot_mat),
+                                -rot_mat * centroid);
+
+  // if camera plane ends up below ground then flip basis vectors and create new
+  // transform
+  Image test_img = recon->Images().begin()->second;
+  tform->TransformPose(&test_img.Qvec(), &test_img.Tvec());
+  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 = SimilarityTransform3(1.0, RotationMatrixToQuaternion(rot_mat),
+                                  -rot_mat * centroid);
+  }
+
+  recon->Transform(*tform);
+}
+
+void AlignToENUPlane(Reconstruction* recon, SimilarityTransform3* tform,
+                     bool unscaled) {
+  const Eigen::Vector3d centroid = recon->ComputeCentroid(0.0, 1.0);
+  GPSTransform gps_tform;
+  const Eigen::Vector3d ell_centroid = gps_tform.XYZToEll({centroid}).at(0);
+
+  // Create rotation matrix from ECEF to ENU coordinates
+  const double sin_lat = sin(DegToRad(ell_centroid(0)));
+  const double sin_lon = sin(DegToRad(ell_centroid(1)));
+  const double cos_lat = cos(DegToRad(ell_centroid(0)));
+  const double cos_lon = cos(DegToRad(ell_centroid(1)));
+
+  // Create ECEF to ENU rotation matrix
+  Eigen::Matrix3d rot_mat;
+  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 = SimilarityTransform3(scale, RotationMatrixToQuaternion(rot_mat),
+                                -(scale * rot_mat) * centroid);
+  recon->Transform(*tform);
+}
+
 }  // namespace colmap

src/estimators/coordinate_frame.h

@@ -70,6 +70,19 @@ Eigen::Matrix3d EstimateManhattanWorldFrame(
     const ManhattanWorldFrameEstimationOptions& options,
     const Reconstruction& reconstruction, const std::string& image_path);
 
+// Aligns the reconstruction to the plane defined by running PCA on the 3D
+// points. The model centroid is at the origin of the new coordinate system
+// and the X axis is the first principal component with the Y axis being the
+// second principal component
+void AlignToPrincipalPlane(Reconstruction* recon, SimilarityTransform3* tform);
+
+// Aligns the reconstruction to the local ENU plane orientation. Rotates the
+// reconstruction such that the x-y plane aligns with the ENU tangent plane at
+// the point cloud centroid and translates the origin to the centroid.
+// If unscaled == true, then the original scale of the model remains unchanged.
+void AlignToENUPlane(Reconstruction* recon, SimilarityTransform3* tform,
+                     bool unscaled);
+
 }  // namespace colmap
 
 #endif  // COLMAP_SRC_ESTIMATORS_COORDINATE_AXES_H_

src/estimators/coordinate_frame_test.cc

@@ -33,6 +33,7 @@
 #include "util/testing.h"
 
 #include "estimators/coordinate_frame.h"
+#include "base/gps.h"
 
 using namespace colmap;
 
@@ -50,3 +51,107 @@ BOOST_AUTO_TEST_CASE(TestEstimateManhattanWorldFrame) {
                                   reconstruction, image_path),
       Eigen::Matrix3d::Zero());
 }
+
+BOOST_AUTO_TEST_CASE(TestAlignToPrincipalPlane) {
+  // Start with reconstruction containing points on the Y-Z plane and cameras
+  // "above" the plane on the positive X axis. After alignment the points should
+  // be on the X-Y plane and the cameras "above" the plane on the positive Z
+  // axis.
+  SimilarityTransform3 tform;
+  Reconstruction reconstruction;
+  // Setup image with projection center at (1, 0, 0)
+  Image image;
+  image.SetImageId(1);
+  image.Qvec() = Eigen::Vector4d(1.0, 0.0, 0.0, 0.0);
+  image.Tvec() = Eigen::Vector3d(-1.0, 0.0, 0.0);
+  reconstruction.AddImage(image);
+  // Setup 4 points on the Y-Z plane
+  point3D_t p1 =
+      reconstruction.AddPoint3D(Eigen::Vector3d(0.0, -1.0, 0.0), Track());
+  point3D_t p2 =
+      reconstruction.AddPoint3D(Eigen::Vector3d(0.0, 1.0, 0.0), Track());
+  point3D_t p3 =
+      reconstruction.AddPoint3D(Eigen::Vector3d(0.0, 0.0, -1.0), Track());
+  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;
+
+  // Verify that points lie on the correct locations of the X-Y plane
+  BOOST_CHECK_LE((reconstruction.Point3D(p1).XYZ() -
+                  Eigen::Vector3d(inverted ? 1.0 : -1.0, 0.0, 0.0))
+                     .norm(),
+                 1e-6);
+  BOOST_CHECK_LE((reconstruction.Point3D(p2).XYZ() -
+                  Eigen::Vector3d(inverted ? -1.0 : 1.0, 0.0, 0.0))
+                     .norm(),
+                 1e-6);
+  BOOST_CHECK_LE((reconstruction.Point3D(p3).XYZ() -
+                  Eigen::Vector3d(0.0, inverted ? 1.0 : -1.0, 0.0))
+                     .norm(),
+                 1e-6);
+  BOOST_CHECK_LE((reconstruction.Point3D(p4).XYZ() -
+                  Eigen::Vector3d(0.0, inverted ? -1.0 : 1.0, 0.0))
+                     .norm(),
+                 1e-6);
+  // Verify that projection center is at (0, 0, 1)
+  BOOST_CHECK_LE((reconstruction.Image(1).ProjectionCenter() -
+                  Eigen::Vector3d(0.0, 0.0, 1.0))
+                     .norm(),
+                 1e-6);
+  // Verify that transform matrix does shuffling of axes
+  Eigen::Matrix4d mat;
+  if (inverted) {
+    mat << 0, -1, 0, 0, 0, 0, -1, 0, 1, 0, 0, 0, 0, 0, 0, 1;
+  } else {
+    mat << 0, 1, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 1;
+  }
+  std::cout << tform.Matrix() << std::endl;
+  BOOST_CHECK_LE((tform.Matrix() - mat).norm(), 1e-6);
+}
+
+BOOST_AUTO_TEST_CASE(TestAlignToENUPlane) {
+  // Create reconstruction with 4 points with known LLA coordinates. After the
+  // ENU transform all 4 points should land approximately on the X-Y plane.
+  GPSTransform gps;
+  auto points = gps.EllToXYZ(
+      {Eigen::Vector3d(50, 10.1, 100), Eigen::Vector3d(50.1, 10, 100),
+       Eigen::Vector3d(50.1, 10.1, 100), Eigen::Vector3d(50, 10, 100)});
+  SimilarityTransform3 tform;
+  Reconstruction reconstruction;
+  std::vector<point3D_t> point_ids;
+  for (size_t i = 0; i < points.size(); ++i) {
+    point_ids.push_back(reconstruction.AddPoint3D(points[i], Track()));
+    std::cout << points[i].transpose() << std::endl;
+  }
+  AlignToENUPlane(&reconstruction, &tform, false);
+  // Verify final locations of points
+  BOOST_CHECK_LE((reconstruction.Point3D(point_ids[0]).XYZ() -
+                  Eigen::Vector3d(3584.8565215, -5561.5336506, 0.0742643))
+                     .norm(),
+                 1e-6);
+  BOOST_CHECK_LE((reconstruction.Point3D(point_ids[1]).XYZ() -
+                  Eigen::Vector3d(-3577.3888622, 5561.6397107, 0.0783761))
+                     .norm(),
+                 1e-6);
+  BOOST_CHECK_LE((reconstruction.Point3D(point_ids[2]).XYZ() -
+                  Eigen::Vector3d(3577.4152111, 5561.6397283, 0.0783613))
+                     .norm(),
+                 1e-6);
+  BOOST_CHECK_LE((reconstruction.Point3D(point_ids[3]).XYZ() -
+                  Eigen::Vector3d(-3584.8301178, -5561.5336683, 0.0742791))
+                     .norm(),
+                 1e-6);
+
+  // Verify that straight line distance between points is preserved
+  for (size_t i = 1; i < points.size(); ++i) {
+    const double dist_orig = (points[i] - points[i - 1]).norm();
+    const double dist_tform = (reconstruction.Point3D(point_ids[i]).XYZ() -
+                               reconstruction.Point3D(point_ids[i - 1]).XYZ())
+                                  .norm();
+    BOOST_CHECK_LE(std::abs(dist_orig - dist_tform), 1e-6);
+  }
+
+}