Learning Open3D→Tutorial→再構成システム→フラグメントの位置合わせ
フラグメントの位置合わせ
シーンのフラグメントを作ったら、次のステップはそれらをグローバルな空間で整列させることである。
入力引数
このプログラムは、python run_system.py [config_file] --registerとして実行される。ここで[config_file]として指定するファイル(例えばReconstructionディレクトリのconfig/tutorial.json)では、["path_dataset"]として指定するディレクトリ(例えばdataset/tutorial/)に、フラグメントがある.plyファイルとポーズグラフがある.jsonファイルを格納してあるディレクトリfragmentsがないといけない(これらは前項のフラグメントの作成を実行していればできているはずである)。
次はexamples/Python/ReconstructionSystem/register_fragments.pyで定義されている関数runであり、これがメイン関数である。まずplyファイルのリストを["path_dataset"]と["folder_fragment"]で指定されるディレクトリから取り出し、sceneディレクトリを["path_dataset"]で指定されているディクトリの下に作成し、make_posegraph_for_scene関数を呼び出し、その中でポイントクラウドの前処理とペアごとの位置合わせを行う。そしてoptimize_posegraph_for_scene関数により、多方向の位置合わせを行っている。
def run(config):
print("register 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_clean_folder(join(config["path_dataset"], config["folder_scene"]))
make_posegraph_for_scene(ply_file_names, config)
optimize_posegraph_for_scene(config["path_dataset"], config)
ポイントクラウドの前処理
examples/Python/ReconstructionSystem/register_fragments.pyで定義され、run→make_posegraph_for_scene→register_point_cloud_pairから呼び出される。
# examples/Python/ReconstructionSystem/register_fragments.py
def preprocess_point_cloud(pcd, config):
voxel_size = config["voxel_size"]
pcd_down = pcd.voxel_down_sample(voxel_size)
pcd_down.estimate_normals(
o3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size * 2.0,
max_nn=30))
pcd_fpfh = o3d.registration.compute_fpfh_feature(
pcd_down,
o3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size * 5.0,
max_nn=100))
return (pcd_down, pcd_fpfh)
この関数はポイントクラウドをダウンサンプリングしてスパースかつ規則的な分布にする。 法線とFPFH特徴量は事前に計算される。詳細については、ボクセル・ダウンサンプリング、頂点法線推定、幾何学的特徴量の抽出の項目を参照されたい。
初期位置合わせの計算
examples/Python/ReconstructionSystem/register_fragments.pyで定義され、proprocess_point_cloudの後に呼び出される:
# examples/Python/ReconstructionSystem/register_fragments.py
def compute_initial_registration(s, t, source_down, target_down, source_fpfh,
target_fpfh, path_dataset, config):
if t == s + 1: # odometry case
print("Using RGBD odometry")
pose_graph_frag = o3d.io.read_pose_graph(
join(path_dataset,
config["template_fragment_posegraph_optimized"] % s))
n_nodes = len(pose_graph_frag.nodes)
transformation_init = np.linalg.inv(pose_graph_frag.nodes[n_nodes -
1].pose)
(transformation, information) = \
multiscale_icp(source_down, target_down,
[config["voxel_size"]], [50], config, transformation_init)
else: # loop closure case
(success, transformation,
information) = register_point_cloud_fpfh(source_down, target_down,
source_fpfh, target_fpfh,
config)
if not success:
print("No resonable solution. Skip this pair")
return (False, np.identity(4), np.zeros((6, 6)))
print(transformation)
if config["debug_mode"]:
draw_registration_result(source_down, target_down, transformation)
return (True, transformation, information)
この関数は、2つのフラグメント間の大まかなアライメントを計算する。これらのフラグメントが隣接フラグメントの場合、フラグメント作成で得られた集約RGBDオドメトリーにより大まかな位置合わせが決められる。そうでなければ、register_point_cloud_fpfh関数を呼び出し、グローバル位置合わせを行う。Choi et al. (2015)によればグローバル位置合わせは信頼性が低いことに注意されたい。
ペアごとのグローバル位置合わせ
examples/Python/ReconstructionSystem/register_fragments.pyで定義され、前節のcompute_initial_registration関数の中で呼び出される:
# examples/Python/ReconstructionSystem/register_fragments.py
def register_point_cloud_fpfh(source, target, source_fpfh, target_fpfh, config):
distance_threshold = config["voxel_size"] * 1.4
if config["global_registration"] == "fgr":
result = o3d.registration.registration_fast_based_on_feature_matching(
source, target, source_fpfh, target_fpfh,
o3d.registration.FastGlobalRegistrationOption(
maximum_correspondence_distance=distance_threshold))
if config["global_registration"] == "ransac":
result = o3d.registration.registration_ransac_based_on_feature_matching(
source, target, source_fpfh, target_fpfh, distance_threshold,
o3d.registration.TransformationEstimationPointToPoint(False), 4, [
o3d.registration.CorrespondenceCheckerBasedOnEdgeLength(0.9),
o3d.registration.CorrespondenceCheckerBasedOnDistance(
distance_threshold)
], o3d.registration.RANSACConvergenceCriteria(4000000, 500))
if (result.transformation.trace() == 4.0):
return (False, np.identity(4), np.zeros((6, 6)))
information = o3d.registration.get_information_matrix_from_point_clouds(
source, target, distance_threshold, result.transformation)
if information[5, 5] / min(len(source.points), len(target.points)) < 0.3:
return (False, np.identity(4), np.zeros((6, 6)))
return (True, result.transformation, information)
この関数はconfigファイルの中で指定される["global_registration"]の値により、RANSACか高速グローバル位置合わせを用いてペアごとのグローバル位置合わせを行っている。
多方向位置合わせ
examples/Python/ReconstructionSystem/register_fragments.pyで定義され、make_posegraph_for_scene関数のなかで、register_point_cloud_pair関数の後に呼び出される。
# examples/Python/ReconstructionSystem/register_fragments.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_scene関数は、多方向位置合わせで説明した手法を使っている。この関数は、すべてのフラグメントに対する多方向位置合わせのポーズグラフを作成する。 グラフノードは、フラグメントと、そのジオメトリをグローバル・スペースに変換するポーズを表す。
ポーズ・グラフが作成されると、関数runに戻って、多方向位置合わせのために関数optimize_posegraph_for_sceneが呼び出される。
メインの位置合わせループ
次の関数make_posegraph_for_sceneは、ポイントクラウドの前処理からここまでに説明したすべての関数を呼び出すものである。 主なワークフローは:ペアごとのグローバル位置合わせ→多方向位置合わせ。
# examples/Python/ReconstructionSystem/register_fragments.py
def make_posegraph_for_scene(ply_file_names, config):
pose_graph = o3d.registration.PoseGraph()
odometry = np.identity(4)
pose_graph.nodes.append(o3d.registration.PoseGraphNode(odometry))
n_files = len(ply_file_names)
matching_results = {}
for s in range(n_files):
for t in range(s + 1, n_files):
matching_results[s * n_files + t] = matching_result(s, t)
if config["python_multi_threading"]:
from joblib import Parallel, delayed
import multiprocessing
import subprocess
MAX_THREAD = min(multiprocessing.cpu_count(),
max(len(matching_results), 1))
results = Parallel(n_jobs=MAX_THREAD)(delayed(
register_point_cloud_pair)(ply_file_names, matching_results[r].s,
matching_results[r].t, config)
for r in matching_results)
for i, r in enumerate(matching_results):
matching_results[r].success = results[i][0]
matching_results[r].transformation = results[i][1]
matching_results[r].information = results[i][2]
else:
for r in matching_results:
(matching_results[r].success, matching_results[r].transformation,
matching_results[r].information) = \
register_point_cloud_pair(ply_file_names,
matching_results[r].s, matching_results[r].t, config)
for r in matching_results:
if matching_results[r].success:
(odometry, pose_graph) = update_posegrph_for_scene(
matching_results[r].s, matching_results[r].t,
matching_results[r].transformation,
matching_results[r].information, odometry, pose_graph)
o3d.io.write_pose_graph(
join(config["path_dataset"], config["template_global_posegraph"]),
pose_graph)
[GlobalOptimizationLM] Optimizing PoseGraph having 14 nodes and 42 edges.
Line process weight : 55.885667
[Initial ] residual : 7.791139e+04, lambda : 1.205976e+00
[Iteration 00] residual : 6.094275e+02, valid edges : 22, time : 0.001 sec.
[Iteration 01] residual : 4.526879e+02, valid edges : 22, time : 0.000 sec.
[Iteration 02] residual : 4.515039e+02, valid edges : 22, time : 0.000 sec.
[Iteration 03] residual : 4.514832e+02, valid edges : 22, time : 0.000 sec.
[Iteration 04] residual : 4.514825e+02, valid edges : 22, time : 0.000 sec.
Current_residual - new_residual < 1.000000e-06 * current_residual
[GlobalOptimizationLM] total time : 0.003 sec.
[GlobalOptimizationLM] Optimizing PoseGraph having 14 nodes and 35 edges.
Line process weight : 60.762800
[Initial ] residual : 6.336097e+01, lambda : 1.324043e+00
[Iteration 00] residual : 6.334147e+01, valid edges : 22, time : 0.000 sec.
[Iteration 01] residual : 6.334138e+01, valid edges : 22, time : 0.000 sec.
Current_residual - new_residual < 1.000000e-06 * current_residual
[GlobalOptimizationLM] total time : 0.001 sec.
CompensateReferencePoseGraphNode : reference : 014個のフラグメントと、フラグメント間に52個の有効な一致ペアがある。 繰り返し23回の後、11個のエッジが誤り(false positive)と検出されている。これらを枝刈り後、ポーズグラフ最適化を再度実行して、厳密なアライメントがえられる。(注：これらの数値は元サイトからのもの。14個のフラグメントは白井の環境でも確認されたが、他の数値については不明。８コアCPUで40秒ほどかかっている)