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

[nicolaloi]

Type

• Bug fix (non-breaking change which fixes an issue): Fixes Segmentation Fault for Colored Point Cloud Registration #6935
• New feature (non-breaking change which adds functionality). Resolves Segmentation Fault for Colored Point Cloud Registration #6935
• Breaking change (fix or feature that would cause existing functionality to not work as expected) Resolves #

Motivation and Context

The RegistrationICP function can be used to perform point-to-point ICP, point-to-plane ICP, generalized ICP, or colored ICP. Additionally, there are specific functions, RegistrationGeneralizedICP and RegistrationColoredICP, for computing generalized ICP and colored ICP only, respectively. These functions are simple wrappers around RegistrationICP: they initialize the point clouds for the specific ICP algorithm (e.g., computing color gradients for colored ICP) and then call the RegistrationICP function with the modified point clouds.

However, this point cloud initialization is skipped if you directly call the RegistrationICP function to compute generalized ICP or colored ICP because the initialization is only performed in the wrapper functions. While this is not a significant issue for generalized ICP, it causes a segmentation fault for colored ICP, as the target point cloud must be initialized as an instance of PointCloudForColoredICP to compute the additional color_gradients_ values.

Checklist:

• I have run python util/check_style.py --apply to apply Open3D code style
  to my code.
• This PR changes Open3D behavior or adds new functionality.
  • Both C++ (Doxygen) and Python (Sphinx / Google style) documentation is
    updated accordingly.
  • I have added or updated C++ and / or Python unit tests OR included test
    results (e.g. screenshots or numbers) here.
• I will follow up and update the code if CI fails.
• For fork PRs, I have selected Allow edits from maintainers.

Description

This PR introduces a unified point cloud initialization pipeline. A new virtual function, InitializePointCloudsForTransformation, is added to the TransformationEstimation class. This function is overridden in derived classes to handle the specific initialization required for each specific ICP algorithm, and is being called at the beginning of the RegistrationICP function.

This fixes the segmentation fault that occurs when calling RegistrationICP for colored ICP, because now the target point cloud will be correctly initialized with the color_gradient_ values. It also allows RegistrationICP to compute generalized ICP with point clouds that don't have pre-computed normals. Previously, this was only possible by calling the RegistrationGeneralizedICP wrapper, which exclusively handled the initialization of the point clouds.

With this change the point clouds are correctly and consistently initialized across the different ICP algorithms, regardless of whether the user directly calls RegistrationICP or a specialized wrapper function like RegistrationColoredICP.

Testing

import open3d as o3d
import numpy as np

cylinder = o3d.geometry.TriangleMesh.create_cylinder(radius=0.05, height=0.3, resolution=20, split=20)
source = cylinder.sample_points_uniformly(3000)
source.colors = o3d.utility.Vector3dVector([np.random.rand(3) for i in range(len(source.points))])
target = o3d.geometry.PointCloud(source)
target.transform(np.array([[1, 0, 0, 0.03], [0, 1, 0, 0.01], [0, 0, 1, 0.03], [0, 0, 0, 1]]))
target.estimate_normals()
o3d.visualization.draw_geometries([source, target])

print("Colored ICP using registration_colored_icp")
result_registration_colored_icp = o3d.pipelines.registration.registration_colored_icp(
        source, target, max_correspondence_distance=0.1, init=np.eye(4))
print(result_registration_colored_icp.transformation)
source_copy = o3d.geometry.PointCloud(source)
source_copy.transform(result_registration_colored_icp.transformation)
o3d.visualization.draw_geometries([source_copy, target])

print("\nColored ICP using registration_icp")
result_registration_icp = o3d.pipelines.registration.registration_icp(
        source, target, max_correspondence_distance=0.1, init=np.eye(4),
        estimation_method=o3d.pipelines.registration.TransformationEstimationForColoredICP())
print(result_registration_icp.transformation)
source_copy = o3d.geometry.PointCloud(source)
source_copy.transform(result_registration_icp.transformation)
o3d.visualization.draw_geometries([source_copy, target])

Before

Colored ICP using registration_colored_icp
[[ 1.00000000e+00 -1.11207724e-16  1.83706789e-16  3.00000000e-02]
 [ 1.14820460e-16  1.00000000e+00 -1.51820511e-16  1.00000000e-02]
 [-1.84268345e-16  1.54561401e-16  1.00000000e+00  3.00000000e-02]
 [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]

Colored ICP using registration_icp
Segmentation fault (core dumped)

After

Colored ICP using registration_colored_icp
[[ 1.00000000e+00 -2.48679083e-17 -1.71910316e-17  3.00000000e-02]
 [ 7.51766613e-18  1.00000000e+00  3.07426720e-18  1.00000000e-02]
 [ 1.81022819e-17 -4.90281364e-18  1.00000000e+00  3.00000000e-02]
 [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]

Colored ICP using registration_icp
[[ 1.00000000e+00 -2.55269579e-17 -1.41811654e-17  3.00000000e-02]
 [ 1.24054495e-17  1.00000000e+00  4.40279123e-18  1.00000000e-02]
 [ 2.22888068e-17 -3.62925246e-18  1.00000000e+00  3.00000000e-02]
 [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]

[update-docs]

Thanks for submitting this pull request! The maintainers of this repository would appreciate if you could update the CHANGELOG.md based on your changes.

[benjaminum]

Thanks for the PR @nicolaloi. Can you explain why the deleter is empty here?

[nicolaloi]

@benjaminum In some cases, like for PointToPoint and PointToPlane ICP, no actual modification/initialization of the point clouds is required. However, now there is a unified initialization interface across different ICP algorithms, so the initialization function (which is returning shared pointers to const point clouds) is being called in these cases too. Since in these cases the point clouds remain unchanged during the initialization step, creating a costly copy would be unnecessary (eg pc_initialized = make_shared(pc_original)). Instead, the shared pointers are created from the addresses of the original point clouds, effectively creating a const "view" of the originals. The empty deleter ensures that when these shared pointers go out of scope, they don't free the memory owned by the original point clouds.

[benjaminum]

OK, got it.

[benjaminum]

lgtm