多视角配准是在全局空间中对齐多个几何形状的过程。通常,输入是一组几何形状(例如:点云或者RGBD图像)。输出是一组刚性变换,以便变换后的点云可在全局空间中对齐。
Open3D通过姿态图优化实现多视角配准。后端实现了[Choi2015]中介绍的技术
输入
教程代码的第一部分是从三个文件中读取三个点云数据,并对其下采样和可视化,可看出它们是没有不对齐的。
def load_point_clouds(voxel_size=0.0):
pcds = []
for i in range(3):
# 读取点云
pcd = o3d.io.read_point_cloud("../../test_data/ICP/cloud_bin_%d.pcd" % i)
# 下采样
pcd_down = pcd.voxel_down_sample(voxel_size=voxel_size)
pcds.append(pcd_down)
return pcds
voxel_size = 0.02
pcds_down = load_point_clouds(voxel_size)
o3d.visualization.draw_geometries(pcds_down,
zoom=0.3412,
front=[0.4257, -0.2125, -0.8795],
lookat=[2.6172, 2.0475, 1.532],
up=[-0.0694, -0.9768, 0.2024])
姿态图
姿态图有两个关键元素:节点和边。节点是与姿态矩阵关联的一组几何体,通过该矩阵能够将转换到全局空间。集和是一组待优化的未知的变量。PoseGraph.nodes
是PoseGraphNode
的列表。我们设的空间是全局空间。因此是单位矩阵。其他的姿态矩阵通过累加相邻节点之间的变换来初始化。相邻节点通常都有着大规模的重叠并且能够通过Point-to-plane ICP来配准。
姿态图的边连接着两个重叠的节点(几何形状)。每个边都包含着能够将源几何体和目标几何体对齐的变换矩阵。本教程使用Point-to-plane ICP来估计变换矩阵。在更复杂的情况中,成对的配准问题一般是通过全局配准来解决的。
[Choi2015] 观察到,成对的配准容易出错。甚至错误的匹配会大于正确的匹配,因此,他们将姿态图的边分为两类。Odometry edges连接着邻域节点,使用局部配准的方式比如ICP就可以对齐他们。Loop closure edges连接着非邻域的节点。该对齐是通过不太可靠的全局配准找到的。在Open3d中,这两类边缘通过PoseGraphEdge
初始化程序中的uncertain
参数来确定。
除了旋转矩阵以外,用户也可以去设置每一条边的信息矩阵。如果是通过 get_information_matrix_from_point_clouds
设置的信息矩阵,那么姿态图的边的损失将以 line process weight 近似于两组节点对应点集的RMSE。有关详细细节请参考[Choi2015] 和 the Redwood registration benchmark。
下面的脚本创造了具有三个节点和三个边的姿态图。这些边里,两个是odometry edges(uncertain = False
),一个是loop closure edge(uncertain = True
)。
def pairwise_registration(source, target):
print("应用点到面的ICP")
# 粗配准
icp_coarse = o3d.pipelines.registration.registration_icp(source, target, max_correspondence_distance_coarse,
np.identity(4),
o3d.pipelines.registration.TransformationEstimationPointToPlane())
# 精配准
icp_fine = o3d.pipelines.registration.registration_icp(source, target, max_correspondence_distance_fine,
icp_coarse.transformation,
o3d.pipelines.registration.TransformationEstimationPointToPlane())
transformation_icp = icp_fine.transformation
# 从变换矩阵计算信息矩阵
information_icp = o3d.pipelines.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.pipelines.registration.PoseGraph()
odometry = np.identity(4)
pose_graph.nodes.append(o3d.pipelines.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.pipelines.registration.PoseGraph")
if target_id == source_id + 1: # odometry case
odometry = np.dot(transformation_icp, odometry)
pose_graph.nodes.append(o3d.pipelines.registration.PoseGraphNode(np.linalg.inv(odometry)))
pose_graph.edges.append(o3d.pipelines.registration.PoseGraphEdge(source_id,
target_id,
transformation_icp,
information_icp,
uncertain=False))
else: # loop closure case
pose_graph.edges.append(o3d.pipelines.registration.PoseGraphEdge(source_id,
target_id,
transformation_icp,
information_icp,
uncertain=True))
return pose_graph
print("Full registration ...")
max_correspondence_distance_coarse = voxel_size * 15
max_correspondence_distance_fine = voxel_size * 1.5
with o3d.utility.VerbosityContextManager(o3d.utility.VerbosityLevel.Debug) as cm:
pose_graph = full_registration(pcds_down, max_correspondence_distance_coarse, max_correspondence_distance_fine)
print("Optimizing PoseGraph ...")
option = o3d.pipelines.registration.GlobalOptimizationOption(
max_correspondence_distance=max_correspondence_distance_fine,
edge_prune_threshold=0.25,
reference_node=0)
with o3d.utility.VerbosityContextManager(o3d.utility.VerbosityLevel.Debug) as cm:
o3d.pipelines.registration.global_optimization(pose_graph,
o3d.pipelines.registration.GlobalOptimizationLevenbergMarquardt(),
o3d.pipelines.registration.GlobalOptimizationConvergenceCriteria(),
option)
Open3d使用函数global_optimization
进行姿态图估计,可以选择两种类型的优化算法,分别是GlobalOptimizationGaussNewton
和GlobalOptimizationLevenbergMarquardt
。比较推荐后一种的原因是因为它具有比较好的收敛性。GlobalOptimizationConvergenceCriteria
类可以用来设置最大迭代次数和别的优化参数。
GlobalOptimizationOption定义
了两个参数。max_correspondence_distance
定义了对应阈值。edge_prune_threshold
是修剪异常边缘的阈值。reference_node
是被视为全局空间的节点ID。
print("优化姿态图 ...")
option = o3d.registration.GlobalOptimizationOption(
max_correspondence_distance=max_correspondence_distance_fine,
edge_prune_threshold=0.25,
reference_node=0)
with o3d.utility.VerbosityContextManager(o3d.utility.VerbosityLevel.Debug) as cm:
o3d.registration.global_optimization(
pose_graph, o3d.registration.GlobalOptimizationLevenbergMarquardt(),
o3d.registration.GlobalOptimizationConvergenceCriteria(), option)
全局优化在姿态图上执行两次。 第一遍将考虑所有边缘的情况优化原始姿态图的姿态,并尽量区分不确定边缘之间的错误对齐。这些错误对齐将会产生小的 line process weights,它们将会在第一遍被剔除。第二遍将会在没有这些边的情况下运行,产生更紧密地全局对齐效果。在这个例子中,所有的边都将被考虑为真实的匹配,所以第二遍将会立即终止。
可视化优化
使用draw_geometries函数可视化变换点云。
print("变换点云并可视化")
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,
zoom=0.3412,
front=[0.4257, -0.2125, -0.8795],
lookat=[2.6172, 2.0475, 1.532],
up=[-0.0694, -0.9768, 0.2024])
得到合并的点云
PointCloud有一个方便的操作符+,可以将两个点云合并为一个点云。在下面的代码中,合并之后,将会使用voxel_down_sample
进行重新采样。建议在合并之后对点云进行后处理,因为这样可以减轻重复或过密的点。
# 读取点云
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],
zoom=0.3412,
front=[0.4257, -0.2125, -0.8795],
lookat=[2.6172, 2.0475, 1.532],
up=[-0.0694, -0.9768, 0.2024])
注意:
尽管这个教程展示的点云的多视角配准,但是相同的处理步骤可以应用于RGBD图像,请参看 Make fragments 示例。