基于rgbd的三维重建_3d map generator 教程

全栈程序员站长

发布于 2022-09-29 09:40:44

2.1K0

发布于 2022-09-29 09:40:44

文章被收录于专栏：全栈程序员必看

大家好，又见面了，我是你们的朋友全栈君。

RGBD重建场景

本文从摘抄于Open3D官方文档

文章目录

1 系统总览

该system一共有四个重要步骤：

制作片段：从输入RGBD序列的短子序列构建局部几何表面（称为片段）。这部分使用RGBD里程计、Multiway注册和RGBD积分；
注册片段：片段在全局空间中对齐以检测闭环。本部分使用全局注册、ICP注册、Multiway注册；
精细配准：使注册片段后更加紧密对齐，这部分使用ICP注册和Multiway注册；
场景整合：整合RGB-D图像以生成场景的网络模型。

1.1 数据集举例

我们将使用the SceneNN dataset来演示本教程的系统框架。另外，还有很多优秀的RGBD数据集，例如Redwood 数据、TUM RGBD 数据、ICL-NUIM 数据和 SUN3D 数据。

本教程使用SceneNN数据集中的序列016，可通过使用此快速链接来下载RGBD序列。一些帮助脚本可以从reconstruction_system/scripts找到.

1.2 快速开始

将所有RGB图像放入image文件夹中，所有深度图像放入depth文件夹中，在open3d根目录中运行以下命令

cd examples/python/reconstruction_system/
python run_system.py [config_file] [--make] [--register] [--refine] [--integrate]

config_file有以下参数和文件路径。例如，reconstruction_system/config/tutorial.json有以下脚本内容：

{ 
   
    "name": "Open3D reconstruction tutorial http://open3d.org/docs/release/tutorial/reconstruction_system/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
}

我们假设image和depth是同步配准的。path_intrinsic指定存储相机内参矩阵的json文件的路径。如果未给出，则使用PrimeSense出厂设置。对于自己创建的数据集，在使用系统前，使用适当的相机内参并可视化深度图。

python_multi_threading：true利用joblib使用每个CPU内核并行化系统。

1.3 制作数据集

一个利用Intel RealSense camara制作数据集的例子，更多细节click

2 制作片段

场景重建的第一步是利用短RGBD序列来创建片段

2.1 输入

该脚本使用python run_system.py [config] --make运行。在[config]中，["path_dataset"]应该有image和depth子文件夹来分别存放彩色图像和深度图像。我们假设彩色图像和深度图像是同步和配准的。在[config]中，可选参数[path_intrinsic]指定存储相机内参矩阵的json文件路径。

2.2 利用一对RGBD图像完成注册

# examples/python/reconstruction_system/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.pipelines.odometry.OdometryOption()
option.max_depth_diff = config["max_depth_diff"]
if abs(s - t) != 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.pipelines.odometry.compute_rgbd_odometry(
source_rgbd_image, target_rgbd_image, intrinsic, odo_init,
o3d.pipelines.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.pipelines.odometry.compute_rgbd_odometry(
source_rgbd_image, target_rgbd_image, intrinsic, odo_init,
o3d.pipelines.odometry.RGBDOdometryJacobianFromHybridTerm(), option)
return [success, trans, info]

该函数读取一对RGBD图像，并将source_rgbd_image注册到target_rgbd_image。其中，compute_rgbd_odometry是用来对齐RGBD图像的。对于相邻的RGBD图像，使用单位矩阵初始化。对于不相邻的RGBD图像，使用宽基线匹配作为初始化。特别的，pose_estimation计算OpenCV ORB特征来匹配基线图像上的稀疏特征，然后执行RANSAC来估计粗略对齐，用作compute_rgbd_odometry的初始化。

2.3 Multiway注册

# examples/python/reconstruction_system/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.pipelines.registration.PoseGraph()
trans_odometry = np.identity(4)
pose_graph.nodes.append(
o3d.pipelines.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.pipelines.registration.PoseGraphNode(
trans_odometry_inv))
pose_graph.edges.append(
o3d.pipelines.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.pipelines.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)

此脚本使用Multiway注册。make_posegraph_for_fragment为该序列中所有RGBD图像的Multiway注册构建位姿图。每个图节点代表一个RGBD图像，并将几何转换为全局片段空间的位姿。为了效率，只是用关键帧。

一旦一个位姿图被创建，将执行optimize_posegraph_for_fragment，为了估计RGBD图像的位姿。

# examples/python/reconstruction_system/optimize_posegraph.py
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"])

2.4 制作一个片段

# examples/python/reconstruction_system/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.pipelines.integration.ScalableTSDFVolume(
voxel_length=config["tsdf_cubic_size"] / 512.0,
sdf_trunc=0.04,
color_type=o3d.pipelines.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

一旦一个位姿被估计，RGBD integration被用来从RGBD序列中重建一个彩色片段。

2.5 批量处理

# examples/python/reconstruction_system/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"] is True:
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)

2.6 结果

在dataset文件夹中生成fragment文件夹

3 片段注册

一旦场景的片段被创建，下一步就是将他们在全局空间中对齐。

3.1 输入

脚本运行为python run_system.py [config] --register。在[config]中，["path_dataset"]应该有一个以.ply格式存储片段的子文件夹fragments和一个位姿图文件.json

主要函数为make_posegraph_for_scene和optimize_posegraph_for_scene。第一个函数执行图像对注册，第二个函数执行多路注册（Multiway注册）

3.2 处理点云

# examples/python/reconstruction_system/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.pipelines.registration.compute_fpfh_feature(
pcd_down,
o3d.geometry.KDTreeSearchParamHybrid(radius=voxel_size * 5.0,
max_nn=100))
return (pcd_down, pcd_fpfh)

这个函数对点云进行下采样，使其更稀疏且分布规律。法线和FPFH特征是预先计算的。

3.3 计算初始注册

# examples/python/reconstruction_system/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 reasonable 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)

此片段计算两个片段之间的粗略对齐。如果片段是相邻片段，则粗略对齐由从Make fragment获得的aggregating RGBD里程计确定。否则调用register_cloud_fpfh来进行全局注册。注意，全局注册不太可靠。

3.4 全局匹配对注册

# examples/python/reconstruction_system/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.pipelines.registration.registration_fgr_based_on_feature_matching(
source, target, source_fpfh, target_fpfh,
o3d.pipelines.registration.FastGlobalRegistrationOption(
maximum_correspondence_distance=distance_threshold))
if config["global_registration"] == "ransac":
result = o3d.pipelines.registration.registration_ransac_based_on_feature_matching(
source, target, source_fpfh, target_fpfh, True, distance_threshold,
o3d.pipelines.registration.TransformationEstimationPointToPoint(
False), 3,
[
o3d.pipelines.registration.
CorrespondenceCheckerBasedOnEdgeLength(0.9),
o3d.pipelines.registration.CorrespondenceCheckerBasedOnDistance(
distance_threshold)
],
o3d.pipelines.registration.RANSACConvergenceCriteria(
1000000, 0.999))
if (result.transformation.trace() == 4.0):
return (False, np.identity(4), np.zeros((6, 6)))
information = o3d.pipelines.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)

这个函数使用RANSAC或Fast global registration来完成匹配对全局注册

3.5 Multiway注册

# examples/python/reconstruction_system/register_fragments.py
def update_posegraph_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.pipelines.registration.PoseGraphNode(odometry_inv))
pose_graph.edges.append(
o3d.pipelines.registration.PoseGraphEdge(s,
t,
transformation,
information,
uncertain=False))
else:  # loop closure case
pose_graph.edges.append(
o3d.pipelines.registration.PoseGraphEdge(s,
t,
transformation,
information,
uncertain=True))
return (odometry, pose_graph)

这个脚本使用Multiway注册，update_posegraph_for_scene为用于Multiway注册的所有片段建立了一个位姿图。每一个图节点表示了一个片段，其位姿表示的是将其转到全局空间的位姿。

一旦一个位姿图被创建，将执行optimize_posegraph_for_fragment，为了估计RGBD图像的位姿。

# examples/python/reconstruction_system/optimize_posegraph.py
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"])

3.6 注册主循环

下面的函数make_posegraph_for_scene调用了上面介绍的所有函数，其主要工作流程：pair global registration->multiway registration

# examples/python/reconstruction_system/register_fragments.py
def make_posegraph_for_scene(ply_file_names, config):
pose_graph = o3d.pipelines.registration.PoseGraph()
odometry = np.identity(4)
pose_graph.nodes.append(o3d.pipelines.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"] == True:
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_posegraph_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)

3.7 结果

4 精细配准

4.1 输入

脚本运行python run_system.py [config] --refine。在[config]，["path_dataset"]对应的文件夹下应该有子文件夹fragments，其中保存.ply与.json文件

主要运行函数 local_refinement 和 optimize_posegraph_for_scene。第一个函数对片段对进行注册；第二个函数执行多路注册。

4.2 Fine-grained 注册

# examples/python/reconstruction_system/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 {}".format(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.pipelines.registration.registration_icp(
source_down, target_down, distance_threshold,
current_transformation,
o3d.pipelines.registration.TransformationEstimationPointToPoint(
),
o3d.pipelines.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.pipelines.registration.registration_icp(
source_down, target_down, distance_threshold,
current_transformation,
o3d.pipelines.registration.
TransformationEstimationPointToPlane(),
o3d.pipelines.registration.ICPConvergenceCriteria(
max_iteration=iter))
if config["icp_method"] == "color":
result_icp = o3d.pipelines.registration.registration_colored_icp(
source_down, target_down, distance_threshold,
current_transformation,
o3d.pipelines.registration.
TransformationEstimationForColoredICP(),
o3d.pipelines.registration.ICPConvergenceCriteria(
relative_fitness=1e-6,
relative_rmse=1e-6,
max_iteration=iter))
if config["icp_method"] == "generalized":
result_icp = o3d.pipelines.registration.registration_generalized_icp(
source_down, target_down, distance_threshold,
current_transformation,
o3d.pipelines.registration.
TransformationEstimationForGeneralizedICP(),
o3d.pipelines.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.pipelines.registration.get_information_matrix_from_point_clouds(
source_down, target_down, voxel_size[scale] * 1.4,
result_icp.transformation)
return (result_icp.transformation, information_matrix)

为精细注册提供了两个选项，即ico_method使用color效果更好

4.3 Multiway注册（同3.5）

略

4.4 注册主循环

# examples/python/reconstruction_system/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
transformation_init = edge.transformation
matching_results[s * n_files + t] = \
matching_result(s, t, transformation_init)
if config["python_multi_threading"] == True:
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.pipelines.registration.PoseGraph()
odometry = np.identity(4)
pose_graph_new.nodes.append(
o3d.pipelines.registration.PoseGraphNode(odometry))
for r in matching_results:
(odometry, pose_graph_new) = update_posegraph_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)

主要流程： pairwise local refinement -> multiway registration.

4.5 结果

5 集成场景

系统的最后一步是将RGBD图像集合成一个TSDF列表，并提取出mesh结果

5.1 输入

运行脚本python run_system.py [config] --integrate。在[config]中, ["path_dataset"]对应的子文件夹中应有帧对齐的image、depth。

5.2 集成RGBD框架

# examples/python/reconstruction_system/integrate_scene.py
def scalable_integrate_rgb_frames(path_dataset, intrinsic, config):
poses = []
[color_files, depth_files] = get_rgbd_file_lists(path_dataset)
n_files = len(color_files)
n_fragments = int(math.ceil(float(n_files) / \
config['n_frames_per_fragment']))
volume = o3d.pipelines.integration.ScalableTSDFVolume(
voxel_length=config["tsdf_cubic_size"] / 512.0,
sdf_trunc=0.04,
color_type=o3d.pipelines.integration.TSDFVolumeColorType.RGB8)
pose_graph_fragment = o3d.io.read_pose_graph(
join(path_dataset, config["template_refined_posegraph_optimized"]))
for fragment_id in range(len(pose_graph_fragment.nodes)):
pose_graph_rgbd = o3d.io.read_pose_graph(
join(path_dataset,
config["template_fragment_posegraph_optimized"] % fragment_id))
for frame_id in range(len(pose_graph_rgbd.nodes)):
frame_id_abs = fragment_id * \
config['n_frames_per_fragment'] + frame_id
print(
"Fragment %03d / %03d :: integrate rgbd frame %d (%d of %d)." %
(fragment_id, n_fragments - 1, frame_id_abs, frame_id + 1,
len(pose_graph_rgbd.nodes)))
rgbd = read_rgbd_image(color_files[frame_id_abs],
depth_files[frame_id_abs], False, config)
pose = np.dot(pose_graph_fragment.nodes[fragment_id].pose,
pose_graph_rgbd.nodes[frame_id].pose)
volume.integrate(rgbd, intrinsic, np.linalg.inv(pose))
poses.append(pose)
mesh = volume.extract_triangle_mesh()
mesh.compute_vertex_normals()
if config["debug_mode"]:
o3d.visualization.draw_geometries([mesh])

该函数首先对制作片段和注册片段读取对齐结果，然后计算全局空间中每个RGBD图像的位姿。之后，使用RGBD积分对RGBD图像进行积分。

5.3输出

发布者：全栈程序员栈长，转载请注明出处：https://javaforall.cn/192927.html原文链接：https://javaforall.cn

本文参与腾讯云自媒体同步曝光计划，分享自作者个人站点/博客。

原始发表：2022年9月16日，如有侵权请联系 cloudcommunity@tencent.com 删除

python

本文分享自作者个人站点/博客前往查看

如有侵权，请联系 cloudcommunity@tencent.com 删除。

本文参与腾讯云自媒体同步曝光计划，欢迎热爱写作的你一起参与！

python

登录后参与评论

0 条评论

热度