Learning Open3D→Tutorial→再構成システム→位置合わせの精緻化
入力引数
このプログラムは、python run_system.py [config_file] --refineで実行される。ここで [config_file]で指定するファイルの["path_dataset"]には、フラグメントを格納した.plyファイルとポーズグラフを格納した.jsonファイルが入っているfragmentsディレクトリがなければならない。
メイン関数はlocal_refinement関数とoptimize_posegraph_for_scene関数を実行する。 前者の関数は、フラグメントの位置合わせで検出されたペアに対してペアごとの位置合わせを行う。 後者の関数は、多方向位置合わせを行う。
前項のフラグメントの位置合わせとほぼ同じような処理の流れである。
次はexamples/Python/ReconstructionSystem/refine_registration.pyで定義されている関数runであり、これがメイン関数である。まずplyファイルのリストを["path_dataset"]と["folder_fragment"]で指定されるディレクトリから取り出す。次にmake_posegraph_for_refined_scene関数を呼び出し、その中でポイントクラウドの前処理とペアごとの位置合わせを行う。そしてoptimize_posegraph_for_refined_scene関数により、多方向の位置合わせを行っている。
def run(config):
print("refine rough registration of fragments.")
o3d.utility.set_verbosity_level(o3d.utility.VerbosityLevel.Debug)
ply_file_names = get_file_list(
join(config["path_dataset"], config["folder_fragment"]), ".ply")
make_posegraph_for_refined_scene(ply_file_names, config)
optimize_posegraph_for_refined_scene(config["path_dataset"], config)
精密位置合わせ
examples/Python/ReconstructionSystem/refine_registration.pyで定義されている。run→make_posegraph_for_refined_scene→(並列)register_point_cloud_pair→local_refinement→multiscale_icpという呼び出し関係にある。前に述べたように、make_posegraph_for_refined_scene関数ではポイントクラウドの前処理とペアごとの位置合わせを行っている。
# examples/Python/ReconstructionSystem/refine_registration.py
def multiscale_icp(source,
target,
voxel_size,
max_iter,
config,
init_transformation=np.identity(4)):
current_transformation = init_transformation
for i, scale in enumerate(range(len(max_iter))): # multi-scale approach
iter = max_iter[scale]
distance_threshold = config["voxel_size"] * 1.4
print("voxel_size %f" % voxel_size[scale])
source_down = source.voxel_down_sample(voxel_size[scale])
target_down = target.voxel_down_sample(voxel_size[scale])
if config["icp_method"] == "point_to_point":
result_icp = o3d.registration.registration_icp(
source_down, target_down, distance_threshold,
current_transformation,
o3d.registration.TransformationEstimationPointToPoint(),
o3d.registration.ICPConvergenceCriteria(max_iteration=iter))
else:
source_down.estimate_normals(
o3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size[scale] *
2.0,
max_nn=30))
target_down.estimate_normals(
o3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size[scale] *
2.0,
max_nn=30))
if config["icp_method"] == "point_to_plane":
result_icp = o3d.registration.registration_icp(
source_down, target_down, distance_threshold,
current_transformation,
o3d.registration.TransformationEstimationPointToPlane(),
o3d.registration.ICPConvergenceCriteria(max_iteration=iter))
if config["icp_method"] == "color":
result_icp = o3d.registration.registration_colored_icp(
source_down, target_down, voxel_size[scale],
current_transformation,
o3d.registration.ICPConvergenceCriteria(
relative_fitness=1e-6,
relative_rmse=1e-6,
max_iteration=iter))
current_transformation = result_icp.transformation
if i == len(max_iter) - 1:
information_matrix = o3d.registration.get_information_matrix_from_point_clouds(
source_down, target_down, voxel_size[scale] * 1.4,
result_icp.transformation)
if config["debug_mode"]:
draw_registration_result_original_color(source, target,
result_icp.transformation)
return (result_icp.transformation, information_matrix)
精密位置合わせには、config_fileで指定された["icp_method"]の値により2つのオプションがある。colorオプションがお勧めであるが、それはカラー情報を使用してドリフトを防ぐからである。 詳細については、Park et al.(2017)を参照のこと。
Park, J., Zhou, Q.-Y., & Koltun, V. (2017). Colored Point Cloud Registration Revisited. In ICCV 2017.
多方向位置合わせ
examples/Python/ReconstructionSystem/refine_registration.pyで定義されている。make_posegraph_for_refined_scene関数の中から呼び出され、上のmultiscale_icp関数を受けて多方向位置合わせを行う.
# examples/Python/ReconstructionSystem/refine_registration.py
def update_posegrph_for_scene(s, t, transformation, information, odometry,
pose_graph):
if t == s + 1: # odometry case
odometry = np.dot(transformation, odometry)
odometry_inv = np.linalg.inv(odometry)
pose_graph.nodes.append(o3d.registration.PoseGraphNode(odometry_inv))
pose_graph.edges.append(
o3d.registration.PoseGraphEdge(s,
t,
transformation,
information,
uncertain=False))
else: # loop closure case
pose_graph.edges.append(
o3d.registration.PoseGraphEdge(s,
t,
transformation,
information,
uncertain=True))
return (odometry, pose_graph)
この関数は、多方向位置合わせで説明した手法を使用している。関数update_posegrph_for_refined_sceneは、すべてのフラグメントの多方向位置合わせのためのポーズグラフを作成する。 それぞれのグラフ・ノードは、フラグメントと、そのジオメトリをグローバルスペースに変換するポーズを表している。
ポーズグラフが作成されると、関数runに戻り、多方向位置合わせのために関数optimize_posegraph_for_sceneを呼び出す。
なおこれはexamples/Python/ReconstructionSystem/optimize_posegraph.pyで定義されている。
# examples/Python/ReconstructionSystem/optimize_posegraph.py
def optimize_posegraph_for_refined_scene(path_dataset, config):
pose_graph_name = join(path_dataset, config["template_refined_posegraph"])
pose_graph_optimized_name = join(
path_dataset, config["template_refined_posegraph_optimized"])
run_posegraph_optimization(pose_graph_name, pose_graph_optimized_name,
max_correspondence_distance = config["voxel_size"] * 1.4,
preference_loop_closure = \
config["preference_loop_closure_registration"])
メインの位置合わせのループ
次の関数make_posegraph_for_refined_sceneは、上記で説明した(egister_point_cloud_pairを経由して)multiscale_icp関数とupdate_posegrph_for_scene関数を呼び出す中心的な役割を果たすものである。
# examples/Python/ReconstructionSystem/refine_registration.py
def make_posegraph_for_refined_scene(ply_file_names, config):
pose_graph = o3d.io.read_pose_graph(
join(config["path_dataset"],
config["template_global_posegraph_optimized"]))
n_files = len(ply_file_names)
matching_results = {}
for edge in pose_graph.edges:
s = edge.source_node_id
t = edge.target_node_id
matching_results[s * n_files + t] = \
matching_result(s, t, edge.transformation)
if config["python_multi_threading"]:
from joblib import Parallel, delayed
import multiprocessing
import subprocess
MAX_THREAD = min(multiprocessing.cpu_count(),
max(len(pose_graph.edges), 1))
results = Parallel(n_jobs=MAX_THREAD)(
delayed(register_point_cloud_pair)(
ply_file_names, matching_results[r].s, matching_results[r].t,
matching_results[r].transformation, config)
for r in matching_results)
for i, r in enumerate(matching_results):
matching_results[r].transformation = results[i][0]
matching_results[r].information = results[i][1]
else:
for r in matching_results:
(matching_results[r].transformation,
matching_results[r].information) = \
register_point_cloud_pair(ply_file_names,
matching_results[r].s, matching_results[r].t,
matching_results[r].transformation, config)
pose_graph_new = o3d.registration.PoseGraph()
odometry = np.identity(4)
pose_graph_new.nodes.append(o3d.registration.PoseGraphNode(odometry))
for r in matching_results:
(odometry, pose_graph_new) = update_posegrph_for_scene(
matching_results[r].s, matching_results[r].t,
matching_results[r].transformation, matching_results[r].information,
odometry, pose_graph_new)
print(pose_graph_new)
o3d.io.write_pose_graph(
join(config["path_dataset"], config["template_refined_posegraph"]),
pose_graph_new)
この主なワークフロー: ペアごとのローカルな精緻化→多方向位置合わせ
[GlobalOptimizationLM] Optimizing PoseGraph having 14 nodes and 35 edges.
Line process weight : 789.730200
[Initial ] residual : 1.208286e+04, lambda : 1.706306e+01
[Iteration 00] residual : 2.410383e+03, valid edges : 22, time : 0.000 sec.
[Iteration 01] residual : 8.127856e+01, valid edges : 22, time : 0.000 sec.
[Iteration 02] residual : 8.031355e+01, valid edges : 22, time : 0.000 sec.
Delta.norm() < 1.000000e-06 * (x.norm() + 1.000000e-06)
[GlobalOptimizationLM] total time : 0.001 sec.
[GlobalOptimizationLM] Optimizing PoseGraph having 14 nodes and 35 edges.
Line process weight : 789.730200
[Initial ] residual : 8.031355e+01, lambda : 1.716826e+01
Delta.norm() < 1.000000e-06 * (x.norm() + 1.000000e-06)
[GlobalOptimizationLM] total time : 0.000 sec.
CompensateReferencePoseGraphNode : reference : 014個のフラグメントと、フラグメント間に52個の有効な一致ペアがある。 23回の反復後、11個のエッジが誤り(false positive)と検出された。 これらを枝刈り後、ポーズグラフの最適化を再度実行して、厳密なアライメントが得られる。(注：これらの数値は元サイトからのもの。14個のフラグメントは白井の環境でも確認されたが、他の数値については不明。８コアCPUで30秒ほどかかっている)