isl-org / isl-org/Open3D

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

Open
#5,445 0 comments 4 reactions 0 assignees View on GitHub
feature request
Dominant language
C++
Stars
14k
Forks
2.6k
Avg merge
5d 18h
Merged PRs (30d)
6

Description

### Checklist

- [X] I have searched for [similar issues](https://github.com/isl-org/Open3D/issues).
- [X] For Python issues, I have tested with the [latest development wheel](http://www.open3d.org/docs/latest/getting_started.html#development-version-pip).
- [X] I have checked the [release documentation](http://www.open3d.org/docs/release/) and the [latest documentation](http://www.open3d.org/docs/latest/) (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, rubensleite11@gmail.com
"""

# ------------------------- 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

![analise_REGISTRO_GICP_MULTISCALE_10x](https://user-images.githubusercontent.com/52086242/185248534-a247bde2-6d4a-4574-8f69-81746266bee3.png)

Contributor guide

No contributing guide indexed for this repository

Assessment

This issue has not been assessed yet.

Get new issues in your inbox

A short digest of beginner-friendly GitHub issues.