Request to add Multiscale Generalized ICP (already works bealtifully!)

[RubensBenevides]

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).

Proposed new feature or change

Generalized ICP already works bealtifully, but in multiscale it can do amazing things, like global registration, and in some cases can register pairs that even the FGR (Fast Global Registration) can't. I implemented the multiscale GICP using the examples in Open3D for multiscale registration, but I'm not a expert in programming, I only work with point clouds because of my master degree in Geodetic Sciences (Photogrammetry and Remote Sensing, UFPR, Brasil).

With this implementation I was able to register some pairs of the dataset Arch from the work "Globally consistent registration of terrestrial laser scans via graph optimization. Theiler et al. (2015)". The dataset has only 5 clouds, but with a low overlap of 50% or less, and it has a lot of artefacts, vegetation, moving objects, etc. Along one year of work I tried all kinds of fine tuning in FGR to reconstruct that dataset, but never successful without manual registration. Because all point clouds overlap I tried all pairs, from the 10 possible FGR only worked on 2 or 3, but to reconstruct the dataset I need at least 4 correct transformations conecting all clouds.

The GICP Multiscale that I implemented managed to register some pairs of this dataset 2 of 10.

References

Here is my code:

""" Created on Tue Apr 19 15:33:56 2022 @author: Rubens Antonio Leite Benevides, [email protected] """

------------------------- MULTISCALE GICP -------------------------

import numpy as np np.set_printoptions(suppress=True) import copy import open3d as o3d

This function applies GICP at n scales with a decreasing number of iterations per scale. The voxels that define the scales are created linearly in the interval [distance,0] without including 0. The iterations also divide the space [iterations,0] linearly in a number of intervals equal to the number of scales chosen. INPUT: source, target = point cloud pair to be registered (open3d object). n_scales = how many scales to use in GICP (integer) distance = max correspondence distance, will be transformed in a descending vector along scales (float). iteracoes_max = maximum iterations on the coarsest scale, will be transformed in a descending vector along scales (integer). inicial_T = inicial pose to initialize GICP (4x4 matrix). OUTPUT: Open3D object containing the registration result.

def multiscale_GICP_registration(source, target, n_escales, distance, iterations_max, inicial_T): # vector of scales (voxel sizes) voxel_sizes = list(np.linspace(distance, distance/n_escales, n_escales)) # Vector of max_corresp_dist by scale max_correspondence_distances = list(np.linspace(distance, distance/n_escales, n_escales)*2.5) # Vector of iterations by scale (more iterations on coarse/firsts scales) iterations = list(np.linspace(iterations_max, iterations_max/n_escales, n_escales).astype(int)) # Parameter-free function to filter outliers loss = o3d.pipelines.registration.L1Loss() # Define the ICP type (GICP) ICP_mode = o3d.pipelines.registration.TransformationEstimationForGeneralizedICP(loss) # Aply the GICP on all scales for i in range(n_escales): # Downsample source = source.voxel_down_sample(voxel_sizes[i]) target = target.voxel_down_sample(voxel_sizes[i]) # Estimate normals on the current scale source.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(voxel_sizes[i]*2, max_nn=20)) target.estimate_normals(o3d.geometry.KDTreeSearchParamHybrid(voxel_sizes[i]*2, max_nn=20)) # Aply GICP on the current scale result_icp = o3d.pipelines.registration.registration_generalized_icp(source, target, max_correspondence_distances[i], inicial_T, ICP_mode, o3d.pipelines.registration.ICPConvergenceCriteria(relative_fitness=1e-6, relative_rmse=1e-6, max_iteration=iterations[i])) inicial_T = result_icp.transformation return result_icp

Import the dataset (5 clouds)

arch_1 = o3d.io.read_point_cloud("nuvens_amostradas_20_cm/originais/s0.pcd") arch_2 = o3d.io.read_point_cloud("nuvens_amostradas_20_cm/originais/s1.pcd") arch_3 = o3d.io.read_point_cloud("nuvens_amostradas_20_cm/originais/s2.pcd") arch_4 = o3d.io.read_point_cloud("nuvens_amostradas_20_cm/originais/s3.pcd") arch_5 = o3d.io.read_point_cloud("nuvens_amostradas_20_cm/originais/s4.pcd")

Show all clouds

o3d.visualization.draw_geometries([arch_1,arch_2,arch_3,arch_4,arch_5])

Define the paramters

inicial_T = np.eye(4) # no initial information n_escales = 10 # use 10 scales distance = 2 # meters iterations = 1000 # distribute 1000 iterations througth the 10 scales

Try to register some pairs

(1 -> 2)

resultado_12 = multiscale_GICP_registration(arch_1, arch_2, n_escales, distance, iterations, inicial_T)

(2 -> 3)

resultado_23 = multiscale_GICP_registration(arch_2, arch_3, n_escales, distance, iterations, inicial_T)

(3 -> 4)

resultado_34 = multiscale_GICP_registration(arch_3, arch_4, n_escales, distance, iterations, inicial_T)

(4 -> 5)

resultado_45 = multiscale_GICP_registration(arch_4, arch_5, n_escales, distance, iterations, inicial_T)

(5 -> 1)

resultado_51 = multiscale_GICP_registration(arch_5, arch_1, n_escales, distance, iterations, inicial_T)

Loop closure pairs

(1 -> 3)

resultado_13 = multiscale_GICP_registration(arch_1, arch_3, n_escales, distance, iterations, inicial_T)

(1 -> 4)

resultado_14 = multiscale_GICP_registration(arch_1, arch_4, n_escales, distance, iterations, inicial_T)

(2 -> 4)

resultado_24 = multiscale_GICP_registration(arch_2, arch_4, n_escales, distance, iterations, inicial_T)

(2 -> 5)

resultado_25 = multiscale_GICP_registration(arch_2, arch_5, n_escales, distance, iterations, inicial_T)

(3 -> 5)

resultado_35 = multiscale_GICP_registration(arch_3, arch_5, n_escales, distance, iterations, inicial_T)

Function to draw the registration result (clouds with random colors)

def draw_registration_result(source, target, transformation): source_temp = copy.deepcopy(source) target_temp = copy.deepcopy(target) source_temp.paint_uniform_color(np.random.rand(3)**2) target_temp.paint_uniform_color(np.random.rand(3)**2) source_temp.transform(transformation) o3d.visualization.draw_geometries([source_temp, target_temp])

Registration result of odometry pairs

draw_registration_result(arch_1, arch_2, resultado_12.transformation) draw_registration_result(arch_2, arch_3, resultado_23.transformation) draw_registration_result(arch_3, arch_4, resultado_34.transformation) draw_registration_result(arch_4, arch_5, resultado_45.transformation) draw_registration_result(arch_5, arch_1, resultado_51.transformation)

Registration result of loop closure pairs

draw_registration_result(arch_1, arch_3, resultado_13.transformation) draw_registration_result(arch_1, arch_4, resultado_14.transformation) draw_registration_result(arch_2, arch_4, resultado_24.transformation) draw_registration_result(arch_2, arch_5, resultado_25.transformation) draw_registration_result(arch_3, arch_5, resultado_35.transformation)

Additional information