PointToPlane ICP diverging. Bug or better tuning needed?

[chisarie]

Checklist

• [X] I have searched for similar issues.
• [X] For Python issues, I have tested with the latest development wheel.
• [X] I have checked the release documentation and the latest documentation (for master branch).

Describe the issue

I am trying to use an off the shelf ICP to match two simple point cloud. I have found that PointToPoint ICP always works, whereas PointToPlane ICP sometimes fails to return a reasonable result, even diverging a lot and bringing the two point clouds even further apart.

Is this a bug, or do I need to better tune the pipeline somehow? Please see these two attached point cloud as an example case:

• file1
• file2

Steps to reproduce the bug

import copy
import numpy as np
import open3d as o3d

pred_view_pcd = o3d.io.read_point_cloud("pred_view_pcd.pcd")
masked_pcd = o3d.io.read_point_cloud("masked_pcd.pcd")
# pred_view_pcd.estimate_normals()
# pred_view_pcd.orient_normals_towards_camera_location()

o3d.visualization.draw_geometries([pred_view_pcd, masked_pcd])  # type: ignore

p2l = o3d.pipelines.registration.TransformationEstimationPointToPlane()
reg_p2l = o3d.pipelines.registration.registration_icp(
    source=pred_view_pcd,
    target=masked_pcd,
    max_correspondence_distance=0.3,
    estimation_method=p2l,
)

print(reg_p2l.transformation)

corrected_shape = copy.deepcopy(pred_view_pcd).transform(reg_p2l.transformation)
if np.linalg.norm(corrected_shape.get_center() - masked_pcd.get_center()) > 0.3:
    print("WARNING: icp did not converge!")

o3d.visualization.draw_geometries([pred_view_pcd, masked_pcd, corrected_shape])  # type: ignore

Error message

No response

Expected behavior

The two point cloud should be matched, but instead they diverge

Open3D, Python and System information

- Operating system: Ubuntu 20.04
- Python version: tested both Python 3.8 and 3.9
- Open3D version: output from python: tested both open3d 0.17.0 and latest from today
- System architecture: x86
- Is this a remote workstation?: no
- How did you install Open3D?: pip inside a conda environment

Additional information

No response

[hernot]

To our experience point2plane is more sensitive to the proper selection of correspondence distance when there is a significant positional gap along normal direction of surfaces. This is cause surface normals might force it to align similar oriented and shaped surface parts which are not corresponding. Swapping source and target and inverting transformation obtained by first can be an option to also force target points to be aligned to source surface and not just points of source surface to target surface. Picking which ever results in the smaller rmse (forward/swapped) might yield the more reasonable result. Needs to be tested.

[chisarie]

I actually tested swapping source and target already, in both cases it fails unfortunately

[hernot]

Just to catchup where you actually are. Why is the pred_view_pcd.estimate_normals line commented out in your above example (*)? Did you try it with this line enabled already? - It should not do any harm for Point2Point ICP and Point2Plane requires them on target surface and when doing simple symmetric ICP you need them on source too. Otherwise it will fail. In other words the two are not fully interchangeable.

(*) The pred_view_pcd.orient_normals_towards_camera_location mainly ensures that all normals are pointing to the same side. of surface it does not make them all straight towards camera. Camera i guess at 0,-1,0 or at 0,0,-1 just guessing.

Do you have any possibility to get a triangulated version of point clouds. In our project we decided to export Textured TriangleMeshes (.obj) from our 3D camera system. Is more work to cleanup pre triangulated grid before storing but has the advantage that the resulting TriangleMesh is oriented and thus triangle_normals and vertex_normals are correct. No need for estimating.

[chisarie]

Yes I already tried with normal estimated also for the source, it did not have an effect. It's commented out just to remember myself that I already tried it. I could have deleted it. The camera location is at (0, 0, 0), which is the default value of orient_normals_towards_camera_location.

I have not tried triangulating the point cloud, but I assume it will be slower, right? For my application I need something that runs real-time, so a slowdown in processing is not ideal.

Did you have the chance to download the two files attached and run the script? To me it seems a pretty easy matching problem, it's just a few centimeters translation. That's why I thought there must be something off

[hernot]

First of all i guess the reason why you did see no difference is that Point2Plain implicitly calls the normal estimation when no normals are on the point cloud. but that normals are insufficient in any case. So if you have any chance to get from your recording system a triangulated surface instead of a point cloud than your normals are also proper and not just guesses.

In case the acquisition system does not provide textured mesh or you have no possibility to extend it to provide one than i do fear you are stuck with Point2Point. Worse than ignoring surface normals is having improper ones and be it just that they are not normalized (*).

Have you visualized your point clouds and can you tell where the camera is and in which direction it is pointing on each individual surface. Maybe you are lucky and you can find an approximate point differing from 0,0,0 which is common to all of your surfaces and ensures that all your normals are properly outward pointing.

In the end in our project we resorted to having the acquisition system generate an counter clockwise oriented pretriangulated grid covering the whole depth image and on storing deleting all triangles which touch pixels without depth, where the normals flip to clock wise orientation when mapped on texture using uvs coordinates or shrinking to less than one texture pixel and triangles which are very skinny, means their longest two edges are significantly longer than the edges of most of the triangles in their neighborhood.

Further we let the surfaces only be shifted by 1/3 at max from each other and only rotated by 20 deg max with respect to each other.

Futhrer can you post a visualization of your point clouds before and after ICP. would help to judge.

Did you try to reduce the max_correspondence distance. Eg. half or even less. Are the two point clouds closely overlapping or is there a gap in between perpendicular to the surfaces.

In other words a visual images showing both before and and after would help. Do not forget to also visualize the normals. In worst you have to gnerate a line set containing a line for each normal vector starting at the point and extending 0.02 or less in direction of normal vector.

(*) you can check that easily by

assert np.allclose((np.asarray(pointcloud.normals)*np.asarray(pointcloud.normals)).sum(axis=1),1.0)

[chisarie]

Hi, thank you for the detailed feedback. I think instead of me posting screenshots here, the best way for you to visualize the pointclouds is to run the script in my first post. In case you are not able to access the google drive folder I shared at the beginning, here is another link where you can download the files (expires in 7 days): https://www.sendbig.com/view-files/?Id=2339a179-d570-4229-a5c8-724a810bfd11

Please let me know if you are able to download the files and run the script

[hernot]

Hi, thank you for the detailed feedback. I think instead of me posting screenshots here, the best way for you to visualize the point clouds

Au contrair it is. And if it is just to figure ones own misconceptions on has to do this by his own.

As you are indicating i do have some questions concerning your script. What is the purpose of using ICP. Is it to stitch several surface shifted and rotated relative to each other to form an overall surface. Or is it to align two point clouds from two distinct devices representing the same object from the same view, with only small rotational and tranlational differences. In that case what is the average distance between two closest neighbors within each point cloud. is it 10 cm 1cm 1mm or less assuming that your max_correspondence_distance value represents 30cm. Try to reduce it to at max 10 times the average neighbor distance and run the above script again. I would expect that the misfit becomes visibly smaller.

The issue here is normals are normalized and thus have a strong impact on error measure (root mean square error - rmse). Thus when your max_correspondence_distance is too large, 30cm seems very large for me, an accidential match between normals of far apart points can cause the ICP optimizer to be distracted to wrong solution. If, that is why i asked you to visualize it, that large distance is needed to ensure ICP finds corresponding points at all, I would suggest the following to get both closer to each other. From that also point2point ICP would benefit as not beeing a hit by chance any more:

initial_shift = np.eye(4)
initial_shift[:3,3] = np.asarray(masked_pcd.points).mean(axis=0) - np.asarray(pred_view.points).mean(axis=0)

reg_p2l = o3d.pipelines.registration.registration_icp( 
     source=pred_view_pcd,
    target=masked_pcd,
    max_correspondence_distance=0.05,
    estimation_method=p2l,
     init = initial_shift
)

Initial shift can thereby be any transformation matrix including the inverse of a previous result as suggested above when saying to take the inverse transformation from the forward run swap source and target.

[chisarie]

Hello, thank you again for the detailed information. In fact, the tip of using an initial_shift made it work! Thank you very much. I will also keep in mind the hint regarding the max_correspondence_distance, even though here it seems not to make a big difference.

To respond quickly to your questions:

And if it is just to figure ones own misconceptions on has to do this by his own.

Yes of course I visualized the pointcloud myself already, I am looking at these pointclouds everyday the whole day. I just meant that running the script yourself is better than me posting a screenshot because there you can rotate and zoom the view. A screenshot is just static. Anyway, if you are curious, here it is:

What is the purpose of using ICP.

I have one pointcloud from an rgbd sensor and another one predicted by a neural network. Since the pose estimation of my network is not perfect, I want to refine the output via ICP.

For future reference, here is the script that works:

import numpy as np
import open3d as o3d

pred_view_pcd = o3d.io.read_point_cloud("pred_view_pcd.pcd")
masked_pcd = o3d.io.read_point_cloud("masked_pcd.pcd")

o3d.visualization.draw_geometries([pred_view_pcd, masked_pcd])  # type: ignore

initial_shift = np.eye(4)
initial_shift[:3, 3] = masked_pcd.get_center() - pred_view_pcd.get_center()

p2l = o3d.pipelines.registration.TransformationEstimationPointToPlane()
reg_p2l = o3d.pipelines.registration.registration_icp(
    source=pred_view_pcd,
    target=masked_pcd,
    max_correspondence_distance=0.05,
    estimation_method=p2l,
    init=initial_shift,
)

print(reg_p2l.transformation)

corrected_shape = copy.deepcopy(pred_view_pcd).transform(reg_p2l.transformation)
if np.linalg.norm(corrected_shape.get_center() - masked_pcd.get_center()) > 0.3:
    print("WARNING: icp did not converge!")

o3d.visualization.draw_geometries([masked_pcd, corrected_shape])  # type: ignore

Thank you again for the help!