Global Regstration Local Refinement

[bkev13]

I am following the tutorial for global registration using a custom point cloud and everything went well until the local refinement step. I get this error message:

result = o3d.pipelines.registration.registration_icp(

RuntimeError: [Open3D Error] (class open3d::pipelines::registration::RegistrationResult __cdecl open3d::pipelines::registration::RegistrationICP(const class open3d::geometry::PointCloud &,const class open3d::geometry::PointCloud &,double,const class Eigen::Matrix<double,4,4,0,4,4> &,const class open3d::pipelines::registration::TransformationEstimation &,const class open3d::pipelines::registration::ICPConvergenceCriteria &)) D:\a\Open3D\Open3D\cpp\open3d\pipelines\registration\Registration.cpp:128: TransformationEstimationPointToPlane and TransformationEstimationColoredICP require pre-computed normal vectors for target PointCloud.

Here is my script:

%%

''' LIBRARIES '''

import numpy as np
import open3d as o3d

import matplotlib.pyplot as plt

import os

import time

import re

import copy

tic_start = time.time()

toc0 = 1000 * (time.time() - tic_start)
print("Library Computation took {:.0f} [ms]".format(toc0))

%%

tic1 = time.time()

'''Parameters'''

voxel_size = 0.07
radius_normal = voxel_size * 2
radius_feature = voxel_size * 5
radius = 0.2
n_max_nn = 30 # maximum number of nearest neighbors for normal estimation
f_max_nn = 100 # maximum number of nearest neighbors for feature estimation
max_correspondence_distance = 0.2

toc1 = 1000 * (time.time() - tic1)
print("Parameters Computation took {:.0f} [ms]".format(toc1))

%%

tic2 = time.time()

'''Draw registration result'''

front = [ 0.94034343769500039, 0.33760247309706132, 0.042175696113961887 ]
lookat = [ -0.0088697734162195285, 0.027093871415519999, -0.032977855769833142 ]
up = [ -0.043025960325164418, -0.0049672007989513276, 0.9990616065360135 ]
zoom = 0.3000000000000017

def draw_registration_result(source, target, transformation):
source_temp = copy.deepcopy(source)
target_temp = copy.deepcopy(target)
source_temp.paint_uniform_color([1, 0.706, 0])
target_temp.paint_uniform_color([0, 0.651, 0.929])
source_temp.transform(transformation)
o3d.visualization.draw_geometries([source_temp, target_temp]) # visualize the source and target point clouds

toc2 = 1000 * (time.time() - tic2)
print("Draw Registration Computation took {:.0f} [ms]".format(toc2))

%%

tic3 = time.time()

'''Preprocess point cloud'''

def preprocess_point_cloud(pcd, voxel_size):
print(":: Downsample with a voxel size %.3f." % voxel_size)
pcd_down = pcd.voxel_down_sample(voxel_size)

print(":: Estimate normal with search radius %.3f." % radius_normal)
pcd_down.estimate_normals(
    o3d.geometry.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=n_max_nn))

print(":: Compute FPFH feature with search radius %.3f." % radius_feature)
pcd_fpfh = o3d.pipelines.registration.compute_fpfh_feature(
    pcd_down,
    o3d.geometry.KDTreeSearchParamHybrid(radius=radius_feature, max_nn=f_max_nn))
return pcd_down, pcd_fpfh

toc3 = 1000 * (time.time() - tic3)
print("Preprocesse Point Cloud Computation took {:.0f} [ms]".format(toc3))

%%

tic4 = time.time()

'''Prepare Dataset'''

def prepare_dataset(voxel_size):

# demo_icp_pcds = o3d.data.DemoICPPointClouds()
# source = o3d.io.read_point_cloud(demo_icp_pcds.paths[0])
# target = o3d.io.read_point_cloud(demo_icp_pcds.paths[1])
# trans_init = np.asarray([[0.0, 0.0, 1.0, 0.0], [1.0, 0.0, 0.0, 0.0],
#                          [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]])
# source.transform(trans_init)
# draw_registration_result(source, target, np.identity(4))



source = o3d.io.read_point_cloud("appartment_cloud.ply")
target = o3d.io.read_point_cloud("appartment_cloud.ply")
trans_init = np.asarray([[0.0, 0.0, 1.0, 0.0], 
                         [1.0, 0.0, 0.0, 0.0],
                        [0.0, 1.0, 0.0, 0.0], 
                        [0.0, 0.0, 0.0, 1.0]])

trans_initt = np.asarray([[0.0, -1.0, 0.0, 0.0],
                         [1.0, 0.0, 0.0, 0.0],
                         [0.0, 0.0, 1, 0.0],
                         [0.0, 0.0, 0.0, 1.0]])
source.transform(trans_init)
target.transform(trans_initt)
# target.estimate_normals(
#     o3d.geometry.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=n_max_nn))
draw_registration_result(source, target, np.identity(4))

print('source: ', source)
print('target: ', target)

source_down, source_fpfh = preprocess_point_cloud(source, voxel_size)
target_down, target_fpfh = preprocess_point_cloud(target, voxel_size)

return source, target, source_down, target_down, source_fpfh, target_fpfh

toc4 = 1000 * (time.time() - tic4)
print("Prepare Dataset Computation took {:.0f} [ms]".format(toc4))

%%

'''Input Data'''

tic5 = time.time()

source, target, source_down, target_down, source_fpfh, target_fpfh = prepare_dataset(voxel_size)

toc5 = 1000 * (time.time() - tic5)
print("Input Data Computation took {:.0f} [ms]".format(toc5))

%%

tic6 = time.time()

'''Registration'''

def execute_global_registration(source_down, target_down,
source_fpfh, target_fpfh, voxel_size):
distance_threshold = voxel_size * 1.5
print(":: RANSAC registration on downsampled point clouds.")
print(" Since the downsampling voxel size is %.3f," % voxel_size)
print(" we use a liberal threshold %.3f." % distance_threshold)
result = o3d.pipelines.registration.registration_ransac_based_on_feature_matching(
source_down, target_down, source_fpfh, target_fpfh, True,
distance_threshold,
o3d.pipelines.registration.TransformationEstimationPointToPoint(False), 3,
[o3d.pipelines.registration.CorrespondenceCheckerBasedOnEdgeLength(0.9),
o3d.pipelines.registration.CorrespondenceCheckerBasedOnDistance(distance_threshold)],
o3d.pipelines.registration.RANSACConvergenceCriteria(100000, 0.999))

return result

toc6 = 1000 * (time.time() - tic6)
print("Registration Computation took {:.0f} [ms]".format(toc6))

%%

tic7 = time.time()

'''Visualise Result'''

result_ransac = execute_global_registration(source_down, target_down,
source_fpfh, target_fpfh,
voxel_size)
print(result_ransac)
draw_registration_result(source_down, target_down, result_ransac.transformation)

toc7 = 1000 * (time.time() - tic7)
print("Visualise Result Computation took {:.0f} [ms]".format(toc7))
print("Total Computation took {:.0f} [ms]".format(1000 * (time.time() - tic_start)))

%%

tic8 = time.time()

'''Refinement'''

def refine_registration(source, target, source_fpfh, target_fpfh, voxel_size):
distance_threshold = voxel_size * 0.4
print(":: Point-to-plane ICP registration is applied on original point")
print(" clouds to refine the alignment. This time we use a strict")
print(" distance threshold %.3f." % distance_threshold)

result = o3d.pipelines.registration.registration_icp(
    source, target, distance_threshold, result_ransac.transformation,
    o3d.pipelines.registration.TransformationEstimationPointToPlane())
return result

toc8 = 1000 * (time.time() - tic8)
print("Refinement Computation took {:.0f} [ms]".format(toc8))

%%

tic9 = time.time()

'''Visualise Refinement'''

result_ransac = execute_global_registration(source_down, target_down,
source_fpfh, target_fpfh,
voxel_size)
result_icp = refine_registration(source, target, source_fpfh, target_fpfh, voxel_size)

toc9 = 1000 * (time.time() - tic9)
print("Visualise Refinement Computation took {:.0f} [ms]".format(toc9))
print("Total Computation took {:.0f} [ms]".format(1000 * (time.time() - tic9)))

print(result_icp)
draw_registration_result(source, target, result_icp.transformation)

Here is the link to the to the dataset: https://drive.google.com/drive/folders/1nLy3zKByIfbkZo3vrinfe_fCfIA1ljXt

I am running the script on python 3.9.4, open3d version 0.18 on VSCode