Add CorrespondenceCheckerBasedOnRotation for orientation-constrained RANSAC

CHANGELOG.md

@@ -1,4 +1,5 @@
 ## Main
+-   Add `CorrespondenceCheckerBasedOnSourceRotation` to constrain global orientation priors in RANSAC registration (PR #7461)
 -   Upgrade stdgpu third-party library to commit d7c07d0.
 -   Fix performance for non-contiguous NumPy array conversion in pybind vector converters. This change removes restrictive `py::array::c_style` flags and adds a runtime contiguity check, improving Pandas-to-Open3D conversion speed by up to ~50×. (issue #5250)(PR #7343).
 -   Corrected documentation for Link Open3D in C++ projects (broken links).

cpp/open3d/pipelines/registration/CorrespondenceChecker.cpp

@@ -82,6 +82,30 @@ bool CorrespondenceCheckerBasedOnNormal::Check(
     return true;
 }
 
+bool CorrespondenceCheckerBasedOnSourceRotation::Check(
+        const geometry::PointCloud &source,
+        const geometry::PointCloud &target,
+        const CorrespondenceSet &corres,
+        const Eigen::Matrix4d &transformation) const {
+    Eigen::Vector6d transform_6d =
+            open3d::utility::TransformMatrix4dToVector6d(transformation);
+
+    if (rotation_threshold_[0] >= 0.0 &&
+        std::abs(transform_6d[0]) > rotation_threshold_[0]) {
+        return false;
+    }
+    if (rotation_threshold_[1] >= 0.0 &&
+        std::abs(transform_6d[1]) > rotation_threshold_[1]) {
+        return false;
+    }
+    if (rotation_threshold_[2] >= 0.0 &&
+        std::abs(transform_6d[2]) > rotation_threshold_[2]) {
+        return false;
+    }
+
+    return true;
+}
+
 }  // namespace registration
 }  // namespace pipelines
 }  // namespace open3d

cpp/open3d/pipelines/registration/CorrespondenceChecker.h

@@ -46,7 +46,7 @@ class CorrespondenceChecker {
     /// \param source Source point cloud.
     /// \param target Target point cloud.
     /// \param corres Correspondence set between source and target point cloud.
-    /// \param transformation The estimated transformation (inplace).
+    /// \param transformation The estimated transformation.
     virtual bool Check(const geometry::PointCloud &source,
                        const geometry::PointCloud &target,
                        const CorrespondenceSet &corres,
@@ -143,6 +143,50 @@ class CorrespondenceCheckerBasedOnNormal : public CorrespondenceChecker {
     double normal_angle_threshold_;
 };
 
+/// \class CorrespondenceCheckerBasedOnSourceRotation
+///
+/// \brief Class to limit the rotation of the source object.
+///
+/// It checks if the transformation is rotated too much from its initial, 
+/// unrotated state (identity matrix). 
+/// Rotations are checked by comparing the components of the angle-axis 
+/// representation (SO(3) log vector) of the estimated transformation 
+/// to the given thresholds. It is assumed that the user is aware of the 
+/// x, y, z axes of the source object when setting these tolerances. 
+class CorrespondenceCheckerBasedOnSourceRotation
+    : public CorrespondenceChecker {
+public:
+    /// \brief Parameterized Constructor.
+    ///
+    /// \param rotation_threshold specifies the threshold in radians within the
+    /// transformation. Rotations are checked by cartesian angles. If a rotation
+    /// threshold is set to < 0, it is not checked (free to rotate).
+    CorrespondenceCheckerBasedOnSourceRotation(
+            const Eigen::Vector3d &rotation_threshold = Eigen::Vector3d(-1,
+                                                                        -1,
+                                                                        -1))
+        : CorrespondenceChecker(false),
+          rotation_threshold_(rotation_threshold) {}
+    ~CorrespondenceCheckerBasedOnSourceRotation() override {}
+
+public:
+    /// \brief Function to check if two points can be aligned.
+    ///
+    /// \param source Source point cloud.
+    /// \param target Target point cloud.
+    /// \param corres Correspondence set between source and target point cloud.
+    /// \param transformation The estimated transformation.
+    bool Check(const geometry::PointCloud &source,
+               const geometry::PointCloud &target,
+               const CorrespondenceSet &corres,
+               const Eigen::Matrix4d &transformation) const override;
+
+public:
+    /// \brief 3-element vector [rx, ry, rz] representing the rotation angle 
+    /// thresholds in radians. A value < 0 means unconstrained.
+    Eigen::Vector3d rotation_threshold_;
+};
+
 }  // namespace registration
 }  // namespace pipelines
 }  // namespace open3d

cpp/pybind/pipelines/registration/registration.cpp

@@ -151,6 +151,19 @@ void pybind_registration_declarations(py::module &m) {
                  "normals. It considers vertex normal affinity of any "
                  "correspondences. It computes dot product of two normal "
                  "vectors. It takes radian value for the threshold.");
+    py::class_<
+            CorrespondenceCheckerBasedOnSourceRotation,
+            PyCorrespondenceChecker<CorrespondenceCheckerBasedOnSourceRotation>,
+            CorrespondenceChecker>
+            cc_r(m_registration, "CorrespondenceCheckerBasedOnSourceRotation",
+                 "Class to limit the rotation of the source object.\n"
+                 "It checks if the transformation is rotated too much from its "
+                 "initial, unrotated state (identity matrix).\n"
+                 "Rotations are checked by comparing the components of the "
+                 "angle-axis representation (SO(3) log vector) of the "
+                 "estimated transformation to the given thresholds. It is "
+                 "assumed that the user is aware of the x, y, z axes of the "
+                 "source object when setting these tolerances.");
     py::class_<FastGlobalRegistrationOption> fgr_option(
             m_registration, "FastGlobalRegistrationOption",
             "Options for FastGlobalRegistration.");
@@ -415,7 +428,7 @@ Sets :math:`c = 1` if ``with_scaling`` is ``False``.
              {"target", "Target point cloud."},
              {"corres",
               "Correspondence set between source and target point cloud."},
-             {"transformation", "The estimated transformation (inplace)."}});
+             {"transformation", "The estimated transformation."}});
 
     // open3d.registration.CorrespondenceCheckerBasedOnEdgeLength:
     // CorrespondenceChecker
@@ -504,6 +517,38 @@ must hold true for all edges.)");
                                    normal_angle_threshold_,
                            "Radian value for angle threshold.");
 
+    // open3d.registration.CorrespondenceCheckerBasedOnSourceRotation:
+    // CorrespondenceChecker
+    auto cc_r = static_cast<py::class_<
+            CorrespondenceCheckerBasedOnSourceRotation,
+            PyCorrespondenceChecker<CorrespondenceCheckerBasedOnSourceRotation>,
+            CorrespondenceChecker>>(
+            m_registration.attr("CorrespondenceCheckerBasedOnSourceRotation"));
+    py::detail::bind_copy_functions<CorrespondenceCheckerBasedOnSourceRotation>(
+            cc_r);
+    cc_r.def(py::init([](const Eigen::Vector3d &rotation_threshold) {
+                 return new CorrespondenceCheckerBasedOnSourceRotation(
+                         rotation_threshold);
+             }),
+             "rotation_threshold"_a)
+            .def("__repr__",
+                 [](const CorrespondenceCheckerBasedOnSourceRotation &c) {
+                     return fmt::format(
+                             ""
+                             "CorrespondenceCheckerBasedOnSourceRotation with "
+                             "rotation_threshold={:f}, {:f}, {:f} radians.",
+                             c.rotation_threshold_[0], c.rotation_threshold_[1],
+                             c.rotation_threshold_[2]);
+                 })
+            .def_readwrite("rotation_threshold",
+                           &CorrespondenceCheckerBasedOnSourceRotation::
+                                   rotation_threshold_,
+                           "Float64 numpy array of shape (3,) representing "
+                           "the maximum allowed thresholds [rx, ry, rz] "
+                           "in radians for the angle-axis representation components. "
+                           "It is assumed the user is aware of the x, y, z axes "
+                           "of the source object. A value < 0 means unconstrained.");
+
     // open3d.registration.FastGlobalRegistrationOption:
     auto fgr_option = static_cast<py::class_<FastGlobalRegistrationOption>>(
             m_registration.attr("FastGlobalRegistrationOption"));
@@ -614,7 +659,8 @@ must hold true for all edges.)");
                      "clouds can be aligned. One of "
                      "(``CorrespondenceCheckerBasedOnEdgeLength``, "
                      "``CorrespondenceCheckerBasedOnDistance``, "
-                     "``CorrespondenceCheckerBasedOnNormal``)"},
+                     "``CorrespondenceCheckerBasedOnNormal``, "
+                     "``CorrespondenceCheckerBasedOnSourceRotation``)"},
                     {"confidence",
                      "Desired probability of success for RANSAC. Used for "
                      "estimating early termination by k = log(1 - "