Made it possible for the ICP methods (except p2plane) to be constrain…

…ed to a rotation around the z-axis

src/Open3D/Registration/FastGlobalRegistration.cpp

@@ -27,6 +27,7 @@
 #include "FastGlobalRegistration.h"
 
 #include <ctime>
+#include <iostream>
 
 #include <Open3D/Geometry/PointCloud.h>
 #include <Open3D/Geometry/KDTreeFlann.h>
@@ -357,6 +358,14 @@ RegistrationResult FastGlobalRegistration(
             NormalizePointCloud(point_cloud_vec, option);
     std::vector<std::pair<int, int>> corres;
     corres = AdvancedMatching(point_cloud_vec, features_vec, option);
+
+    if (option.with_constraint_) {
+      for (size_t i = 0; i < point_cloud_vec.size(); i++) {
+          for (size_t j = 0; j < point_cloud_vec[i].points_.size(); j++) {
+              point_cloud_vec[i].points_[j][2] = 0.0;
+          }
+      }
+    }
     Eigen::Matrix4d transformation;
     transformation = OptimizePairwiseRegistration(point_cloud_vec, corres,
                                                   scale_global, option);

src/Open3D/Registration/FastGlobalRegistration.h

@@ -45,14 +45,16 @@ class RegistrationResult;
 
 class FastGlobalRegistrationOption {
 public:
-    FastGlobalRegistrationOption(double division_factor = 1.4,
+    FastGlobalRegistrationOption(bool with_constraint = false,
+                                 double division_factor = 1.4,
                                  bool use_absolute_scale = false,
                                  bool decrease_mu = true,
                                  double maximum_correspondence_distance = 0.025,
                                  int iteration_number = 64,
                                  double tuple_scale = 0.95,
                                  int maximum_tuple_count = 1000)
-        : division_factor_(division_factor),
+        : with_constraint_(with_constraint),
+          division_factor_(division_factor),
           use_absolute_scale_(use_absolute_scale),
           decrease_mu_(decrease_mu),
           maximum_correspondence_distance_(maximum_correspondence_distance),
@@ -62,6 +64,7 @@ class FastGlobalRegistrationOption {
     ~FastGlobalRegistrationOption() {}
 
 public:
+    bool with_constraint_;
     // Division factor used for graduated non-convexity
     double division_factor_;
     // Measure distance in absolute scale (1) or in scale relative to the

src/Open3D/Registration/TransformationEstimation.cpp

@@ -26,9 +26,124 @@
 
 #include "TransformationEstimation.h"
 
+#include <Eigen/Core>
 #include <Eigen/Geometry>
+#include <Eigen/LU>
+#include <Eigen/SVD>
+#include <Eigen/Dense>
 #include <Open3D/Geometry/PointCloud.h>
 #include <Open3D/Utility/Eigen.h>
+#include <iostream>
+
+namespace Eigen { 
+
+#ifndef EIGEN_PARSED_BY_DOXYGEN
+
+    // These helpers are required since it allows to use mixed types as parameters
+    // for the Umeyama. The problem with mixed parameters is that the return type
+    // cannot trivially be deduced when float and double types are mixed.
+    namespace internal {
+
+        // Compile time return type deduction for different MatrixBase types.
+        // Different means here different alignment and parameters but the same underlying
+        // real scalar type.
+        template<typename MatrixType, typename OtherMatrixType>
+            struct umeyama_transform_matrix_type2
+            {
+                enum {
+                    MinRowsAtCompileTime = EIGEN_SIZE_MIN_PREFER_DYNAMIC(MatrixType::RowsAtCompileTime, OtherMatrixType::RowsAtCompileTime),
+
+                    // When possible we want to choose some small fixed size value since the result
+                    // is likely to fit on the stack. So here, EIGEN_SIZE_MIN_PREFER_DYNAMIC is not what we want.
+                    HomogeneousDimension = int(MinRowsAtCompileTime) == Dynamic ? Dynamic : int(MinRowsAtCompileTime)+1
+                };
+
+                typedef Matrix<typename traits<MatrixType>::Scalar,
+                        HomogeneousDimension,
+                        HomogeneousDimension,
+                        AutoAlign | (traits<MatrixType>::Flags & RowMajorBit ? RowMajor : ColMajor),
+                        HomogeneousDimension,
+                        HomogeneousDimension
+                            > type;
+            };
+
+    }
+
+#endif
+
+    // Similar to the original Eigen::Umeyama  https://eigen.tuxfamily.org/dox/Umeyama_8h_source.html
+    template <typename Derived, typename OtherDerived>
+    Eigen::Matrix4d umeyama_constrained(const Eigen::MatrixXd& _src, const Eigen::MatrixXd& _dst, bool with_scaling = true) {
+        typedef typename Eigen::internal::umeyama_transform_matrix_type2<Derived, OtherDerived>::type TransformationMatrixType;
+        typedef typename Eigen::internal::traits<TransformationMatrixType>::Scalar Scalar;
+        typedef typename Eigen::NumTraits<Scalar>::Real RealScalar;
+
+        enum { Dimension = EIGEN_SIZE_MIN_PREFER_DYNAMIC(Derived::RowsAtCompileTime, OtherDerived::RowsAtCompileTime) };
+
+        typedef Eigen::Matrix<Scalar, Dimension, 1> VectorType;
+        typedef Eigen::Matrix<Scalar, Dimension, Dimension> MatrixType;
+        typedef typename Eigen::internal::plain_matrix_type_row_major<Derived>::type RowMajorMatrixType;
+
+        const Index m = _src.rows(); // dimension
+        const Index n = _src.cols(); // number of measurements
+
+        Eigen::MatrixXd src(m, n);
+        src << _src.topRows(2), Eigen::MatrixXd::Zero(1, n);
+        Eigen::MatrixXd dst(m, n);
+        dst << _dst.topRows(2), Eigen::MatrixXd::Zero(1, n);
+
+        // required for demeaning ...
+        const RealScalar one_over_n = RealScalar(1) / static_cast<RealScalar>(n);
+
+        // computation of mean
+        const VectorType src_mean = src.rowwise().sum() * one_over_n;
+        const VectorType dst_mean = dst.rowwise().sum() * one_over_n;
+
+        // demeaning of src and dst points
+        const RowMajorMatrixType src_demean = src.colwise() - src_mean;
+        const RowMajorMatrixType dst_demean = dst.colwise() - dst_mean;
+
+        // Eq. (36)-(37)
+        const Scalar src_var = src_demean.rowwise().squaredNorm().sum() * one_over_n;
+
+        // Eq. (38)
+        const MatrixType sigma = one_over_n * dst_demean * src_demean.transpose();
+
+        Eigen::JacobiSVD<MatrixType> svd(sigma, Eigen::ComputeFullU | Eigen::ComputeFullV);
+
+        // Initialize the resulting transformation with an identity matrix...
+        TransformationMatrixType Rt = TransformationMatrixType::Identity(m+1,m+1);
+
+        // Eq. (39)
+        VectorType S = VectorType::Ones(m);
+
+        if  ( svd.matrixU().determinant() * svd.matrixV().determinant() < 0 ) {
+            // S(m-1) = -1;
+            Rt.col(m).head(m) = dst_mean-src_mean;
+            return Rt;
+        }
+
+        // Eq. (40) and (43)
+        Rt.block(0,0,m,m).noalias() = svd.matrixU() * S.asDiagonal() * svd.matrixV().transpose();
+
+        if (with_scaling)
+        {
+            // Eq. (42)
+            const Scalar c = Scalar(1)/src_var * svd.singularValues().dot(S);
+
+            // Eq. (41)
+            Rt.col(m).head(m) = dst_mean;
+            Rt.col(m).head(m).noalias() -= c*Rt.topLeftCorner(m,m)*src_mean;
+            Rt.block(0,0,m,m) *= c;
+        }
+        else
+        {
+            Rt.col(m).head(m) = dst_mean;
+            Rt.col(m).head(m).noalias() -= Rt.topLeftCorner(m,m)*src_mean;
+        }
+        return Rt;
+    }
+}
 
 namespace open3d {
 namespace registration {
@@ -56,7 +171,12 @@ Eigen::Matrix4d TransformationEstimationPointToPoint::ComputeTransformation(
         source_mat.block<3, 1>(0, i) = source.points_[corres[i][0]];
         target_mat.block<3, 1>(0, i) = target.points_[corres[i][1]];
     }
-    return Eigen::umeyama(source_mat, target_mat, with_scaling_);
+    if (with_constraint_) {
+        return Eigen::umeyama_constrained<Eigen::MatrixXd, Eigen::MatrixXd>(source_mat, target_mat, with_scaling_);
+    }
+    else {
+        return Eigen::umeyama(source_mat, target_mat, with_scaling_);
+    }
 }
 
 double TransformationEstimationPointToPlane::ComputeRMSE(

src/Open3D/Registration/TransformationEstimation.h

@@ -71,8 +71,8 @@ class TransformationEstimation {
 /// Estimate a transformation for point to point distance
 class TransformationEstimationPointToPoint : public TransformationEstimation {
 public:
-    TransformationEstimationPointToPoint(bool with_scaling = false)
-        : with_scaling_(with_scaling) {}
+    TransformationEstimationPointToPoint(bool with_constraint = false, bool with_scaling = false)
+        : with_constraint_(with_constraint), with_scaling_(with_scaling) {}
     ~TransformationEstimationPointToPoint() override {}
 
 public:
@@ -89,6 +89,7 @@ class TransformationEstimationPointToPoint : public TransformationEstimation {
             const CorrespondenceSet &corres) const override;
 
 public:
+    bool with_constraint_ = false;
     bool with_scaling_ = false;
 
 private:

src/Python/registration/registration.cpp

@@ -199,11 +199,11 @@ void pybind_registration_classes(py::module &m) {
                    "distance.");
     py::detail::bind_copy_functions<
             registration::TransformationEstimationPointToPoint>(te_p2p);
-    te_p2p.def(py::init([](bool with_scaling) {
+    te_p2p.def(py::init([](bool with_constraint, bool with_scaling) {
                    return new registration::
                            TransformationEstimationPointToPoint(with_scaling);
                }),
-               "with_scaling"_a = false)
+               "with_scaling"_a = false, "with_constraint"_a = false)
             .def("__repr__",
                  [](const registration::TransformationEstimationPointToPoint
                             &te) {
@@ -380,7 +380,7 @@ must hold true for all edges.)");
     py::detail::bind_copy_functions<registration::FastGlobalRegistrationOption>(
             fgr_option);
     fgr_option
-            .def(py::init([](double division_factor, bool use_absolute_scale,
+            .def(py::init([](bool with_constraint, double division_factor, bool use_absolute_scale,
                              bool decrease_mu,
                              double maximum_correspondence_distance,
                              int iteration_number, double tuple_scale,
@@ -390,7 +390,7 @@ must hold true for all edges.)");
                              maximum_correspondence_distance, iteration_number,
                              tuple_scale, maximum_tuple_count);
                  }),
-                 "division_factor"_a = 1.4, "use_absolute_scale"_a = false,
+                 "division_factor"_a = 1.4, "with_constraint"_a = false, "use_absolute_scale"_a = false,
                  "decrease_mu"_a = false,
                  "maximum_correspondence_distance"_a = 0.025,
                  "iteration_number"_a = 64, "tuple_scale"_a = 0.95,