Unified cloud initializer pipeline for ICP (fixes segfault colored IC…

…P) (#6942)

* Unified cloud initializer pipeline for ICP (fixes segfault colored ICP)

* Update CHANGELOG

---------

Co-authored-by: Benjamin Ummenhofer <benjamin.ummenhofer@intel.com>

CHANGELOG.md

@@ -47,6 +47,7 @@
 -   Fix projection of point cloud to Depth/RGBD image if no position attribute is provided (PR #6880)
 -   Support lowercase types when reading PCD files (PR #6930)
 -   Fix visualization/draw ICP example and add warnings (PR #6933)
+-   Unified cloud initializer pipeline for ICP (fixes segfault colored ICP) (PR #6942)
 -   Fix tensor EstimatePointWiseNormalsWithFastEigen3x3 (PR #6980)
 -   Fix alpha shape reconstruction if alpha too small for point scale (PR #6998)
 -   Fix render to depth image on Apple Retina displays (PR #7001)

cpp/open3d/pipelines/registration/ColoredICP.cpp

@@ -211,15 +211,12 @@ double TransformationEstimationForColoredICP::ComputeRMSE(
     return residual;
 };
 
-RegistrationResult RegistrationColoredICP(
+std::tuple<std::shared_ptr<const geometry::PointCloud>,
+           std::shared_ptr<const geometry::PointCloud>>
+TransformationEstimationForColoredICP::InitializePointCloudsForTransformation(
         const geometry::PointCloud &source,
         const geometry::PointCloud &target,
-        double max_distance,
-        const Eigen::Matrix4d &init /* = Eigen::Matrix4d::Identity()*/,
-        const TransformationEstimationForColoredICP &estimation
-        /* = TransformationEstimationForColoredICP()*/,
-        const ICPConvergenceCriteria
-                &criteria /* = ICPConvergenceCriteria()*/) {
+        double max_correspondence_distance) const {
     if (!target.HasNormals()) {
         utility::LogError(
                 "ColoredICP requires target pointcloud to have normals.");
@@ -232,17 +229,34 @@ RegistrationResult RegistrationColoredICP(
         utility::LogError(
                 "ColoredICP requires source pointcloud to have colors.");
     }
-
-    if (auto target_c = InitializePointCloudForColoredICP(
-                target,
-                geometry::KDTreeSearchParamHybrid(max_distance * 2.0, 30))) {
-        return RegistrationICP(source, *target_c, max_distance, init,
-                               estimation, criteria);
-    } else {
+    std::shared_ptr<const geometry::PointCloud> source_initialized_c(
+            &source, [](const geometry::PointCloud *) {});
+    std::shared_ptr<geometry::PointCloud> target_initialized_c(
+            InitializePointCloudForColoredICP(
+                    target, geometry::KDTreeSearchParamHybrid(
+                                    max_correspondence_distance * 2.0, 30)));
+    if (!source_initialized_c || !target_initialized_c) {
         utility::LogError(
-                "Internal error: InitializePointCloudForColoredICP returns "
+                "Internal error: InitializePointCloudsForTransformation "
+                "returns "
                 "nullptr.");
-    };
+    }
+    return std::make_tuple(source_initialized_c,
+                           std::const_pointer_cast<const geometry::PointCloud>(
+                                   target_initialized_c));
+}
+
+RegistrationResult RegistrationColoredICP(
+        const geometry::PointCloud &source,
+        const geometry::PointCloud &target,
+        double max_distance,
+        const Eigen::Matrix4d &init /* = Eigen::Matrix4d::Identity()*/,
+        const TransformationEstimationForColoredICP &estimation
+        /* = TransformationEstimationForColoredICP()*/,
+        const ICPConvergenceCriteria
+                &criteria /* = ICPConvergenceCriteria()*/) {
+    return RegistrationICP(source, target, max_distance, init, estimation,
+                           criteria);
 }
 
 }  // namespace registration

cpp/open3d/pipelines/registration/ColoredICP.h

@@ -51,6 +51,13 @@ class TransformationEstimationForColoredICP : public TransformationEstimation {
             const geometry::PointCloud &target,
             const CorrespondenceSet &corres) const override;
 
+    std::tuple<std::shared_ptr<const geometry::PointCloud>,
+               std::shared_ptr<const geometry::PointCloud>>
+    InitializePointCloudsForTransformation(
+            const geometry::PointCloud &source,
+            const geometry::PointCloud &target,
+            double max_correspondence_distance) const override;
+
 public:
     double lambda_geometric_ = 0.968;
     /// shared_ptr to an Abstract RobustKernel that could mutate at runtime.

cpp/open3d/pipelines/registration/GeneralizedICP.cpp

@@ -147,6 +147,29 @@ TransformationEstimationForGeneralizedICP::ComputeTransformation(
     return is_success ? extrinsic : Eigen::Matrix4d::Identity();
 }
 
+std::tuple<std::shared_ptr<const geometry::PointCloud>,
+           std::shared_ptr<const geometry::PointCloud>>
+TransformationEstimationForGeneralizedICP::
+        InitializePointCloudsForTransformation(
+                const geometry::PointCloud &source,
+                const geometry::PointCloud &target,
+                double max_correspondence_distance) const {
+    std::shared_ptr<geometry::PointCloud> source_initialized_c =
+            InitializePointCloudForGeneralizedICP(source, epsilon_);
+    std::shared_ptr<geometry::PointCloud> target_initialized_c =
+            InitializePointCloudForGeneralizedICP(target, epsilon_);
+    if (!source_initialized_c || !target_initialized_c) {
+        utility::LogError(
+                "Internal error: InitializePointCloudsForTransformation "
+                "returns "
+                "nullptr.");
+    }
+    return std::make_pair(std::const_pointer_cast<const geometry::PointCloud>(
+                                  source_initialized_c),
+                          std::const_pointer_cast<const geometry::PointCloud>(
+                                  target_initialized_c));
+}
+
 RegistrationResult RegistrationGeneralizedICP(
         const geometry::PointCloud &source,
         const geometry::PointCloud &target,
@@ -156,10 +179,8 @@ RegistrationResult RegistrationGeneralizedICP(
                 &estimation /* = TransformationEstimationForGeneralizedICP()*/,
         const ICPConvergenceCriteria
                 &criteria /* = ICPConvergenceCriteria()*/) {
-    return RegistrationICP(
-            *InitializePointCloudForGeneralizedICP(source, estimation.epsilon_),
-            *InitializePointCloudForGeneralizedICP(target, estimation.epsilon_),
-            max_correspondence_distance, init, estimation, criteria);
+    return RegistrationICP(source, target, max_correspondence_distance, init,
+                           estimation, criteria);
 }
 
 }  // namespace registration

cpp/open3d/pipelines/registration/GeneralizedICP.h

@@ -51,6 +51,13 @@ class TransformationEstimationForGeneralizedICP
             const geometry::PointCloud &target,
             const CorrespondenceSet &corres) const override;
 
+    std::tuple<std::shared_ptr<const geometry::PointCloud>,
+               std::shared_ptr<const geometry::PointCloud>>
+    InitializePointCloudsForTransformation(
+            const geometry::PointCloud &source,
+            const geometry::PointCloud &target,
+            double max_correspondence_distance) const override;
+
 public:
     /// Small constant representing covariance along the normal.
     double epsilon_ = 1e-3;

cpp/open3d/pipelines/registration/Registration.cpp

@@ -117,45 +117,34 @@ RegistrationResult RegistrationICP(
     if (max_correspondence_distance <= 0.0) {
         utility::LogError("Invalid max_correspondence_distance.");
     }
-    if ((estimation.GetTransformationEstimationType() ==
-                 TransformationEstimationType::PointToPlane ||
-         estimation.GetTransformationEstimationType() ==
-                 TransformationEstimationType::ColoredICP) &&
-        (!target.HasNormals())) {
-        utility::LogError(
-                "TransformationEstimationPointToPlane and "
-                "TransformationEstimationColoredICP "
-                "require pre-computed normal vectors for target PointCloud.");
-    }
-    if ((estimation.GetTransformationEstimationType() ==
-         TransformationEstimationType::GeneralizedICP) &&
-        (!target.HasCovariances() || !source.HasCovariances())) {
-        utility::LogError(
-                "TransformationEstimationForGeneralizedICP require "
-                "pre-computed per point covariances matrices for source and "
-                "target PointCloud.");
-    }
+
+    auto [source_initialized_c, target_initialized_c] =
+            estimation.InitializePointCloudsForTransformation(
+                    source, target, max_correspondence_distance);
 
     Eigen::Matrix4d transformation = init;
     geometry::KDTreeFlann kdtree;
-    kdtree.SetGeometry(target);
-    geometry::PointCloud pcd = source;
+    const geometry::PointCloud &target_initialized = *target_initialized_c;
+    kdtree.SetGeometry(target_initialized);
+    geometry::PointCloud pcd(*source_initialized_c);
+
     if (!init.isIdentity()) {
         pcd.Transform(init);
     }
     RegistrationResult result;
     result = GetRegistrationResultAndCorrespondences(
-            pcd, target, kdtree, max_correspondence_distance, transformation);
+            pcd, target_initialized, kdtree, max_correspondence_distance,
+            transformation);
     for (int i = 0; i < criteria.max_iteration_; i++) {
         utility::LogDebug("ICP Iteration #{:d}: Fitness {:.4f}, RMSE {:.4f}", i,
                           result.fitness_, result.inlier_rmse_);
         Eigen::Matrix4d update = estimation.ComputeTransformation(
-                pcd, target, result.correspondence_set_);
+                pcd, target_initialized, result.correspondence_set_);
         transformation = update * transformation;
         pcd.Transform(update);
         RegistrationResult backup = result;
         result = GetRegistrationResultAndCorrespondences(
-                pcd, target, kdtree, max_correspondence_distance,
+                pcd, target_initialized, kdtree, max_correspondence_distance,
                 transformation);
         if (std::abs(backup.fitness_ - result.fitness_) <
                     criteria.relative_fitness_ &&

cpp/open3d/pipelines/registration/TransformationEstimation.cpp

@@ -11,6 +11,7 @@
 
 #include "open3d/geometry/PointCloud.h"
 #include "open3d/utility/Eigen.h"
+#include "open3d/utility/Logging.h"
 
 namespace open3d {
 namespace pipelines {
@@ -42,6 +43,25 @@ Eigen::Matrix4d TransformationEstimationPointToPoint::ComputeTransformation(
     return Eigen::umeyama(source_mat, target_mat, with_scaling_);
 }
 
+std::tuple<std::shared_ptr<const geometry::PointCloud>,
+           std::shared_ptr<const geometry::PointCloud>>
+TransformationEstimationPointToPoint::InitializePointCloudsForTransformation(
+        const geometry::PointCloud &source,
+        const geometry::PointCloud &target,
+        double max_correspondence_distance) const {
+    std::shared_ptr<const geometry::PointCloud> source_initialized_c(
+            &source, [](const geometry::PointCloud *) {});
+    std::shared_ptr<const geometry::PointCloud> target_initialized_c(
+            &target, [](const geometry::PointCloud *) {});
+    if (!source_initialized_c || !target_initialized_c) {
+        utility::LogError(
+                "Internal error: InitializePointCloudsForTransformation "
+                "returns "
+                "nullptr.");
+    }
+    return std::make_tuple(source_initialized_c, target_initialized_c);
+}
+
 double TransformationEstimationPointToPlane::ComputeRMSE(
         const geometry::PointCloud &source,
         const geometry::PointCloud &target,
@@ -89,6 +109,29 @@ Eigen::Matrix4d TransformationEstimationPointToPlane::ComputeTransformation(
     return is_success ? extrinsic : Eigen::Matrix4d::Identity();
 }
 
+std::tuple<std::shared_ptr<const geometry::PointCloud>,
+           std::shared_ptr<const geometry::PointCloud>>
+TransformationEstimationPointToPlane::InitializePointCloudsForTransformation(
+        const geometry::PointCloud &source,
+        const geometry::PointCloud &target,
+        double max_correspondence_distance) const {
+    if (!target.HasNormals()) {
+        utility::LogError(
+                "PointToPlaneICP requires target pointcloud to have normals.");
+    }
+    std::shared_ptr<const geometry::PointCloud> source_initialized_c(
+            &source, [](const geometry::PointCloud *) {});
+    std::shared_ptr<const geometry::PointCloud> target_initialized_c(
+            &target, [](const geometry::PointCloud *) {});
+    if (!source_initialized_c || !target_initialized_c) {
+        utility::LogError(
+                "Internal error: InitializePointCloudsForTransformation "
+                "returns "
+                "nullptr.");
+    }
+    return std::make_tuple(source_initialized_c, target_initialized_c);
+}
+
 }  // namespace registration
 }  // namespace pipelines
 }  // namespace open3d

cpp/open3d/pipelines/registration/TransformationEstimation.h

@@ -67,6 +67,19 @@ class TransformationEstimation {
             const geometry::PointCloud &source,
             const geometry::PointCloud &target,
             const CorrespondenceSet &corres) const = 0;
+
+    /// Initialize the source and target point cloud for the transformation
+    /// estimation.
+    ///
+    /// \param source Source point cloud.
+    /// \param target Target point cloud.
+    /// \param max_correspondence_distance Maximum correspondence distance.
+    virtual std::tuple<std::shared_ptr<const geometry::PointCloud>,
+                       std::shared_ptr<const geometry::PointCloud>>
+    InitializePointCloudsForTransformation(
+            const geometry::PointCloud &source,
+            const geometry::PointCloud &target,
+            double max_correspondence_distance) const = 0;
 };
 
 /// \class TransformationEstimationPointToPoint
@@ -95,6 +108,13 @@ class TransformationEstimationPointToPoint : public TransformationEstimation {
             const geometry::PointCloud &target,
             const CorrespondenceSet &corres) const override;
 
+    std::tuple<std::shared_ptr<const geometry::PointCloud>,
+               std::shared_ptr<const geometry::PointCloud>>
+    InitializePointCloudsForTransformation(
+            const geometry::PointCloud &source,
+            const geometry::PointCloud &target,
+            double max_correspondence_distance) const override;
+
 public:
     /// \brief Set to True to estimate scaling, False to force scaling to be 1.
     ///
@@ -137,6 +157,13 @@ class TransformationEstimationPointToPlane : public TransformationEstimation {
             const geometry::PointCloud &target,
             const CorrespondenceSet &corres) const override;
 
+    std::tuple<std::shared_ptr<const geometry::PointCloud>,
+               std::shared_ptr<const geometry::PointCloud>>
+    InitializePointCloudsForTransformation(
+            const geometry::PointCloud &source,
+            const geometry::PointCloud &target,
+            double max_correspondence_distance) const override;
+
 public:
     /// shared_ptr to an Abstract RobustKernel that could mutate at runtime.
     std::shared_ptr<RobustKernel> kernel_ = std::make_shared<L2Loss>();