Learning Open3D→Tutorial→上級編→多方向位置合わせ
Multiway Registration¶
多方向位置合わせ
多方向位置合わせとは、グローバル空間内の複数のジオメトリを整列させる処理である。 典型的には、入力は幾何学集合（例えば点群やRGBD画像）$\{ {\bf P}_i \}$である。 出力は、剛体変換$\{ {\bf T}_i\}$のセットであり、変換された点群$\{ {\bf T}_i{\bf P}_i\}$はグローバル空間内で整列される。
Open3Dは、ポーズグラフの最適化による多方向位置合わせを実装し、バックエンドとしてChoi(2015)で提示された技術を実装している。
Choi, S., Zhou, Q.-Y., & Koltun, V. (2015). Robust Reconstruction of Indoor Scenes. In CVPR 2015
注意: コードを走らせる環境に注意すること。TestDataディレクトリがカレント(作業)ディレクトリからみて、祖先ディレクトリの下にあることを確認しよう
# examples/Python/Advanced/multiway_registration.py
import open3d as o3d
import numpy as np
voxel_size = 0.02
max_correspondence_distance_coarse = voxel_size * 15
max_correspondence_distance_fine = voxel_size * 1.5
def load_point_clouds(voxel_size=0.0):
pcds = []
for i in range(3):
pcd = o3d.io.read_point_cloud("../../TestData/ICP/cloud_bin_%d.pcd" % i)
pcd_down = pcd.voxel_down_sample(voxel_size=voxel_size)
pcds.append(pcd_down)
return pcds
def pairwise_registration(source, target):
print("Apply point-to-plane ICP")
icp_coarse = o3d.registration.registration_icp(
source, target, max_correspondence_distance_coarse, np.identity(4),
o3d.registration.TransformationEstimationPointToPlane())
icp_fine = o3d.registration.registration_icp(
source, target, max_correspondence_distance_fine,
icp_coarse.transformation,
o3d.registration.TransformationEstimationPointToPlane())
transformation_icp = icp_fine.transformation
information_icp = o3d.registration.get_information_matrix_from_point_clouds(
source, target, max_correspondence_distance_fine,
icp_fine.transformation)
return transformation_icp, information_icp
def full_registration(pcds, max_correspondence_distance_coarse,
max_correspondence_distance_fine):
pose_graph = o3d.registration.PoseGraph()
odometry = np.identity(4)
pose_graph.nodes.append(o3d.registration.PoseGraphNode(odometry))
n_pcds = len(pcds)
for source_id in range(n_pcds):
for target_id in range(source_id + 1, n_pcds):
transformation_icp, information_icp = pairwise_registration(
pcds[source_id], pcds[target_id])
print("Build o3d.registration.PoseGraph")
if target_id == source_id + 1: # odometry case
odometry = np.dot(transformation_icp, odometry)
pose_graph.nodes.append(
o3d.registration.PoseGraphNode(np.linalg.inv(odometry)))
pose_graph.edges.append(
o3d.registration.PoseGraphEdge(source_id,
target_id,
transformation_icp,
information_icp,
uncertain=False))
else: # loop closure case
pose_graph.edges.append(
o3d.registration.PoseGraphEdge(source_id,
target_id,
transformation_icp,
information_icp,
uncertain=True))
return pose_graph
if __name__ == "__main__":
# (1) Input
o3d.utility.set_verbosity_level(o3d.utility.VerbosityLevel.Debug)
pcds_down = load_point_clouds(voxel_size)
o3d.visualization.draw_geometries(pcds_down)
# (2) Full registration
print("Full registration ...")
pose_graph = full_registration(pcds_down,
max_correspondence_distance_coarse,
max_correspondence_distance_fine)
# (3) Optimizing PoseGraph
print("Optimizing PoseGraph ...")
option = o3d.registration.GlobalOptimizationOption(
max_correspondence_distance=max_correspondence_distance_fine,
edge_prune_threshold=0.25,
reference_node=0)
o3d.registration.global_optimization(
pose_graph, o3d.registration.GlobalOptimizationLevenbergMarquardt(),
o3d.registration.GlobalOptimizationConvergenceCriteria(), option)
# (4) Transform points and display
print("Transform points and display")
for point_id in range(len(pcds_down)):
print(pose_graph.nodes[point_id].pose)
pcds_down[point_id].transform(pose_graph.nodes[point_id].pose)
o3d.visualization.draw_geometries(pcds_down)
# (5) Make a combined point cloud
print("Make a combined point cloud")
pcds = load_point_clouds(voxel_size)
pcd_combined = o3d.geometry.PointCloud()
for point_id in range(len(pcds)):
pcds[point_id].transform(pose_graph.nodes[point_id].pose)
pcd_combined += pcds[point_id]
pcd_combined_down = pcd_combined.voxel_down_sample(voxel_size=voxel_size)
o3d.io.write_point_cloud("multiway_registration.pcd", pcd_combined_down)
o3d.visualization.draw_geometries([pcd_combined_down])
def load_point_clouds(voxel_size=0.0):
pcds = []
for i in range(3):
pcd = o3d.io.read_point_cloud("../../TestData/ICP/cloud_bin_%d.pcd" % i)
pcd_down = pcd.voxel_down_sample(voxel_size=voxel_size)
pcds.append(pcd_down)
return pcds
メイン・プログラムの先頭の部分で、3つのファイル(cloud_bin_0.pcd, cloud_bin_1.pcd, cloud_bin_2.pcd)からそれぞれポイントクラウドを読み込み、ダウンサンプリングして、一緒に可視化する。 これらは(まだ)整列してはいない。
`examples/Python/Advanced/multiway_registration.pyの2番目と3番目の関数:
def pairwise_registration(source, target):
print("Apply point-to-plane ICP")
icp_coarse = o3d.registration.registration_icp(
source, target, max_correspondence_distance_coarse, np.identity(4),
o3d.registration.TransformationEstimationPointToPlane())
icp_fine = o3d.registration.registration_icp(
source, target, max_correspondence_distance_fine,
icp_coarse.transformation,
o3d.registration.TransformationEstimationPointToPlane())
transformation_icp = icp_fine.transformation
information_icp = o3d.registration.get_information_matrix_from_point_clouds(
source, target, max_correspondence_distance_fine,
icp_fine.transformation)
return transformation_icp, information_icp
def full_registration(pcds, max_correspondence_distance_coarse,
max_correspondence_distance_fine):
pose_graph = o3d.registration.PoseGraph()
odometry = np.identity(4)
pose_graph.nodes.append(o3d.registration.PoseGraphNode(odometry))
n_pcds = len(pcds)
for source_id in range(n_pcds):
for target_id in range(source_id + 1, n_pcds):
transformation_icp, information_icp = pairwise_registration(
pcds[source_id], pcds[target_id])
print("Build o3d.registration.PoseGraph")
if target_id == source_id + 1: # odometry case
odometry = np.dot(transformation_icp, odometry)
pose_graph.nodes.append(
o3d.registration.PoseGraphNode(np.linalg.inv(odometry)))
pose_graph.edges.append(
o3d.registration.PoseGraphEdge(source_id,
target_id,
transformation_icp,
information_icp,
uncertain=False))
else: # loop closure case
pose_graph.edges.append(
o3d.registration.PoseGraphEdge(source_id,
target_id,
transformation_icp,
information_icp,
uncertain=True))
return pose_graph
ポーズグラフには、ノードとエッジという2つの重要な要素がある。ノードは、${\bf P_i}$をグローバル空間に変換するポーズ行列${\bf T_i}$に関連付けられた幾何学形状${\bf P_i}$の一部分である。集合${\bf T_i}$は最適化される未知の変数であり、 PoseGraph.nodesはPoseGraphNodeのリストである。グローバル空間を${\bf P_0}$の空間とする。従って、${\bf T_0}$は恒等行列である。他のポーズ行列は、隣接ノード間で変換を累積することによって初期化される。隣接ノードは、通常、大きなオーバーラップを有し、ポイントツープレーンICPで位置合わせすることができる。
ポーズグラフのエッジは、重なっている2つのノード（ジオメトリの一部）を接続する。各エッジは、ソースジオメトリ${\bf P_i}$をターゲットジオメトリ${\bf P_j}$に整列させる変換行列${\bf T_{i,j}}$を含む。このチュートリアルでは、ポイントツープレーンICPを使用して変換を推定する。より複雑なケースでは、このペアごとの位置合わせの問題は、グローバル位置合わせを介して解決する必要がある。
Choi(2015)は、ペアごとの位置合わせがエラーを起こしやすいことを指摘している。誤ったアラインメントのペアの個数は、正しくアライメントされたペアの数を上回る。したがって、ポーズグラフのエッジを2つのクラスに分割する。オドメトリ・エッジは、時間的に近い、隣接するノードを接続する。 ICPのような局所的位置合わせアルゴリズムは、それらを確実に整列させることができる。ループ閉包エッジは、非隣接ノードを接続する。アライメントはグローバル位置合わせによって検出され、信頼性が低くなる。 Open3Dでは、これら2つのエッジのクラスは、PoseGraphEdgeのインスタンス作成時においてuncertainパラメータによって区別する。
ユーザは、変換行列${\bf T_i}$に加えて、各エッジについて情報行列${\bf \Lambda_i}$を設定することができる。関数get_information_matrix_from_point_cloudsを使用して${\bf \Lambda_i}$が設定された場合、このポーズグラフのエッジ上の損失関数は、2つのノード間の対応するセットのRMSE(Root Mean Squared Error, 平均二乗誤差平方根)で近似する。詳細はChoi(2015)の式（3）〜（9）とRedwood位置合わせベンチマークを参照のこと。
ここでのスクリプトは3つのノードと3つのエッジを持つポーズグラフを作成する。エッジのうち、2つはオドメト・エッジ（uncertain = False）であり、1つはループ閉包エッジ（uncertain = True）である。
examples/Python/Advanced/multiway_registration.pyのメインの3番目の部分コード:
print("Optimizing PoseGraph ...")
option = o3d.registration.GlobalOptimizationOption(
max_correspondence_distance=max_correspondence_distance_fine,
edge_prune_threshold=0.25,
reference_node=0)
o3d.registration.global_optimization(
pose_graph, o3d.registration.GlobalOptimizationLevenbergMarquardt(),
o3d.registration.GlobalOptimizationConvergenceCriteria(), option)
Open3Dは関数global_optimizationを使ってポーズグラフの最適化を行う。 2種類の最適化メソッド、GlobalOptimizationGaussNewton とGlobalOptimizationLevenbergMarquardt を選択できる。 後者は収束性が良いので推奨する。 クラスGlobalOptimizationConvergenceCriteriaを使用すると、反復の最大回数とさまざまな最適化パラメータを設定できる。
クラスGlobalOptimizationOptionはいくつかのオプションをとる。 max_correspondence_distanceは対応閾値を決定に用いられ、 edge_prune_thresholdは、外れ値エッジをプルーニングするためのしきい値であり、reference_nodeは、グローバル・スペースとみなされるノードIDでである。
以下はターミナルに出力されたログの一部である(注意：白井が実行したログであり、元サイトの表示とは異なるが、傾向は同じようにみえる）
[Open3D DEBUG] [GlobalOptimizationLM] Optimizing PoseGraph having 3 nodes and 3 edges.
[Open3D DEBUG] Line process weight : 15.334800
[Open3D DEBUG] [Initial ] residual : 1.064185e+00, lambda : 2.960561e+00
[Open3D DEBUG] [Iteration 00] residual : 2.127069e-01, valid edges : 1, time : 0.000 sec.
[Open3D DEBUG] [Iteration 01] residual : 1.521838e-01, valid edges : 1, time : 0.000 sec.
[Open3D DEBUG] Delta.norm() < 1.000000e-06 * (x.norm() + 1.000000e-06)
[Open3D DEBUG] [GlobalOptimizationLM] total time : 0.000 sec.
[Open3D DEBUG] [GlobalOptimizationLM] Optimizing PoseGraph having 3 nodes and 3 edges.
[Open3D DEBUG] Line process weight : 15.334800
[Open3D DEBUG] [Initial ] residual : 1.521810e-01, lambda : 3.050692e+00
[Open3D DEBUG] Delta.norm() < 1.000000e-06 * (x.norm() + 1.000000e-06)
[Open3D DEBUG] [GlobalOptimizationLM] total time : 0.000 sec.
[Open3D DEBUG] CompensateReferencePoseGraphNode : reference : 0グローバル最適化は、ポーズグラフに対して2回実行される。 最初のパスは、すべてのエッジを考慮した元のポーズグラフのポーズを最適化し、不確実なエッジ間の誤ったアライメントを識別しようとする。 誤ったアライメントは、小さなラインプロセスの重みを持ち、最初のパスの後で枝刈りされれる。 2回目のパスではグローバルな整合性が確保される。 この例では、すべてのエッジが真のアライメントと見なされるため、2番目のパスは直ちに終了する。
Visualize optimization¶
最適化の視覚化
examples/Python/Advanced/multiway_registration.pyのメインの4番目の部分コード:
# (4) Transform points and display
print("Transform points and display")
for point_id in range(len(pcds_down)):
print(pose_graph.nodes[point_id].pose)
pcds_down[point_id].transform(pose_graph.nodes[point_id].pose)
o3d.visualization.draw_geometries(pcds_down)
出力:
Make a combined point cloud¶
ポイントクラウドの統合
examples/Python/Advanced/multiway_registration.pyのメインの5番目の部分コード:
# (5) Make a combined point cloud
print("Make a combined point cloud")
pcds = load_point_clouds(voxel_size)
pcd_combined = o3d.geometry.PointCloud()
for point_id in range(len(pcds)):
pcds[point_id].transform(pose_graph.nodes[point_id].pose)
pcd_combined += pcds[point_id]
pcd_combined_down = pcd_combined.voxel_down_sample(voxel_size=voxel_size)
o3d.io.write_point_cloud("multiway_registration.pcd", pcd_combined_down)
o3d.visualization.draw_geometries([pcd_combined_down])
PointCloudには、2つのポイントクラウドを1つのポイントクラウドにマージできる便利な演算子+がある。 マージ後、voxel_down_sampleを使用して、点が均一にリサンプリングされる。 これは、ポイントクラウドをマージした後に行う後処理としてお勧めする。これにより、点の重複や過密化を緩和できるためである。