isl-org / Open3D

Open3D: A Modern Library for 3D Data Processing
http://www.open3d.org
Other
11.35k stars 2.29k forks source link

Error in calculating error/loss for rmse for Point Cloud Registration with ICP Point2Point #5807

Closed Sheradil closed 1 year ago

Sheradil commented 1 year ago

Checklist

Describe the issue

I have been trying to run the tutorial of the point cloud registration for a point cloud of my own. It didn't work. I therefore created my own point cloud of 12 points, a rectangle of 3x4 points lying on the X-Y plane. I duplicated this cloud and moved it along the Z axis by 1.0 or by 0.1. For both (modified) point clouds I tried to do ICP and match them with the original cloud. The 'initial guess' transformation matrix is the identity matrix. Even though the clouds obviously do not match perfectly, the error that the RMSE gives is 0. But in both cases, the result of the ICP is the input transformation (so the identity matrix). So basically the ICP is doing nothing. (I tried some other things to address certain things that could go wrong (in my opinion), but nothing worked and this is a quite simple example. I probably did something stupid and just can't see the forest for the trees)

Steps to reproduce the bug

import numpy as np
import open3d as o3d
import copy

# Copied from the tutorial. To see the points the camera has to be rotated.
def draw_registration_result(source, target, transformation):
    source_temp = copy.deepcopy(source)
    target_temp = copy.deepcopy(target)
    source_temp.paint_uniform_color([1, 0.206, 0])
    target_temp.paint_uniform_color([0, 0.651, 0.929])
    print("Transformation: " + str(transformation))
    source_temp.transform(transformation)
    coord_frame = o3d.geometry.TriangleMesh.create_coordinate_frame()
    o3d.visualization.draw_geometries([source_temp, target_temp, coord_frame],
                                  zoom=0.5,
                                  front=[0.9288, -0.2951, -0.2242],
                                  lookat=[0, 1, 1],
                                  up=[0, 0, 1])

src_points = np.array([
    [0.0, 0.0, 0.0],
    [1.0, 0.0, 0.0],
    [2.0, 0.0, 0.0],
    [0.0, 1.0, 0.0],
    [1.0, 1.0, 0.0],
    [2.0, 1.0, 0.0],
    [0.0, 2.0, 0.0],
    [1.0, 2.0, 0.0],
    [2.0, 2.0, 0.0],
    [0.0, 3.0, 0.0],
    [1.0, 3.0, 0.0],
    [2.0, 3.0, 0.0],
])
tgt_points = np.array([
    [0.0, 0.0, 0.1], # Due to the 0.1 the clouds do not match perfectly
    [1.0, 0.0, 0.1],
    [2.0, 0.0, 0.1],
    [0.0, 1.0, 0.1],
    [1.0, 1.0, 0.1],
    [2.0, 1.0, 0.1],
    [0.0, 2.0, 0.1],
    [1.0, 2.0, 0.1],
    [2.0, 2.0, 0.1],
    [0.0, 3.0, 0.1],
    [1.0, 3.0, 0.1],
    [2.0, 3.0, 0.1],
]) 
o3d.utility.set_verbosity_level(o3d.utility.VerbosityLevel.Debug)
source = o3d.geometry.PointCloud()
source.points = o3d.utility.Vector3dVector(src_points)

target = o3d.geometry.PointCloud()
target.points = o3d.utility.Vector3dVector(tgt_points)

trans_init = np.asarray([[1.0, 0.0, 0.0, 0.0],
                         [0.0, 1.0, 0.0, 0.0],
                         [0.0, 0.0, 1.0, 0.0],
                         [0.0, 0.0, 0.0, 1.0]])

threshold = 0.02
reg_p2p = o3d.pipelines.registration.registration_icp(
    source, target, threshold, trans_init,
    o3d.pipelines.registration.TransformationEstimationPointToPoint())
print("Post Registration")
print("Inlier Fitness: ", reg_p2p.fitness) # prints 0.0
print("Inlier RMSE: ", reg_p2p.inlier_rmse) # prints 0.0
draw_registration_result(source, target, reg_p2p.transformation)

# Just run without parameters

Error message

No error message, but the result is obviously wrong.

Expected behavior

Output the correct transformation matrix.

Open3D, Python and System information

- Operating system: Ubuntu 20.04
- Python version: Python 3.9.12
- Open3D version: output from python: 0.15.2 AND 0.16.0 (tested both)
- System architecture: arm64
- Is this a remote workstation?: no
- How did you install Open3D?: pip

Additional information

No response

Sheradil commented 1 year ago

Ok, so the documentation is weird. I fixed it in the end. The documentation says: max_correspondence_distance (float) – Maximum correspondence points-pair distance. In their tutorial this parameter is called "threshold" and I would expect the algorithm to run until the error is less than the threshold. However, the algorithm doesn't start if the error is bigger than the threshold. This should be formulated more precisely in the documentation. And especially in the tutorial this has to be fixed.