Learning Open3D→Tutorial→再構成システム→フラグメント作成
シーン再構築システムの最初のステップは、短いRGBDシーケンスからフラグメントを作成することである。
Input arguments¶
入力引数
このプログラムは、python run_system.py [configz_file] --makeという形式で実行する。ここで[config_file]として指定するファイル(例えばReconstructionディレクトリのconfig/tutorial.json)では、["path_dataset"]として指定するディレクトリ(例えばdataset/tutorial/)には、imageとdepthという、それぞれカラー画像と深度画像が入っているディレクトリがないといけない。 ここではカラー画像と深度画像が同期して位置合わせされていると想定している。[config_file]として指定するファイルにオプションの引数["path_intrinsic"]がある場合、これはカメラ固有行列（詳細についてはカメラ固有行列の読み込みを参照のこと）が記録されているjsonファイルへのパスである。これが指定されていない場合は、PrimeSense工場設定値が使用される。
注: PrimesenseはKinectやXtionで使われている3Dセンサの開発元でAppleが2013年に買収した。
次はexamples/Python/ReconstructionSystem/make_fragments.pyで定義されている関数で、これが本節で説明する関数を逐次呼び出す関数である（これ自体はバッチ処理の中で並列的に呼び出される）。この関数において入力引数項で述べるカメラ固有行列がセットされ、多方向位置合わせ項で述べる位置合わせが行われ(RGBD画像ペアの位置合わせ項の処理はこの中で行われる）、その結果を受けて(optimize_posegraph.pyで定義されている)関数optimize_posegraph_for_fragmentを呼び出し、多方向の位置合わせを行う。そしてmake_pointcloud_for_fragment関数によりフラグメントのための点群を生成している。
In [ ]:
def process_single_fragment(fragment_id, color_files, depth_files, n_files,
n_fragments, config):
if config["path_intrinsic"]:
intrinsic = o3d.io.read_pinhole_camera_intrinsic(
config["path_intrinsic"])
else:
intrinsic = o3d.camera.PinholeCameraIntrinsic(
o3d.camera.PinholeCameraIntrinsicParameters.PrimeSenseDefault)
sid = fragment_id * config['n_frames_per_fragment']
eid = min(sid + config['n_frames_per_fragment'], n_files)
make_posegraph_for_fragment(config["path_dataset"], sid, eid, color_files,
depth_files, fragment_id, n_fragments,
intrinsic, with_opencv, config)
optimize_posegraph_for_fragment(config["path_dataset"], fragment_id, config)
make_pointcloud_for_fragment(config["path_dataset"], color_files,
depth_files, fragment_id, n_fragments,
intrinsic, config)
In [ ]:
# examples/Python/ReconstructionSystem/make_fragments.py
def register_one_rgbd_pair(s, t, color_files, depth_files, intrinsic,
with_opencv, config):
source_rgbd_image = read_rgbd_image(color_files[s], depth_files[s], True,
config)
target_rgbd_image = read_rgbd_image(color_files[t], depth_files[t], True,
config)
option = o3d.odometry.OdometryOption()
option.max_depth_diff = config["max_depth_diff"]
if abs(s - t) is not 1:
if with_opencv:
success_5pt, odo_init = pose_estimation(source_rgbd_image,
target_rgbd_image,
intrinsic, False)
if success_5pt:
[success, trans, info] = o3d.odometry.compute_rgbd_odometry(
source_rgbd_image, target_rgbd_image, intrinsic, odo_init,
o3d.odometry.RGBDOdometryJacobianFromHybridTerm(), option)
return [success, trans, info]
return [False, np.identity(4), np.identity(6)]
else:
odo_init = np.identity(4)
[success, trans, info] = o3d.odometry.compute_rgbd_odometry(
source_rgbd_image, target_rgbd_image, intrinsic, odo_init,
o3d.odometry.RGBDOdometryJacobianFromHybridTerm(), option)
return [success, trans, info]
この関数register_one_rgbd_pairは、RGBDイメージのペアをregister_one_rgbd_pair関数で読み込み、source_rgbd_imageとtarget_rgbd_imageにセットする。次にOpen3D関数のcompute_rgbd_odometryを呼び出してRGBD画像を位置合わせする。隣接するRGBD画像の場合、初期値に単位行列を使用する。 隣接していないRGBD画像に対しては、初期値としてワイド・ベースライン・マッチングを使用する。ここでは関数pose_estimation(注: opencv_pose_estimation.pyで定義されている)により、OpenCVのORB特徴量を計算しワイド・ベースライン画像に対してスパースな特徴量をマッチさせ、次に5ポイントRANSACを実行して大まかなアライメントを推定する。そしてこれをcompute_rgbd_odometryの初期値として使用している。
In [ ]:
# examples/Python/ReconstructionSystem/make_fragments.py
def make_posegraph_for_fragment(path_dataset, sid, eid, color_files,
depth_files, fragment_id, n_fragments,
intrinsic, with_opencv, config):
o3d.utility.set_verbosity_level(o3d.utility.VerbosityLevel.Error)
pose_graph = o3d.registration.PoseGraph()
trans_odometry = np.identity(4)
pose_graph.nodes.append(o3d.registration.PoseGraphNode(trans_odometry))
for s in range(sid, eid):
for t in range(s + 1, eid):
# odometry
if t == s + 1:
print(
"Fragment %03d / %03d :: RGBD matching between frame : %d and %d"
% (fragment_id, n_fragments - 1, s, t))
[success, trans,
info] = register_one_rgbd_pair(s, t, color_files, depth_files,
intrinsic, with_opencv, config)
trans_odometry = np.dot(trans, trans_odometry)
trans_odometry_inv = np.linalg.inv(trans_odometry)
pose_graph.nodes.append(
o3d.registration.PoseGraphNode(trans_odometry_inv))
pose_graph.edges.append(
o3d.registration.PoseGraphEdge(s - sid,
t - sid,
trans,
info,
uncertain=False))
# keyframe loop closure
if s % config['n_keyframes_per_n_frame'] == 0 \
and t % config['n_keyframes_per_n_frame'] == 0:
print(
"Fragment %03d / %03d :: RGBD matching between frame : %d and %d"
% (fragment_id, n_fragments - 1, s, t))
[success, trans,
info] = register_one_rgbd_pair(s, t, color_files, depth_files,
intrinsic, with_opencv, config)
if success:
pose_graph.edges.append(
o3d.registration.PoseGraphEdge(s - sid,
t - sid,
trans,
info,
uncertain=True))
o3d.io.write_pose_graph(
join(path_dataset, config["template_fragment_posegraph"] % fragment_id),
pose_graph)
この関数make_posegraph_for_fragmentは、多方向位置合わせ項の手法を使用している。 シーケンス内のすべてのRGBD画像の多方向位置合わせのポーズグラフを作成する。それぞれのグラフ・ノードは、RGBD画像と、ジオメトリをグローバルなフラグメント空間に変換するポーズを表す。効率のために、キーフレームだけが使用される。
ポーズグラフが作成されたならば、(関数process_single_fragmentの中で)(optimize_posegraph.pyで定義されている)以下の関数optimize_posegraph_for_fragmentを呼び出し、多方向の位置合わせを行う。
In [ ]:
# examples/Python/ReconstructionSystem/optimize_posegraph.py
def run_posegraph_optimization(pose_graph_name, pose_graph_optimized_name,
max_correspondence_distance,
preference_loop_closure):
# to display messages from o3d.registration.global_optimization
o3d.utility.set_verbosity_level(o3d.utility.VerbosityLevel.Debug)
method = o3d.registration.GlobalOptimizationLevenbergMarquardt()
criteria = o3d.registration.GlobalOptimizationConvergenceCriteria()
option = o3d.registration.GlobalOptimizationOption(
max_correspondence_distance=max_correspondence_distance,
edge_prune_threshold=0.25,
preference_loop_closure=preference_loop_closure,
reference_node=0)
pose_graph = o3d.io.read_pose_graph(pose_graph_name)
o3d.registration.global_optimization(pose_graph, method, criteria, option)
o3d.io.write_pose_graph(pose_graph_optimized_name, pose_graph)
o3d.utility.set_verbosity_level(o3d.utility.VerbosityLevel.Error)
def optimize_posegraph_for_fragment(path_dataset, fragment_id, config):
pose_graph_name = join(path_dataset,
config["template_fragment_posegraph"] % fragment_id)
pose_graph_optimized_name = join(
path_dataset,
config["template_fragment_posegraph_optimized"] % fragment_id)
run_posegraph_optimization(pose_graph_name, pose_graph_optimized_name,
max_correspondence_distance = config["max_depth_diff"],
preference_loop_closure = \
config["preference_loop_closure_odometry"])
この関数はOpen3Dの関数global_optimizationを呼び出してRGBD画像のポーズを推定している。
In [ ]:
# examples/Python/ReconstructionSystem/make_fragments.py
def integrate_rgb_frames_for_fragment(color_files, depth_files, fragment_id,
n_fragments, pose_graph_name, intrinsic,
config):
pose_graph = o3d.io.read_pose_graph(pose_graph_name)
volume = o3d.integration.ScalableTSDFVolume(
voxel_length=config["tsdf_cubic_size"] / 512.0,
sdf_trunc=0.04,
color_type=o3d.integration.TSDFVolumeColorType.RGB8)
for i in range(len(pose_graph.nodes)):
i_abs = fragment_id * config['n_frames_per_fragment'] + i
print(
"Fragment %03d / %03d :: integrate rgbd frame %d (%d of %d)." %
(fragment_id, n_fragments - 1, i_abs, i + 1, len(pose_graph.nodes)))
rgbd = read_rgbd_image(color_files[i_abs], depth_files[i_abs], False,
config)
pose = pose_graph.nodes[i].pose
volume.integrate(rgbd, intrinsic, np.linalg.inv(pose))
mesh = volume.extract_triangle_mesh()
mesh.compute_vertex_normals()
return mesh
def make_pointcloud_for_fragment(path_dataset, color_files, depth_files,
fragment_id, n_fragments, intrinsic, config):
mesh = integrate_rgb_frames_for_fragment(
color_files, depth_files, fragment_id, n_fragments,
join(path_dataset,
config["template_fragment_posegraph_optimized"] % fragment_id),
intrinsic, config)
pcd = o3d.geometry.PointCloud()
pcd.points = mesh.vertices
pcd.colors = mesh.vertex_colors
pcd_name = join(path_dataset,
config["template_fragment_pointcloud"] % fragment_id)
o3d.io.write_point_cloud(pcd_name, pcd, False, True)
ポーズが推定されたら、(関数process_single_fragmentの中で)関数make_pointcloud_for_fragmentを呼び出し、RGBD統合を使用して、それぞれのRGBDシーケンスから色付きのフラグメントを再構築する。
In [ ]:
# examples/Python/ReconstructionSystem/make_fragments.py
def run(config):
print("making fragments from RGBD sequence.")
make_clean_folder(join(config["path_dataset"], config["folder_fragment"]))
[color_files, depth_files] = get_rgbd_file_lists(config["path_dataset"])
n_files = len(color_files)
n_fragments = int(math.ceil(float(n_files) / \
config['n_frames_per_fragment']))
if config["python_multi_threading"]:
from joblib import Parallel, delayed
import multiprocessing
import subprocess
MAX_THREAD = min(multiprocessing.cpu_count(), n_fragments)
Parallel(n_jobs=MAX_THREAD)(delayed(process_single_fragment)(
fragment_id, color_files, depth_files, n_files, n_fragments, config)
for fragment_id in range(n_fragments))
else:
for fragment_id in range(n_fragments):
process_single_fragment(fragment_id, color_files, depth_files,
n_files, n_fragments, config)
これがメイン関数であり、(関数process_single_fragmentを使って)上で説明した個々の関数をそれぞれ呼び出す。なおjoblibモジュールを用いて並行処理を行おうとしている（システムがマルチコアの場合)。また最初にmake_clean_folder関数によってconfig["path_dataset"]で指定されたディレクトリの下にconfig["folder_fragment"]で指定された名前のディレクトリ(デフォルトはfragments)を作成している。このディレクトリの下に、作成した.plyファイルやjsonファイルが書き込まれる。
====================================
Configuration
====================================
name : Open3D reconstruction tutorial http://open3d.org/docs/release/tutorial/ReconstructionSystem/system_overview.html
path_dataset : dataset/tutorial/
path_intrinsic :
max_depth : 3.0
voxel_size : 0.05
max_depth_diff : 0.07
preference_loop_closure_odometry : 0.1
preference_loop_closure_registration : 5.0
tsdf_cubic_size : 3.0
icp_method : color
global_registration : ransac
python_multi_threading : True
depth_map_type : redwood
n_frames_per_fragment : 100
n_keyframes_per_n_frame : 5
min_depth : 0.3
folder_fragment : fragments/
template_fragment_posegraph : fragments/fragment_%03d.json
template_fragment_posegraph_optimized : fragments/fragment_optimized_%03d.json
template_fragment_pointcloud : fragments/fragment_%03d.ply
folder_scene : scene/
template_global_posegraph : scene/global_registration.json
template_global_posegraph_optimized : scene/global_registration_optimized.json
template_refined_posegraph : scene/refined_registration.json
template_refined_posegraph_optimized : scene/refined_registration_optimized.json
template_global_mesh : scene/integrated.ply
template_global_traj : scene/trajectory.log
debug_mode : False
OpenCV is detected. Using ORB + 5pt algorithm
making fragments from RGBD sequence.
OpenCV is detected. Using ORB + 5pt algorithm
OpenCV is detected. Using ORB + 5pt algorithm
OpenCV is detected. Using ORB + 5pt algorithm
OpenCV is detected. Using ORB + 5pt algorithm
Fragment 000 / 013 :: RGBD matching between frame : 0 and 1
Fragment 001 / 013 :: RGBD matching between frame : 100 and 101
Fragment 003 / 013 :: RGBD matching between frame : 300 and 301
Fragment 002 / 013 :: RGBD matching between frame : 200 and 201
Fragment 000 / 013 :: RGBD matching between frame : 0 and 5(途中省略)
[Open3D DEBUG] Validating PoseGraph - finished.
[Open3D DEBUG] [GlobalOptimizationLM] Optimizing PoseGraph having 100 nodes and 249 edges.
[Open3D DEBUG] Line process weight : 115.308286
[Open3D DEBUG] [Initial ] residual : 1.211122e+04, lambda : 9.910960e+01
[Open3D DEBUG] [Iteration 00] residual : 9.916435e+02, valid edges : 147, time : 0.010 sec.
[Open3D DEBUG] [Iteration 01] residual : 4.294951e+02, valid edges : 147, time : 0.009 sec.
[Open3D DEBUG] [Iteration 02] residual : 4.294034e+02, valid edges : 147, time : 0.019 sec.
[Open3D DEBUG] Current_residual - new_residual < 1.000000e-06 * current_residual
[Open3D DEBUG] [GlobalOptimizationLM] total time : 0.045 sec.
[Open3D DEBUG] [GlobalOptimizationLM] Optimizing PoseGraph having 100 nodes and 246 edges.
[Open3D DEBUG] Line process weight : 116.336844
[Open3D DEBUG] [Initial ] residual : 9.908183e+01, lambda : 1.188399e+02
[Open3D DEBUG] [Iteration 00] residual : 9.907652e+01, valid edges : 147, time : 0.021 sec.
[Open3D DEBUG] Current_residual - new_residual < 1.000000e-06 * current_residual
[Open3D DEBUG] [GlobalOptimizationLM] total time : 0.027 sec.
[Open3D DEBUG] CompensateReferencePoseGraphNode : reference : 04コアCPU Intel Core i5-2520M@2.50GHz における実行: (4コアCPU Xeon X5677@3.46GHzのデュアルの場合は 16.08 sec)
====================================
Elapsed time (in h:m:s)
====================================
- Making fragments 0:44:47.227484
- Register fragments 0:00:00
- Refine registration 0:00:00
- Integrate frames 0:00:00
- Total 0:44:47.227484以下はこのプログラムによって作成されたフラグメントの例である:
