Fast global registration leads to local minima sometimes

[prrw]

I have checked the release documentation and the latest documentation (using 0.16.0).

I am trying to get the pose of the object based on its CAD model using first Fast global registration to get an approximate transformation and using that for ICP point to plane to get exact transformation.

I tested it with 2 pointclouds sampled from PLY file (one is source and other is target). Applied a known transformation to one of them and tested with this approach.

It works normally when the the transformation which i apply is less than 90 degrees in each axis. See image: Source= Blue, Target = Red, After_Fast_Global = Yellow, After_ICP = Green

The moment i give a transformation of about 180 degrees along one axis (meaning i flip it), the solution is not optimal it looks like it enters a local minima. See image: Source= Blue, Target = Red, After_Fast_Global = Yellow, After_ICP = Green

My code with the transformation for the latter case:

import open3d as o3d
import copy
import time
import numpy as np
#-----------Load Point Clouds---------------#
time_start = time.time()
mesh = o3d.io.read_triangle_mesh("Geometrie-Platte.ply")
mesh.compute_vertex_normals()
trg = mesh.sample_points_uniformly(number_of_points=50000)
src = mesh.sample_points_uniformly(number_of_points=50000)
trans_init = np.asarray([[-0.4203031, -0.8275815,  0.3720943, 0.5],
                             [-0.9013429,  0.3335468, -0.2762741, 0.7],
                             [0.1045285, -0.4515035, -0.8861255, -1.4], [0.0, 0.0, 0.0, 1.0]])
trg.transform(trans_init)
print("Loading point clouds took %.3f sec." % (time.time() - time_start))
#-----------Voxelize---------------#
time_start = time.time()
src_down = src.voxel_down_sample(0.001)
trg_down = trg.voxel_down_sample(0.001)
print("Source point cloud downsampled to: ",len(src_down.points))
print("Target point cloud downsampled to: ",len(trg_down.points))
print("Voxelize took %.3f sec." % (time.time() - time_start))
#-----------Estimate Normals---------------#
time_start = time.time()
src_down.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=0.01, max_nn=50))
trg_down.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(radius=0.01, max_nn=50))
print("Estimate normals took %.3f sec." % (time.time() - time_start))
#-----------FPFH Feature---------------#
time_start = time.time()
src_fpfh = o3d.pipelines.registration.compute_fpfh_feature(src_down,
        o3d.geometry.KDTreeSearchParamHybrid(radius=0.05, max_nn=100))
trg_fpfh = o3d.pipelines.registration.compute_fpfh_feature(trg_down,
        o3d.geometry.KDTreeSearchParamHybrid(radius=0.05, max_nn=100))
print("Compute FPFH feature took %.3f sec." % (time.time() - time_start))
#-----------Fast Global Registration---------------#
time_start = time.time()
result = o3d.pipelines.registration.registration_fgr_based_on_feature_matching(
        src_down, trg_down, src_fpfh, trg_fpfh, o3d.pipelines.registration.FastGlobalRegistrationOption(
            maximum_correspondence_distance=0.1))
print("Fast global registration took %.3f sec." % (time.time() - time_start))
#--------------Results---------------------------#
print(result)
print(result.transformation)
src_fg = copy.deepcopy(src)
src_fg.transform(result.transformation)
#--------------Point to plane ICP-----------------------#
time_start = time.time()
reg_p2p = o3d.pipelines.registration.registration_icp(
        src, trg, 0.1, result.transformation,
        o3d.pipelines.registration.TransformationEstimationPointToPlane(), o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=100))
print("Point to plane ICP took %.3f sec." % (time.time() - time_start))
#--------------Results---------------------------#
print(reg_p2p)
print(reg_p2p.transformation)
src_icp = copy.deepcopy(src)
src_icp.transform(reg_p2p.transformation)
#--------------Visualization-----------------------#
src.paint_uniform_color([0, 0, 1])
trg.paint_uniform_color([1, 0, 0])
src_fg.paint_uniform_color([1, 1, 0])
src_icp.paint_uniform_color([0, 1, 0])
print("Colors representation: \nCAD = Blue, Target = Red, CAD_Global = Yellow, CAD_ICP = Green")
o3d.visualization.draw_geometries([src, trg, src_fg, src_icp])

Am i doing something wrong ? I need to correct this in global registration because ICP wont work if i do not get an approximate correct transform. In this case since global transform is false, ICP does not propose a better solution.

[halil-karabacak]

Hi,

to find a alignment Fast alignment method uses AdvancedMatching which uses random numbers. so, though FAST may "think" it found a suitable alignment, it simply fails. This is the reason why it works 9 out of 10.

Other side is why it works when you rotate it less than 180 degrees. Well, I guess by doing so you still provide at least small portion of intersection between previous and current point cloud, source and target. But, when you rotate it moer than 180 degree fast fails to align because now intersection is actually reverse faces of the object.

The solution may be an initial transformation. Since you already have the transformation angles, you can simply derotate the second scan to incerase the size of intersection.

Best, Halil

[prrw]

Hey thanks, i just found out that ICP algorithm has this problem which is why there exist hundreds of variants of ICP and i tried with RANSAC Global transformation instead of FAST and results were better but it is still not full proof. I think the initial transformation especially rotation for ICP needs to be accurate within 45 degrees in order to avoid local minimums and RANSAC global even if can achieve that, does not guarantee it.

[halil-karabacak]

Hi,

yes as you mentioned there are several types of ICP and also ICP required a good alignment because most of the time its loss functions depends on the distance between points. A few remarks may be helpful for you: 1- If your point cloud(camera) has RGB color scale, then you may take a look at ColoredICP. 2- If not, from my experience GeneralizedICP gives better result than normal ICP in most cases. Also you can implement it with multi-scale ICP if time is a consideration for you. 3- Lastly, ransac doesN'T restrict the angles between normals of the points. So, it may still fail; you may consider adding this restriction to ransac.

Lastly,

does not guarantee it.

unfortunately, none of the alignment methods does that. It is a quite big problem actually, because in case it fails most of the time inlier_rmse or fitness doesn't also indicate its failed alignment. I am still working on this problem to, at least, 90 percent confidence of detection of failed alignment.

I hope that helps.

Best, Halil

[prrw]

Hi Halil, Thank you for your remarks. For me its not a colored object as it is an industrial component. Yes, i alos switched back to PointToPointICP. Finally i did not get how to add restriction to RANSAC. Can you possibly give me an example ?

[halil-karabacak]

Hi, I don't think such an example exist unless I code it. But, let me give you some idea where you can add it on your ransac code.

In RegistrationRANSACBasedOnCorrespondence function, ransac iterates # of maxiteration. In each iteration it generates a random number, and takes this points as index and finds an estimation between source[index] and target[index] points. So, before passing these points into pose estimation, you can check if you really want to use the alignment between these two points by checking the normal between them.

Best

[prrw]

Okay, thanks. I think i got it.