I try to use the ICP algorithm of open3d to find a transformation that minimizes the distance between 2 point clouds and loosely followed their tutorial page: http://www.open3d.org/docs/latest/tutorial/pipelines/icp_registration.html (I use Ubuntu 20.04)
I tried to use point clouds from my ouster128, but it didn't work and therefore I decided to use 2 'dummy' point clouds that I create with numpy. The icp registration method gets a transformation as input and, in my case, always returns the input transformation (it basically does nothing, probably because the errors are 0). Here's the code (should be ready to use when copy pasted):
import numpy as np
import copy
import open3d as o3d
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)
print("Inlier RMSE: ", reg_p2p.inlier_rmse)
draw_registration_result(source, target, reg_p2p.transformation)
source and target are the same point clouds. Only difference is, that target is translated by 0.1 in z direction. The initial transformation is the identity matrix. The matrix that I would expect as output is the same as I, just with I[2][4]=0.1. Now, the fitness and inlier_rmse are 0. Which makes no sense (unless I completely misunderstood something) as that would mean that the clouds match perfectly, which they obviously don't do. Sometimes the fitness is not zero (for example, when source and target are the same clouds, except that 3 points of target are translated by 0.1).
What I tried before posting this thread:
(The visualization window is white and the camera has to be rotated to view the clouds) So, what am I doing wrong here? Thanks in advance.
Ok, I fixed it in the end. I'd blame the documentation in this case as it 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. If I use an threshold of 50 it works as expected.