Learning Open3D→Tutorial→基礎編→ICP位置合わせ
ICP Registration¶
ICP（反復最近接点）位置合わせ
このチュートリアルでは、ICP（反復最近接点）位置合わせ(registration)アルゴリズムを示す。 長年にわたり、研究と企業の両方で幾何学的な位置合わせ処理の柱となってきた。 入力は2つのポイントクラウド、つまりソースのポイントクラウドをターゲットのポイントクラウドにおおよそ整列させるための初期変換である。 出力は、入力の2つのポイントクラウドを密接に整列させる洗練された変換である。 ヘルパー関数draw_registration_resultは、位置合わせ処理中のアライメントを可視化する。 このチュートリアルでは、ポイントツーポイントICPとポイントツープレーンICP (Rusinkiewicz & Levoy 2001)という2つのICPバリアントを示す。
Rusinkiewicz, S. & M. Levoy. (2001). Efficient variants of the ICP algorithm. In 3-D Digital Imaging and Modeling.
注: ICPについては https://en.wikipedia.org/wiki/Iterative_closest_point 参照
反復最近接点（ICP）は、2つのポイントクラウドの差を最小限に抑えるために使用されるアルゴリズムである。 ICPは、さまざまなスキャンから2Dまたは3Dサーフェスを再構成したり、ロボットにおいて位置推定したり、最適なパス計画を立てたり（特に滑りやすい地形のためにホイールオドメトリーが信頼性が低い場合）、骨モデルの共同位置合わせなどによく使用される。
注意: コードを走らせる環境に注意すること。TestDataディレクトリがカレント(作業)ディレクトリからみて、祖先ディレクトリの下にあることを確認しよう
# examples/Python/Basic/icp_registration.py
import open3d as o3d
import numpy as np
import copy
### Helper visualization function
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])
if __name__ == "__main__":
# (1) Input
source = o3d.io.read_point_cloud("../../TestData/ICP/cloud_bin_0.pcd")
target = o3d.io.read_point_cloud("../../TestData/ICP/cloud_bin_1.pcd")
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)
print("Initial alignment")
evaluation = o3d.registration.evaluate_registration(source, target,
threshold, trans_init)
print(evaluation)
# (2) Point-to-point ICP
print("Apply point-to-point ICP")
reg_p2p = o3d.registration.registration_icp(
source, target, threshold, trans_init,
o3d.registration.TransformationEstimationPointToPoint())
print(reg_p2p)
print("Transformation is:")
print(reg_p2p.transformation)
print("")
draw_registration_result(source, target, reg_p2p.transformation)
# (3) Point-to-plane ICP
print("Apply point-to-plane ICP")
reg_p2l = o3d.registration.registration_icp(
source, target, threshold, trans_init,
o3d.registration.TransformationEstimationPointToPlane())
print(reg_p2l)
print("Transformation is:")
print(reg_p2l.transformation)
print("")
draw_registration_result(source, target, reg_p2l.transformation)
### Helper visualization function
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])
この関数は、ターゲットのポイントクラウドを可視化し、ソースのポイントクラウドをアライメント変換で変換する。 ターゲットのポイントクラウドおよびソースのポイントクラウドは、それぞれシアンおよびイエローの色で塗られている。 2つのポイントクラウドが互いに重なり合っているほど、整列結果が良好になる。
注意:¶
関数transformとpaint_uniform_colorはポイントクラウドを変更するので、copy.deepcopyを呼び出してコピーを作成し、元のポイントクラウドを保護している。
Input¶
examples/Python/Basic/icp_registration.py の最初の部分コード
# (1) Input
source = o3d.io.read_point_cloud("../../TestData/ICP/cloud_bin_0.pcd")
target = o3d.io.read_point_cloud("../../TestData/ICP/cloud_bin_1.pcd")
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)
このスクリプトは、2つのファイルからソース・ポイントクラウドとターゲット・ポイントクラウドを読み込み、大まかな変換を行う。
注意：¶
初期位置合わせは、通常、グローバル位置合わせアルゴリズムによって得られる。 例については、グローバル位置合わせを参照せよ。
print("Initial alignment")
evaluation = o3d.registration.evaluate_registration(source, target,
threshold, trans_init)
print(evaluation)
関数evaluate_registrationは2つの主要なメトリクスを計算する。 fitnessはオーバーラップエリア（inlier対応数/ターゲット内のポイント数）を測定する。数値が大きいほどほど良い。 inlier_rmseはすべてのinlier対応の(Root Mean Squared Error, 平均二乗誤差平方根)を測定する。 数値が小さいほど良い。
Initial alignment
RegistrationResult with fitness = 0.174723, inlier_rmse = 0.011771,
and correspondence_set size of 34741
Access transformation to get result.Point-to-point ICP¶
ポイントツーポイントICP 一般に、ICPアルゴリズムは2つのステップを繰り返す：
ターゲット・ポイントクラウド${\bf P}$からの対応集合${\cal K}=\{（{\bf p}、{\bf q}）\}$と、現在の変換行列${\bf T}$で変換されたソース・ポイントクラウド${\bf Q}$を見つける。
対応集合${\cal K}$に対して定義された目的関数$E({\bf T})$を最小化することによって変換${\bf T}$を更新する。 異なるICPの変形は、異なる目的関数$E({\bf T})$）(Besl & McKay (1992), Chen & Medioni(1992), Park et al. (2017) を使用する。
最初に、次の目的関数を用いたポイントツーポイントICP(点対点ICP)アルゴリズム(Besl & McKay 1992)を示す: $$\displaystyle E({\bf T}) = \sum_{({\bf p},{\bf q}) \in {\cal K}} \|{\bf p} - {\bf Tq}\|^2$$
Besl P. J. & N. D. McKay. (1992) A Method for Registration of 3D Shapes, PAMI.
Chen, Y. & G. G. Medioni (1992) Object modelling by registration of multiple range images, Image and Vision Computing, 10(3).
Park, J., Zhou, Q.-Y., & V. Koltun. (2017) Colored Point Cloud Registration Revisited, ICCV.
examples/Python/Basic/icp_registration.py の2番目の部分コード
# (2) Point-to-point ICP
print("Apply point-to-point ICP")
reg_p2p = o3d.registration.registration_icp(
source, target, threshold, trans_init,
o3d.registration.TransformationEstimationPointToPoint())
print(reg_p2p)
print("Transformation is:")
print(reg_p2p.transformation)
print("")
draw_registration_result(source, target, reg_p2p.transformation)
クラスTransformationEstimationPointToPointは、ポイントツーポイントICP対象の残差行列とヤコビ行列を計算する関数を提供する。 関数registration_icpはそれをパラメータとして取り、ポイントツーポイントICPを実行して結果を返す。
Apply point-to-point ICP
RegistrationResult with fitness = 0.372450, inlier_rmse = 0.007760,
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. ]]fitnessスコアは0.174723から0.372450に増加し、inlier_rmseは0.011771から0.007760に減少する。 デフォルトでは、registration_icpは収束するまで、または最大反復回数（デフォルトでは30）に達するまで実行される。 より多く計算時間をかけることによって、結果を改善するように変更することができる。
reg_p2p = registration_icp(source, target, threshold, trans_init,
TransformationEstimationPointToPoint(),
ICPConvergenceCriteria(max_iteration = 2000))この出力:
Apply point-to-point ICP
RegistrationResult with fitness = 0.621123, inlier_rmse = 0.006583,
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アルゴリズムは収束するまで144回の反復を要した。 最終的なアライメントはタイトで、fitnessスコアは0.621123に向上し、inlier_rmseは0.006583に減少した。
Point-to-plane ICP¶
ポイントツープレーンICP(点対平面ICP)アルゴリズム(Chen & Medioni 1992)は、ポイントツーポイントICPとは異なる目的関数を用いている: $$\displaystyle E({\bf T})=\sum_{(p,q) \in {\cal K}} (({\bf p}−{\bf Tq})\cdot {\bf n}_p)^2$$
ここで、${\bf n}_p$は点${\bf p}$の法線である。 Rusinkiewicz & Levoy(2001)は、ポイントツープレーンICPアルゴリズムの方がポイントツーポイントICPアルゴリズムよりも収束速度が速いことを示している。
Chen Y. & G. G. Medioni. (1992). Object modelling by registration of multiple range images, Image and Vision Computing, 10(3).
Rusinkiewicz, S. & M. Levoy. (2001) Efficient variants of the ICP algorithm. In 3-D Digital Imaging and Modeling.
examples/Python/Basic/icp_registration.py の3番目の部分コード:
# (3) Point-to-plane ICP
print("Apply point-to-plane ICP")
reg_p2l = o3d.registration.registration_icp(
source, target, threshold, trans_init,
o3d.registration.TransformationEstimationPointToPlane())
print(reg_p2l)
print("Transformation is:")
print(reg_p2l.transformation)
print("")
draw_registration_result(source, target, reg_p2l.transformation)
ここではregistration_icpには、別のパラメータTransformationEstimationPointToPlaneが与えられている。 内部的には、このクラスは、ポイントツープレーン(点対平面)ICP対象の残差およびヤコビ行列を計算する関数を実装している。
注意: ポイントツープレーンICPアルゴリズムは点法線を使用する。 このチュートリアルでは、ファイルから法線を読み込んでいる。 法線が与えられていない場合は、頂点の法線推定で計算できる。
Apply point-to-plane ICP
RegistrationResult with fitness = 0.620972, inlier_rmse = 0.006581,
and correspondence_set size of 123471
Access transformation to get result.
Transformation is:
[[ 0.84023324 0.00618369 -0.54244126 0.64720943]
[-0.14752342 0.96523919 -0.21724508 0.81018928]
[ 0.52132423 0.26174429 0.81182576 -1.48366001]
[ 0. 0. 0. 1. ]]ポイントツープレーンICPは、30回の反復で緊密なアライメントに達する（適合度0.620972およびinlier_rmse 0.006581）。