【Python】Open3DでICPを実装して位置合わせ(レジストレーション)
ICPの概要
Iterative Closest Point (ICP)は、3D点群の注目する点に最も近い点を求めるアルゴリズムです。このアルゴリズムは、2つの点群の間で最適な位置関係を見つけるために使用されます。
ICPアルゴリズムは以下のように動作します：
１．初期位置として、両方の点群を重心に配置します。
２．参照点群から最も近い点を探すことにより、注目する点群を更新します。
３．参照点群と注目する点群の間で対応する点を定義します。
４．対応する点を使用して、注目する点群を回転、平行移動することで最適な位置を見つけます。
５．更新された位置に基づいて、操作を繰り返します。
６．ある停止条件（例えば変化量が一定以下になったとき）が満たされるまで繰り返します。
ICPアルゴリズムは、3D形状認識、スキャンマッチング、ロボットなどの姿勢推定などに利用されます。
Pythonコード
はじめにICPの実行結果を可視化するための関数を用意します。
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],
zoom=0.4459,
front=[0.9288, -0.2951, -0.2242],
lookat=[1.6784, 2.0612, 1.4451],
up=[-0.3402, -0.9189, -0.1996])次にレジストレーション用の点群を２つ(source, target)用意します。
初期値(trans_init)を用いて剛体変換した結果を示します。
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])
threshold = 0.02
trans_init = np.asarray([[0.862, 0.011, -0.507, 0.5],
[-0.139, 0.967, -0.215, 0.7],
[0.487, 0.255, 0.835, -1.4], [0.0, 0.0, 0.0, 1.0]])
draw_registration_result(source, target, trans_init)
ソース、ターゲット、初期値、しきい値を用いてICPを実行し、結果を可視化します。
print("Apply point-to-point ICP")
reg_p2p = o3d.pipelines.registration.registration_icp(
source, target, threshold, trans_init,
o3d.pipelines.registration.TransformationEstimationPointToPoint())
print(reg_p2p)
print("Transformation is:")
print(reg_p2p.transformation)
draw_registration_result(source, target, reg_p2p.transformation)実行結果です。
Apply point-to-point ICP
RegistrationResult with fitness=3.724495e-01, inlier_rmse=7.760179e-03, and correspondence_set size of 74056
Access transformation to get result.
Transformation is:
[[ 0.83924644 0.01006041 -0.54390867 0.64639961]
[-0.15102344 0.96521988 -0.21491604 0.75166079]
[ 0.52191123 0.2616952 0.81146378 -1.50303533]
[ 0. 0. 0. 1. ]]
同様の処理をもう一度繰り返します。
reg_p2p = o3d.pipelines.registration.registration_icp(
source, target, threshold, trans_init,
o3d.pipelines.registration.TransformationEstimationPointToPoint(),
o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=2000))
print(reg_p2p)
print("Transformation is:")
print(reg_p2p.transformation)
draw_registration_result(source, target, reg_p2p.transformation)2回目の実行結果です。
RegistrationResult with fitness=6.211230e-01, inlier_rmse=6.583448e-03, and correspondence_set size of 123501
Access transformation to get result.
Transformation is:
[[ 0.84024592 0.00687676 -0.54241281 0.6463702 ]
[-0.14819104 0.96517833 -0.21706206 0.81180074]
[ 0.52111439 0.26195134 0.81189372 -1.48346821]
[ 0. 0. 0. 1. ]]
ICPの結果を並べると2つの点群が近づいて一致するように移動しているのが分かります。
初期値 | ICP1回目 | ICP2回目 |
おすすめ参考書
リンク
リンク