Open3Dの使い方：簡単にICP

pythonで点群処理できるOpen3Dの探検．Open3Dの使い方：読み込みと表示，点と法線の取得の続き．

stanford bunnyの2つをICPで位置合わせしてみる．

コード

open3d

import sys
sys.path.append("../..") # Open3D/build/lib/ へのパス
import numpy as np
import py3d


pcd1 = py3d.read_point_cloud("bun000.ply")
pcd2 = py3d.read_point_cloud("bun045.ply")
py3d.draw_geometries([pcd1, pcd2], "bunny 000 and 045", 640, 480)

# paint_uniform_colorと同じだけど自分で色つけてみる
pcd1.colors = py3d.Vector3dVector(np.repeat(np.asarray([[1, 0, 0]]), len(np.asarray(pcd1.points)), axis=0))
pcd2.colors = py3d.Vector3dVector(np.repeat(np.asarray([[0, 0, 1]]), len(np.asarray(pcd2.points)), axis=0))

kdt = py3d.KDTreeSearchParamHybrid(radius=0.1, max_nn=30)
py3d.estimate_normals(pcd1, search_param=kdt)
py3d.estimate_normals(pcd2, search_param=kdt)

py3d.draw_geometries([pcd1, pcd2], "bunny 000 and 045", 640, 480)


th = 0.02
T = np.asarray([[1, 0, 0, 0],
                [0, 1, 0, 0],
                [0, 0, 1, 0],
                [0, 0, 0, 1]])

# 初期値の情報
info = py3d.evaluate_registration(pcd1, pcd2, 
                                  max_correspondence_distance=th, 
                                  transformation=T)
print("correspondences:", np.asarray(info.correspondence_set))
print("fitness: ", info.fitness)
print("RMSE: ", info.inlier_rmse)
print("transformation: ", info.transformation)

info = py3d.registration_icp(pcd1, pcd2, 
                             max_correspondence_distance=th, 
                             init=T,
                             estimation_method=py3d.TransformationEstimationPointToPoint()
                             # estimation_method=py3d.TransformationEstimationPointToPlane() # 法線が必要                            )
print("correspondences:", np.asarray(info.correspondence_set))
print("fitness: ", info.fitness)
print("RMSE: ", info.inlier_rmse)
print("transformation: ", info.transformation)

pcd1.transform(info.transformation)

py3d.draw_geometries([pcd1, pcd2], "bunny 000 and 045", 640, 480)

結果

初期値の情報

correspondences: [[   15  1320]
 [   16  1320]
 [   17  1320]
 ...
 [40252 38314]
 [40253 38314]
 [40254 38314]]
fitness:  0.6154113672496025
RMSE:  0.010350160221480371
transformation:  [[1. 0. 0. 0.]
 [0. 1. 0. 0.]
 [0. 0. 1. 0.]
 [0. 0. 0. 1.]]

ICP結果

correspondences: [[    0  1318]
 [    1  1319]
 [    2  1106]
 ...
 [40253 40074]
 [40254 40077]
 [40255 40088]]
fitness:  0.9910323926868044
RMSE:  0.0019479950359222648
transformation:  [[ 8.36685395e-01  1.45575710e-02 -5.47490299e-01  3.58575243e-02]
 [-1.87362765e-02  9.99822363e-01 -2.04822591e-03 -4.23147885e-04]
 [ 5.47363227e-01  1.19716503e-02  8.36809523e-01  3.82771389e-02]
 [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]