OPEN3D学习笔记（六）——Multiway registration

Multiway registration

多向配准是在全局空间中对齐多个几何图形的过程。通常，输入是一组几何图形（例如，点云或RGBD图像） {Pi}。输出是一组刚性变换{Ti} ，因此变换后的点云{TiPi}在全局空间中对齐。

Input

读取，并下采样

# 返回的是一个list，里面都是降采样的（如果体素大小为0则源点云）
def load_point_clouds(voxel_size=0.0):
    pcds = []
    for i in range(3):
        pcd = o3d.io.read_point_cloud("../../TestData/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)  # 获得一个经过降采样的点云list
o3d.visualization.draw_geometries(pcds_down)  # 可视化这些点云，看看他们的位置，一般是没对准的，很乱

Pose graph

姿势图具有两个关键元素：nodes，edges。
我的理解：nodes就是每个点云，edges连接两个相邻的node（点云），这两个点云有最多的重叠部分

逐对配准容易出错，因此分成两类：
Odometry edges
Loop closure edges
测渗法边缘连接时间上接近的相邻节点。本地注册算法（例如ICP）可以可靠地对齐它们。闭环边缘连接任何不相邻的节点。该对齐方式是通过全局注册找到的，可靠性较低。在Open3D中，这两类边缘由PoseGraphEdge的初始化程序中的不确定参数来区分。

# 两两配准
def pairwise_registration(source, target):
    print("Apply point-to-plane ICP")
    icp_coarse = o3d.registration.registration_icp(
        source, target, max_correspondence_distance_coarse, np.identity(4),
        o3d.registration.TransformationEstimationPointToPlane())  # 用一个初始4*4转换矩阵，粗配准
    icp_fine = o3d.registration.registration_icp(
        source, target, max_correspondence_distance_fine,
        icp_coarse.transformation,
        o3d.registration.TransformationEstimationPointToPlane())  # 用粗配准的出的旋转矩阵，进行再次配准
    transformation_icp = icp_fine.transformation  # 配准后的出的旋转矩阵
    information_icp = o3d.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.registration.PoseGraph()  # 姿势图
    odometry = np.identity(4)  # 对角线是1的方阵，4*4
    pose_graph.nodes.append(o3d.registration.PoseGraphNode(odometry))  # 不懂这句，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.registration.PoseGraph")
            if target_id == source_id + 1:  # odometry case 在姿势图相邻的两个点云
                odometry = np.dot(transformation_icp, odometry)  # 两个矩阵相乘
                pose_graph.nodes.append(
                    o3d.registration.PoseGraphNode(np.linalg.inv(odometry)))  # 求逆矩阵，往图里添加node
                pose_graph.edges.append(
                    o3d.registration.PoseGraphEdge(source_id,
                                                   target_id,
                                                   transformation_icp,
                                                   information_icp,
                                                   uncertain=False))  # uncertain用于区分是Odometry edges
            else:  # loop closure case
                pose_graph.edges.append(
                    o3d.registration.PoseGraphEdge(source_id,
                                                   target_id,
                                                   transformation_icp,
                                                   information_icp,
                                                   uncertain=True))  # 添加边Loop closure edges，非邻居的点云的边
    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)

Open3D使用函数global_optimization执行姿势图优化。
可以选择两种类型的优化方法：GlobalOptimizationGaussNewton或GlobalOptimizationLevenbergMarquardt。
推荐使用后者，因为它具有更好的收敛性。
GlobalOptimizationConvergenceCriteria类可用于设置最大迭代次数和各种优化参数。
GlobalOptimizationOption类定义了两个选项。max_correspondence_distance确定对应阈值。edge_prune_threshold是修剪异常边缘的阈值。reference_node是被视为全局空间的节点ID。

print("Optimizing PoseGraph ...")
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)  # 进行全局优化

全局优化在姿势图上执行两次。第一遍将考虑所有边缘的情况优化原始姿态图的姿态，并尽最大努力区分不确定边缘之间的错误对齐。这些错误对齐的行处理权重较小，并且在第一次通过后将其修剪。第二遍没有它们运行，并且产生了紧密的全局对齐。在此示例中，所有边缘均被视为真实对齐，因此第二遍将立即终止。

Visualize optimization

使用draw_geometries列出并可视化已变换的点云

print("Transform points and display")
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)  # 可视化配准后的点云
'''
输出
Transform points and display
[[ 1.00000000e+00 -2.50509994e-19  0.00000000e+00  0.00000000e+00]
 [-3.35636805e-20  1.00000000e+00  1.08420217e-19 -8.67361738e-19]
 [-1.08420217e-19 -1.08420217e-19  1.00000000e+00  0.00000000e+00]
 [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]
[[ 0.8401689  -0.14645453  0.52217554  0.34785474]
 [ 0.00617659  0.96536804  0.2608187  -0.39427149]
 [-0.54228965 -0.2159065   0.81197679  1.7300472 ]
 [ 0.          0.          0.          1.        ]]
[[ 0.96271237 -0.07178412  0.2608293   0.3765243 ]
 [-0.00196124  0.96227508  0.27207136 -0.48956598]
 [-0.27051994 -0.26243801  0.92625334  1.29770817]
 [ 0.          0.          0.          1.        ]]
'''

Make a combined point cloud

合并点云
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])  # 渲染配准结果

完美~~