全局配准

译注

本篇译自 Open3D 官方教程 Global registration（0.19.0）。代码、输出、API 名称保持原文未作改动。

ICP 配准与彩色点云配准都属于局部配准（local registration）方法，因为它们都需要一个粗略对齐作为初始化。本教程介绍另一类配准方法——全局配准（global registration）。这类算法不需要任何初始对齐作为初始化，通常得到的结果不如局部方法紧密，因而常被用作局部方法的初始化。

可视化

下面的辅助函数将变换后的源点云与目标点云一起可视化：

def draw_registration_result(source, target, transformation):
    source_temp = copy.deepcopy(source)
    target_temp = copy.deepcopy(target)
    source_temp.paint_uniform_color([1, 0.706, 0])
    target_temp.paint_uniform_color([0, 0.651, 0.929])
    source_temp.transform(transformation)
    o3d.visualization.draw_geometries([source_temp, target_temp],
                                      zoom=0.4559,
                                      front=[0.6452, -0.3036, -0.7011],
                                      lookat=[1.9892, 2.0208, 1.8945],
                                      up=[-0.2779, -0.9482, 0.1556])

提取几何特征

我们对点云进行下采样、估计法向量，然后为每个点计算 FPFH 特征。FPFH 特征是一个 33 维向量，描述了一个点的局部几何属性。在 33 维空间中进行最近邻查询，即可返回具有相似局部几何结构的点。详见 [Rasu2009]。

def preprocess_point_cloud(pcd, voxel_size):
    print(":: Downsample with a voxel size %.3f." % voxel_size)
    pcd_down = pcd.voxel_down_sample(voxel_size)

    radius_normal = voxel_size * 2
    print(":: Estimate normal with search radius %.3f." % radius_normal)
    pcd_down.estimate_normals(
        o3d.geometry.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30))

    radius_feature = voxel_size * 5
    print(":: Compute FPFH feature with search radius %.3f." % radius_feature)
    pcd_fpfh = o3d.pipelines.registration.compute_fpfh_feature(
        pcd_down,
        o3d.geometry.KDTreeSearchParamHybrid(radius=radius_feature, max_nn=100))
    return pcd_down, pcd_fpfh

输入

下面的代码从两个文件中分别读取源点云和目标点云。它们以单位矩阵作为变换，处于未对齐状态。

def prepare_dataset(voxel_size):
    print(":: Load two point clouds and disturb initial pose.")

    demo_icp_pcds = o3d.data.DemoICPPointClouds()
    source = o3d.io.read_point_cloud(demo_icp_pcds.paths[0])
    target = o3d.io.read_point_cloud(demo_icp_pcds.paths[1])
    trans_init = np.asarray([[0.0, 0.0, 1.0, 0.0], [1.0, 0.0, 0.0, 0.0],
                             [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0]])
    source.transform(trans_init)
    draw_registration_result(source, target, np.identity(4))

    source_down, source_fpfh = preprocess_point_cloud(source, voxel_size)
    target_down, target_fpfh = preprocess_point_cloud(target, voxel_size)
    return source, target, source_down, target_down, source_fpfh, target_fpfh

voxel_size = 0.05  # means 5cm for this dataset
source, target, source_down, target_down, source_fpfh, target_fpfh = prepare_dataset(
    voxel_size)

输出：

:: Load two point clouds and disturb initial pose.
[Open3D WARNING] GLFW Error: Failed to detect any supported platform
[Open3D WARNING] GLFW initialized for headless rendering.
:: Downsample with a voxel size 0.050.
:: Estimate normal with search radius 0.100.
:: Compute FPFH feature with search radius 0.250.
:: Downsample with a voxel size 0.050.
:: Estimate normal with search radius 0.100.
:: Compute FPFH feature with search radius 0.250.

RANSAC

我们使用 RANSAC 进行全局配准。在每一次 RANSAC 迭代中，从源点云中随机选取 ransac_n 个点，通过在 33 维 FPFH 特征空间中查询最近邻来找到它们在目标点云中的对应点。随后是一个剪枝（pruning）步骤，利用快速剪枝算法尽早剔除错误的匹配。

Open3D 提供如下剪枝算法：

• CorrespondenceCheckerBasedOnDistance 检查对齐后的点云是否足够接近（小于给定阈值）。
• CorrespondenceCheckerBasedOnEdgeLength 检查分别从源和目标对应关系中任意取出的两条边（由两个顶点连成的线段）的长度是否相似。本教程验证 $||edge_{source}|| > 0.9 \cdot ||edge_{target}||$ 与 $||edge_{target}|| > 0.9 \cdot ||edge_{source}||$ 是否同时成立。
• CorrespondenceCheckerBasedOnNormal 考量任意对应关系的顶点法向量亲和性。它计算两个法向量的点积，阈值以弧度值给出。

只有通过剪枝步骤的匹配才会用于计算变换，并在整个点云上进行验证。核心函数是 registration_ransac_based_on_feature_matching。该函数最重要的超参数是 RANSACConvergenceCriteria，它定义了 RANSAC 的最大迭代次数与置信概率。这两个数值越大，结果越精确，但算法耗时也越长。

我们依据 [Choi2015] 给出的经验值来设置 RANSAC 参数。

def execute_global_registration(source_down, target_down, source_fpfh,
                                target_fpfh, voxel_size):
    distance_threshold = voxel_size * 1.5
    print(":: RANSAC registration on downsampled point clouds.")
    print("   Since the downsampling voxel size is %.3f," % voxel_size)
    print("   we use a liberal distance threshold %.3f." % distance_threshold)
    result = o3d.pipelines.registration.registration_ransac_based_on_feature_matching(
        source_down, target_down, 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(100000, 0.999))
    return result

result_ransac = execute_global_registration(source_down, target_down,
                                            source_fpfh, target_fpfh,
                                            voxel_size)
print(result_ransac)
draw_registration_result(source_down, target_down, result_ransac.transformation)

输出：

:: RANSAC registration on downsampled point clouds.
   Since the downsampling voxel size is 0.050,
   we use a liberal distance threshold 0.075.
RegistrationResult with fitness 0.6xxxxxx and correspondence_set size of 3204
Access transformation to get result.
[Open3D WARNING] GLFW initialized for headless rendering.

说明

Open3D 还提供了一种更快的全局配准实现，请参见下文的快速全局配准。

局部精细化

出于性能考虑，全局配准仅在大幅下采样后的点云上进行，结果也并不紧密。我们使用 Point-to-plane ICP 进一步精细化对齐。

def refine_registration(source, target, source_fpfh, target_fpfh, voxel_size):
    distance_threshold = voxel_size * 0.4
    print(":: Point-to-plane ICP registration is applied on original point")
    print("   clouds to refine the alignment. This time we use a strict")
    print("   distance threshold %.3f." % distance_threshold)
    result = o3d.pipelines.registration.registration_icp(
        source, target, distance_threshold, result_ransac.transformation,
        o3d.pipelines.registration.TransformationEstimationPointToPlane())
    return result

result_icp = refine_registration(source, target, source_fpfh, target_fpfh,
                                 voxel_size)
print(result_icp)
draw_registration_result(source, target, result_icp.transformation)

输出：

:: Point-to-plane ICP registration is applied on original point
   clouds to refine the alignment. This time we use a strict
   distance threshold 0.020.
RegistrationResult with fitness 0.9xxxxxx and correspondence_set size of 123483
Access transformation to get result.
[Open3D WARNING] GLFW initialized for headless rendering.

快速全局配准

基于 RANSAC 的全局配准方案，由于需要进行大量模型提议与评估，可能会耗费较长时间。[Zhou2016] 提出了一种更快的方法，能够快速优化少量对应关系的线过程（line process）权重。由于每次迭代都不涉及模型提议与评估，[Zhou2016] 所提方法可节省大量计算时间。

本教程将基于 RANSAC 的全局配准运行时间与 [Zhou2016] 的实现进行对比。

输入

我们使用与上面全局配准示例相同的输入。

voxel_size = 0.05  # means 5cm for the dataset
source, target, source_down, target_down, source_fpfh, target_fpfh = \
        prepare_dataset(voxel_size)

输出：

:: Load two point clouds and disturb initial pose.
[Open3D WARNING] GLFW initialized for headless rendering.
:: Downsample with a voxel size 0.050.
:: Estimate normal with search radius 0.100.
:: Compute FPFH feature with search radius 0.250.
:: Downsample with a voxel size 0.050.
:: Estimate normal with search radius 0.100.
:: Compute FPFH feature with search radius 0.250.

基线

下面的代码对全局配准方法计时。

start = time.time()
result_ransac = execute_global_registration(source_down, target_down,
                                            source_fpfh, target_fpfh,
                                            voxel_size)
print("Global registration took %.3f sec.\n" % (time.time() - start))
print(result_ransac)
draw_registration_result(source_down, target_down, result_ransac.transformation)

输出：

:: RANSAC registration on downsampled point clouds.
   Since the downsampling voxel size is 0.050,
   we use a liberal distance threshold 0.075.
Global registration took 0.112 sec.

RegistrationResult with fitness 0.7xxxxxx and correspondence_set size of 3200
Access transformation to get result.
[Open3D WARNING] GLFW initialized for headless rendering.

快速全局配准

使用与基线相同的输入，下面的代码调用 [Zhou2016] 的实现。

def execute_fast_global_registration(source_down, target_down, source_fpfh,
                                     target_fpfh, voxel_size):
    distance_threshold = voxel_size * 0.5
    print(":: Apply fast global registration with distance threshold %.3f" \
            % distance_threshold)
    result = o3d.pipelines.registration.registration_fgr_based_on_feature_matching(
        source_down, target_down, source_fpfh, target_fpfh,
        o3d.pipelines.registration.FastGlobalRegistrationOption(
            maximum_correspondence_distance=distance_threshold))
    return result

start = time.time()
result_fast = execute_fast_global_registration(source_down, target_down,
                                               source_fpfh, target_fpfh,
                                               voxel_size)
print("Fast global registration took %.3f sec.\n" % (time.time() - start))
print(result_fast)
draw_registration_result(source_down, target_down, result_fast.transformation)

输出：

:: Apply fast global registration with distance threshold 0.025
Fast global registration took 0.132 sec.

RegistrationResult with fitness 0.4xxxxxx and correspondence_set size of 2429
Access transformation to get result.
[Open3D WARNING] GLFW initialized for headless rendering.

经过合理配置后，快速全局配准的精度甚至可与 ICP 相当。更多实验结果请参阅 [Zhou2016]。

除了基于 FPFH 特征的 FGR，还可以通过 registration_fgr_based_on_correspondence 使用基于对应关系的 FGR 来进行全局配准。当你的对应关系前端（correspondence frontend）与 FPFH 不同，但仍希望在一组假定的对应关系上使用 FGR 时，这种方法很有用。其调用方式如下：

o3d.pipelines.registration.registration_fgr_based_on_correspondence(
        source_down, target_down, correspondence_set,
        o3d.pipelines.registration.FastGlobalRegistrationOption())