Deprecate SimilarityTransform3 and switch to colmap::Sim3d

cmake/FindDependencies.cmake

@@ -56,7 +56,7 @@ message(STATUS "Configuring PoseLib... done")
 # COLMAP
 FetchContent_Declare(COLMAP
     GIT_REPOSITORY    https://github.com/colmap/colmap.git
-    GIT_TAG           63b2cc000de32dc697f45ef1576dec7e67abddbc 
+    GIT_TAG           c9097b031ee00da22609c7ac4b3f6b45b4178de2
     EXCLUDE_FROM_ALL
 )
 message(STATUS "Configuring COLMAP...")

limap/base/CMakeLists.txt

@@ -4,7 +4,6 @@ LIMAP_ADD_SOURCES(
     graph.h graph.cc
     camera.h camera.cc
     camera_models.h
-    transforms.h transforms.cc
     camera_view.h camera_view.cc
     image_collection.h image_collection.cc
     pose.h pose.cc

limap/base/align.py

@@ -60,7 +60,8 @@ def align_imagecols_umeyama(imagecols_src, imagecols_dst):
     xyz_src = np.array(imagecols_src.get_locations()).transpose()
     xyz_dst = np.array(imagecols_dst.get_locations()).transpose()
     r, t, c = umeyama_alignment(xyz_src, xyz_dst, with_scale=True)
-    transform = _base.SimilarityTransform3(r, t, c)
+    matrix = np.concatenate([c * r, t[:, None]], 1)
+    transform = _base.pycolmap.Sim3d(matrix)
     imagecols_aligned = imagecols_src.apply_similarity_transform(transform)
     return transform, imagecols_aligned
 
@@ -142,7 +143,8 @@ def read_trans(fname):
     scale = np.linalg.norm(transform[:, 0])
     R = transform[:3, :3] / scale
     t = transform[:3, 3]
-    transform = _base.SimilarityTransform3(R, t, scale)
+    matrix = np.concatenate([scale * R, t[:, None]], 1)
+    transform = _base.pycolmap.Sim3d(matrix)
     imagecols_aligned = imagecols_src.apply_similarity_transform(transform)
 
     # delete tmp folder

limap/base/bindings.cc

@@ -9,7 +9,6 @@ namespace py = pybind11;
 
 #include "limap/util/kd_tree.h"
 #include "limap/util/types.h"
-#include <colmap/util/threading.h>
 
 #include "limap/base/camera.h"
 #include "limap/base/camera_view.h"
@@ -22,6 +21,11 @@ namespace py = pybind11;
 #include "limap/base/linetrack.h"
 #include "limap/base/pointtrack.h"
 
+#include <colmap/geometry/rigid3.h>
+#include <colmap/geometry/sim3.h>
+#include <colmap/util/threading.h>
+#include <third-party/pycolmap/helpers.h>
+
 namespace limap {
 
 void bind_general_structures(py::module &m) {
@@ -125,18 +129,6 @@ void bind_graph(py::module &m) {
   m.def("count_track_edges", &CountTrackEdges);
 }
 
-void bind_transforms(py::module &m) {
-  py::class_<SimilarityTransform3>(m, "SimilarityTransform3")
-      .def(py::init<>())
-      .def(py::init<V4D, V3D, double>(), py::arg("qvec"), py::arg("tvec"),
-           py::arg("scale") = 1.0)
-      .def(py::init<M3D, V3D, double>(), py::arg("R"), py::arg("T"),
-           py::arg("scale") = 1.0)
-      .def("R", &SimilarityTransform3::R)
-      .def("T", &SimilarityTransform3::T)
-      .def("s", &SimilarityTransform3::s);
-}
-
 void bind_linebase(py::module &m) {
   py::class_<Line2d>(m, "Line2d", "A finite 2D line (segment).")
       .def(py::init<>(), R"(
@@ -1292,7 +1284,7 @@ void bind_camera(py::module &m) {
             Apply similarity transform to all image poses.
 
             Args:
-                transform (:class:`limap.base.SimilarityTransform3`)
+                transform (:class:`limap.base.pycolmap.Sim3d`)
         )",
            py::arg("transform"))
       .def("get_first_image_id_by_camera_id",
@@ -1322,8 +1314,6 @@ void bind_camera(py::module &m) {
             Returns:
                 bool: True if all camera models are undistorted.
         )");
-
-  m.def("pose_similarity_transform", &pose_similarity_transform);
 }
 
 void bind_pointtrack(py::module &m) {
@@ -1362,18 +1352,133 @@ void bind_pointtrack(py::module &m) {
       .def("count_images", &PointTrack::count_images);
 }
 
+void bind_base_pycolmap(py::module &m_parent) {
+  using namespace colmap;
+  m_parent.def("get_effective_num_threads", &colmap::GetEffectiveNumThreads);
+  py::module_ m = m_parent.def_submodule("pycolmap");
+
+  // The following copied from pycolmap
+  py::class_<Eigen::Quaterniond> PyRotation3d(m, "Rotation3d");
+  PyRotation3d.def(py::init([]() { return Eigen::Quaterniond::Identity(); }))
+      .def(py::init<const Eigen::Vector4d &>(), "xyzw"_a,
+           "Quaternion in [x,y,z,w] format.")
+      .def(py::init<const Eigen::Matrix3d &>(), "matrix"_a,
+           "3x3 rotation matrix.")
+      .def(py::init([](const Eigen::Vector3d &vec) {
+             return Eigen::Quaterniond(
+                 Eigen::AngleAxis<double>(vec.norm(), vec.normalized()));
+           }),
+           "axis_angle"_a, "Axis-angle 3D vector.")
+      .def_property(
+          "quat", py::overload_cast<>(&Eigen::Quaterniond::coeffs),
+          [](Eigen::Quaterniond &self, const Eigen::Vector4d &quat) {
+            self.coeffs() = quat;
+          },
+          "Quaternion in [x,y,z,w] format.")
+      .def(py::self * Eigen::Quaterniond())
+      .def(py::self * Eigen::Vector3d())
+      .def("__mul__",
+           [](const Eigen::Quaterniond &self,
+              const py::EigenDRef<const Eigen::MatrixX3d> &points)
+               -> Eigen::MatrixX3d {
+             return points * self.toRotationMatrix().transpose();
+           })
+      .def("normalize", &Eigen::Quaterniond::normalize)
+      .def("matrix", &Eigen::Quaterniond::toRotationMatrix)
+      .def("norm", &Eigen::Quaterniond::norm)
+      .def("angle",
+           [](const Eigen::Quaterniond &self) {
+             return Eigen::AngleAxis<double>(self).angle();
+           })
+      .def(
+          "angle_to",
+          [](const Eigen::Quaterniond &self, const Eigen::Quaterniond &other) {
+            return self.angularDistance(other);
+          },
+          "other"_a)
+      .def("inverse", &Eigen::Quaterniond::inverse)
+      .def("__repr__", [](const Eigen::Quaterniond &self) {
+        std::ostringstream ss;
+        ss << "Rotation3d(xyzw=[" << self.coeffs().format(vec_fmt) << "])";
+        return ss.str();
+      });
+  py::implicitly_convertible<py::array, Eigen::Quaterniond>();
+  MakeDataclass(PyRotation3d);
+
+  py::class_<colmap::Rigid3d> PyRigid3d(m, "Rigid3d");
+  PyRigid3d.def(py::init<>())
+      .def(py::init<const Eigen::Quaterniond &, const Eigen::Vector3d &>(),
+           "rotation"_a, "translation"_a)
+      .def(py::init(&Rigid3d::FromMatrix), "matrix"_a,
+           "3x4 transformation matrix.")
+      .def_readwrite("rotation", &Rigid3d::rotation)
+      .def_readwrite("translation", &Rigid3d::translation)
+      .def("matrix", &Rigid3d::ToMatrix)
+      .def("adjoint", &Rigid3d::Adjoint)
+      .def(py::self * Rigid3d())
+      .def(py::self * Eigen::Vector3d())
+      .def("__mul__",
+           [](const Rigid3d &t,
+              const py::EigenDRef<const Eigen::MatrixX3d> &points)
+               -> Eigen::MatrixX3d {
+             return (points * t.rotation.toRotationMatrix().transpose())
+                        .rowwise() +
+                    t.translation.transpose();
+           })
+      .def("inverse", static_cast<Rigid3d (*)(const Rigid3d &)>(&Inverse))
+      .def("get_covariance_for_inverse",
+           static_cast<Eigen::Matrix6d (*)(const Rigid3d &,
+                                           const Eigen::Matrix6d &)>(
+               &GetCovarianceForRigid3dInverse),
+           py::arg("covar"))
+      .def_static("interpolate", &InterpolateCameraPoses, "cam_from_world1"_a,
+                  "cam_from_world2"_a, "t"_a);
+  py::implicitly_convertible<py::array, Rigid3d>();
+  MakeDataclass(PyRigid3d);
+
+  py::class_<colmap::Sim3d> PySim3d(m, "Sim3d");
+  PySim3d.def(py::init<>())
+      .def(py::init<double, const Eigen::Quaterniond &,
+                    const Eigen::Vector3d &>(),
+           "scale"_a, "rotation"_a, "translation"_a)
+      .def(py::init(&Sim3d::FromMatrix), "matrix"_a,
+           "3x4 transformation matrix.")
+      .def_property(
+          "scale",
+          [](Sim3d &self) {
+            return py::array({}, {}, &self.scale, py::cast(self));
+          },
+          [](Sim3d &self, double scale) { self.scale = scale; })
+      .def_readwrite("rotation", &Sim3d::rotation)
+      .def_readwrite("translation", &Sim3d::translation)
+      .def("matrix", &Sim3d::ToMatrix)
+      .def(py::self * Sim3d())
+      .def(py::self * Eigen::Vector3d())
+      .def("__mul__",
+           [](const Sim3d &t,
+              const py::EigenDRef<const Eigen::MatrixX3d> &points)
+               -> Eigen::MatrixX3d {
+             return (t.scale *
+                     (points * t.rotation.toRotationMatrix().transpose()))
+                        .rowwise() +
+                    t.translation.transpose();
+           })
+      .def("transform_camera_world", &TransformCameraWorld, "cam_from_world"_a)
+      .def("inverse", static_cast<Sim3d (*)(const Sim3d &)>(&Inverse));
+  py::implicitly_convertible<py::array, Sim3d>();
+  MakeDataclass(PySim3d);
+}
+
 void bind_base(py::module &m) {
   bind_general_structures(m);
   bind_graph(m);
-  bind_transforms(m);
   bind_pointtrack(m);
   bind_camera(m);
   bind_linebase(m);
   bind_linetrack(m);
   bind_line_dists(m);
   bind_line_linker(m);
-
-  m.def("get_effective_num_threads", &colmap::GetEffectiveNumThreads);
+  bind_base_pycolmap(m);
 }
 
 } // namespace limap

limap/base/image_collection.cc

@@ -461,10 +461,15 @@ double *ImageCollection::tvec_data(const int img_id) {
 }
 
 ImageCollection ImageCollection::apply_similarity_transform(
-    const SimilarityTransform3 &transform) const {
+    const colmap::Sim3d &transform) const {
   ImageCollection imagecols = ImageCollection(cameras, images);
   for (auto it = imagecols.images.begin(); it != imagecols.images.end(); ++it) {
-    it->second.pose = pose_similarity_transform(it->second.pose, transform);
+    // TODO: use colmap::TransformCameraWorld after we switched pose to
+    // colmap::Rigid3d
+    CameraPose &pose = it->second.pose;
+    M3D new_R = pose.R() * transform.rotation.toRotationMatrix().transpose();
+    V3D new_T = transform.scale * pose.T() - new_R * transform.translation;
+    it->second.pose = CameraPose(new_R, new_T);
   }
   return imagecols;
 }

limap/base/image_collection.h

@@ -11,12 +11,13 @@
 
 namespace py = pybind11;
 
+#include <colmap/geometry/sim3.h>
+
 #include "limap/_limap/helpers.h"
 #include "limap/util/types.h"
 
 #include "limap/base/camera.h"
 #include "limap/base/camera_view.h"
-#include "limap/base/transforms.h"
 
 namespace limap {
 
@@ -93,7 +94,7 @@ class ImageCollection {
   double *tvec_data(const int img_id);
 
   ImageCollection
-  apply_similarity_transform(const SimilarityTransform3 &transform) const;
+  apply_similarity_transform(const colmap::Sim3d &transform) const;
 
   // inverse indexing
   int get_first_image_id_by_camera_id(const int cam_id) const;