Open3Dの使い方：ICPの収束の様子を1ステップ毎に見る

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

stanford bunnyの2つをICPで位置合わせする様子を，1反復毎に表示してみる．

コード

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

pcd1.paint_uniform_color([1, 0, 0])
pcd2.paint_uniform_color([0, 0, 1])

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


th = 0.02

criteria = py3d.ICPConvergenceCriteria(relative_fitness = 1e-6, # fitnessの変化分がこれより小さくなったら収束
                                       relative_rmse = 1e-6, # RMSEの変化分がこれより小さくなったら収束
                                       max_iteration = 1) # 反復1回だけにする
est_method = py3d.TransformationEstimationPointToPoint()

for i in range(30):
    info = py3d.registration_icp(pcd1, pcd2, 
                                 max_correspondence_distance=th, 
                                 # init=T, # デフォルトで単位行列
                                 estimation_method=est_method,
                                 criteria=criteria
                                )
    print("iteration {0:02d} fitness {1:.6f} RMSE {2:.6f}".format(i, info.fitness, info.inlier_rmse))
    
    pcd1.transform(info.transformation)
    
    py3d.draw_geometries([pcd1, pcd2], "iteration {}".format(i), 640, 480)

結果

結果

iteration 00 fitness 0.659380 RMSE 0.010144
iteration 01 fitness 0.716738 RMSE 0.010305
iteration 02 fitness 0.785870 RMSE 0.010074
iteration 03 fitness 0.863424 RMSE 0.010018
iteration 04 fitness 0.920633 RMSE 0.009083
iteration 05 fitness 0.928433 RMSE 0.007641
iteration 06 fitness 0.945002 RMSE 0.007052
iteration 07 fitness 0.971085 RMSE 0.006552
iteration 08 fitness 0.982462 RMSE 0.005477
iteration 09 fitness 0.985741 RMSE 0.004569
iteration 10 fitness 0.987281 RMSE 0.003869
iteration 11 fitness 0.988325 RMSE 0.003337
iteration 12 fitness 0.989219 RMSE 0.002945
iteration 13 fitness 0.989641 RMSE 0.002634
iteration 14 fitness 0.989890 RMSE 0.002404
iteration 15 fitness 0.990237 RMSE 0.002254
iteration 16 fitness 0.990362 RMSE 0.002140
iteration 17 fitness 0.990511 RMSE 0.002069
iteration 18 fitness 0.990685 RMSE 0.002029
iteration 19 fitness 0.990784 RMSE 0.002000
iteration 20 fitness 0.990834 RMSE 0.001979
iteration 21 fitness 0.990859 RMSE 0.001965
iteration 22 fitness 0.990908 RMSE 0.001959
iteration 23 fitness 0.990933 RMSE 0.001954
iteration 24 fitness 0.990958 RMSE 0.001951
iteration 25 fitness 0.990958 RMSE 0.001947
iteration 26 fitness 0.990983 RMSE 0.001947
iteration 27 fitness 0.991032 RMSE 0.001950
iteration 28 fitness 0.991032 RMSE 0.001949
iteration 29 fitness 0.991032 RMSE 0.001948

このapiではウィンドウが1回毎に表示されて消えるので，matplotlibかなにかの3Dプロットが使えないかなぁ，とおもってaxis3dで表示してみたけど，点の数が多すぎて表示速度が極端に遅かった．