Learning Open3D→Tutorial→上級編→色付きポイントクラウド位置合わせ
Colored Point Cloud Registration¶
色付きポイントクラウド位置合わせ
このチュートリアルでは、位置合わせにジオメトリとカラーの両方を使用するICPを示す。 Park et al.(2017)のアルゴリズムを実装している。 色情報は、接平面上の位置合わせを固定する。 したがって、このアルゴリズムは、従来のポイントクラウド位置合わせアルゴリズムよりも正確で頑健であり、実行速度はICP位置合わせアルゴリズムに匹敵する。 このチュートリアルでは、ICP位置合わせの表記法を使用する。
Park, J., Zhou, Q.-Y., & V. Koltun (2017) Colored Point Cloud Registration Revisited, ICCV,
注意: コードを走らせる環境に注意すること。TestDataディレクトリがカレント(作業)ディレクトリからみて、祖先ディレクトリの下にあることを確認しよう
# examples/Python/Advanced/colored_pointcloud_registration.py
import numpy as np
import copy
import open3d as o3d
def draw_registration_result_original_color(source, target, transformation):
source_temp = copy.deepcopy(source)
source_temp.transform(transformation)
o3d.visualization.draw_geometries([source_temp, target])
if __name__ == "__main__":
# (1) Load two point clouds and show initial pose"
print("1. Load two point clouds and show initial pose")
source = o3d.io.read_point_cloud("../../TestData/ColoredICP/frag_115.ply")
target = o3d.io.read_point_cloud("../../TestData/ColoredICP/frag_116.ply")
# (1-a) draw initial alignment
current_transformation = np.identity(4)
draw_registration_result_original_color(source, target,
current_transformation)
# (2) point to plane ICP
current_transformation = np.identity(4)
print("2. Point-to-plane ICP registration is applied on original point")
print(" clouds to refine the alignment. Distance threshold 0.02.")
result_icp = o3d.registration.registration_icp(
source, target, 0.02, current_transformation,
o3d.registration.TransformationEstimationPointToPlane())
print(result_icp)
draw_registration_result_original_color(source, target,
result_icp.transformation)
# (3) Colored point cloud registration
# colored pointcloud registration
# This is implementation of following paper
# J. Park, Q.-Y. Zhou, V. Koltun,
# Colored Point Cloud Registration Revisited, ICCV 2017
voxel_radius = [0.04, 0.02, 0.01]
max_iter = [50, 30, 14]
current_transformation = np.identity(4)
print("3. Colored point cloud registration")
for scale in range(3):
iter = max_iter[scale]
radius = voxel_radius[scale]
print([iter, radius, scale])
print("3-1. Downsample with a voxel size %.2f" % radius)
source_down = source.voxel_down_sample(radius)
target_down = target.voxel_down_sample(radius)
print("3-2. Estimate normal.")
source_down.estimate_normals(
o3d.geometry.KDTreeSearchParamHybrid(radius=radius * 2, max_nn=30))
target_down.estimate_normals(
o3d.geometry.KDTreeSearchParamHybrid(radius=radius * 2, max_nn=30))
print("3-3. Applying colored point cloud registration")
result_icp = o3d.registration.registration_colored_icp(
source_down, target_down, radius, current_transformation,
o3d.registration.ICPConvergenceCriteria(relative_fitness=1e-6,
relative_rmse=1e-6,
max_iteration=iter))
current_transformation = result_icp.transformation
print(result_icp)
draw_registration_result_original_color(source, target,
result_icp.transformation)
Helper visualization function¶
視覚化のための関数
examples/Python/Advanced/colored_pointcloud_registration.pyの最初の関数定義
def draw_registration_result_original_color(source, target, transformation):
source_temp = copy.deepcopy(source)
source_temp.transform(transformation)
o3d.visualization.draw_geometries([source_temp, target])
色付きポイントクラウド間のアラインメントを示すため、draw_registration_result_original_colorによりポイントクラウドを元の色でレンダリングしている。
# (1) Load two point clouds and show initial pose"
print("1. Load two point clouds and show initial pose")
source = o3d.io.read_point_cloud("../../TestData/ColoredICP/frag_115.ply")
target = o3d.io.read_point_cloud("../../TestData/ColoredICP/frag_116.ply")
# (1-a) draw initial alignment
current_transformation = np.identity(4)
draw_registration_result_original_color(source, target,
current_transformation)
このスクリプトは、2つのファイルからソースのポイントクラウドとターゲットのポイントクラウドを読み込む。 初期値として単位行列を使用している。
Point-to-plane ICP¶
examples/Python/Advanced/colored_pointcloud_registration.pyの2番目の部分コード
# (2) point to plane ICP
current_transformation = np.identity(4)
print("2. Point-to-plane ICP registration is applied on original point")
print(" clouds to refine the alignment. Distance threshold 0.02.")
result_icp = o3d.registration.registration_icp(
source, target, 0.02, current_transformation,
o3d.registration.TransformationEstimationPointToPlane())
print(result_icp)
draw_registration_result_original_color(source, target,
result_icp.transformation)
まずベースラインの手法としてPoint-to-Plane ICP (点対平面ICP)を実行する。 以下の可視化では、緑色の三角テクスチャの位置がずれている。 これは、幾何学的制約が2つの平面の滑りを妨げないためである。
Colored point cloud registration¶
色付きポイントクラウド位置合わせの中核はregistration_colored_icpである。 Park et al.(2017)の後、共同最適化の目的でICP反復を実行する(ポイントツーポイントICPを参照）
$ E（{\bf T}）=（1-\delta）E_C（{\bf T}）+\delta E_G({|bf T})$
ここで${\bf T}$は推定される変換行列である。$E_C$と$E_G$ は、それぞれ測光および幾何学項である。$\delta \in [0,1]$は経験的に決定された重みパラメータである。
幾何学項$E_G$は、Point-to-Plane ICP(点対平面ICP)の目的関数と同じである:
$ E_G({\bf T}) = \sum_{({\bf p},{\bf q}) \in {\cal K}} (({\bf p} - {\bf Tq})\cdot {\bf n_p})^2$
ここで${\cal K}$は現在の反復における対応集合で、${\bf n_p}$は${\bf p}$点の法線である。
色の項$E_C$は、点${\bf q}$の色（$C({\bf q})$で表す）と${\bf p}$の接平面上の投影点の色との間の差を測定値を返す。
$\displaystyle E_C（{\bf T}）=\sum_{({\bf p},{\bf q}) \in {\cal K}}（C_{\bf p}（{\bf f（Tq）}）-C（{\bf q}）)^2$
ここで、$C_{\bf p}（\cdot)$は${\bf p}$の接平面上で連続的に定義された関数で、事前に計算されたものである。 関数${\bf f}(\cdot)$は、3D点を接平面に投影する。 詳細はPark et al.(2017)を参照のこと。
効率性をさらに向上させるために、Park et al.(2017)は、マルチスケールの位置合わせスキームを提案している。 これは、次のスクリプトで実装されている。
Park, J., Zhou, Q.-Y., & V. Koltun (2017) Colored Point Cloud Registration Revisited, ICCV,
examples/Python/Advanced/colored_pointcloud_registration.pyの最後の部分コード
# (3) Colored point cloud registration
# colored pointcloud registration
# This is implementation of following paper
# J. Park, Q.-Y. Zhou, V. Koltun,
# Colored Point Cloud Registration Revisited, ICCV 2017
voxel_radius = [0.04, 0.02, 0.01]
max_iter = [50, 30, 14]
current_transformation = np.identity(4)
print("3. Colored point cloud registration")
for scale in range(3):
iter = max_iter[scale]
radius = voxel_radius[scale]
print([iter, radius, scale])
print("3-1. Downsample with a voxel size %.2f" % radius)
source_down = source.voxel_down_sample(radius)
target_down = target.voxel_down_sample(radius)
print("3-2. Estimate normal.")
source_down.estimate_normals(
o3d.geometry.KDTreeSearchParamHybrid(radius=radius * 2, max_nn=30))
target_down.estimate_normals(
o3d.geometry.KDTreeSearchParamHybrid(radius=radius * 2, max_nn=30))
print("3-3. Applying colored point cloud registration")
result_icp = o3d.registration.registration_colored_icp(
source_down, target_down, radius, current_transformation,
o3d.registration.ICPConvergenceCriteria(relative_fitness=1e-6,
relative_rmse=1e-6,
max_iteration=iter))
current_transformation = result_icp.transformation
print(result_icp)
draw_registration_result_original_color(source, target,
result_icp.transformation)
ボクセルのダウンサンプリングを使用して合計3層のマルチ解像度ポイントクラウドが作成される。 法線は、頂点法線推定で計算される。 位置合わせ関数registration_colored_icpは、粗いものから細かいものまで各レイヤーに対して呼び出される。 lambda_geometricは、全体のエネルギー$\lambda E_G+(1-\lambda)E_C$における$\lambda \in [0,1]$の値を決定するregistration_colored_icpのオプション引数である。
出力は、2つの点群の緊密なアライメントである。 壁の緑色の三角形に注目しよう。
