pythonで点群処理できるOpen3Dの探検．

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

Open3Dの使い方：ICPの収束の様子を1ステップ毎に見るではウィンドウ生成破棄の繰り返しでイマイチだったので，コールバックでやってみた．

Tips:

コールバック付きの表示はpy3d.draw_geometries_with_animation_callback()
コールバック関数の返り値はboolで，再描画したければTrueを返すこと．
- これがハマった．．．tutorialのようにFalseを返してしまうと，回転しても視点を動かしても，最初に登録された点群が繰り返し表示されるだけ．どうしたものやら，とgithubのソースを眺めてたらコメントに書いてあったので，ようやく更新した点群の内容の表示ができるようになった．

コード

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

def one_step_ICP(vis):
    vis.get_view_control().rotate(x=50.0, y=5.0) # 毎回回転させる
    info = py3d.registration_icp(pcd1, pcd2, 
                                 max_correspondence_distance=th, 
                                 # init=T, # デフォルトで単位行列
                                 estimation_method=est_method,
                                 criteria=criteria
                                )
    print("fitness {0:.6f} RMSE {1:.6f}".format(info.fitness, info.inlier_rmse))
    pcd1.transform(info.transformation)
    return True # pcd1を変更したので，それを反映させるためにTrueを返すこと

py3d.draw_geometries_with_animation_callback([pcd1, pcd2], one_step_ICP, "iterations", 640, 480)

結果

fitness 0.659380 RMSE 0.010144
fitness 0.716738 RMSE 0.010305
fitness 0.785870 RMSE 0.010074
fitness 0.863424 RMSE 0.010018
fitness 0.920633 RMSE 0.009083
fitness 0.928433 RMSE 0.007641
fitness 0.945002 RMSE 0.007052
fitness 0.971085 RMSE 0.006552
fitness 0.982462 RMSE 0.005477
fitness 0.985741 RMSE 0.004569
fitness 0.987281 RMSE 0.003869
fitness 0.988325 RMSE 0.003337
fitness 0.989219 RMSE 0.002945
fitness 0.989641 RMSE 0.002634
fitness 0.989890 RMSE 0.002404
fitness 0.990237 RMSE 0.002254
fitness 0.990362 RMSE 0.002140
fitness 0.990511 RMSE 0.002069
fitness 0.990685 RMSE 0.002029
fitness 0.990784 RMSE 0.002000
fitness 0.990834 RMSE 0.001979
fitness 0.990859 RMSE 0.001965
fitness 0.990908 RMSE 0.001959
fitness 0.990933 RMSE 0.001954
fitness 0.990958 RMSE 0.001951
fitness 0.990958 RMSE 0.001947
fitness 0.990983 RMSE 0.001947
fitness 0.991032 RMSE 0.001950
fitness 0.991032 RMSE 0.001949
fitness 0.991032 RMSE 0.001948
fitness 0.991032 RMSE 0.001947
fitness 0.991032 RMSE 0.001947
fitness 0.991057 RMSE 0.001949
fitness 0.991057 RMSE 0.001949
fitness 0.991107 RMSE 0.001953
fitness 0.991107 RMSE 0.001953
fitness 0.991107 RMSE 0.001953
fitness 0.991107 RMSE 0.001953
fitness 0.991107 RMSE 0.001953
fitness 0.991132 RMSE 0.001955
fitness 0.991132 RMSE 0.001955
fitness 0.991132 RMSE 0.001955
fitness 0.991132 RMSE 0.001955
fitness 0.991132 RMSE 0.001955
fitness 0.991132 RMSE 0.001955
fitness 0.991132 RMSE 0.001955
fitness 0.991132 RMSE 0.001955
fitness 0.991132 RMSE 0.001955

Open3Dの使い方：コールバックでICPの収束の様子を表示

コード

結果