OPEN3D学习笔记(四)——Global registration

OPEN3D学习笔记(四)

    • Global registration
      • Extract geometric feature
      • Input
      • RANSAC
      • Local refinement
      • Fast global registration
        • Input
        • Baseline
        • Fast global registration

Global registration

这个通常产生不太严格的对齐结果,并用作局部方法的初始化。因为 ICP registration 和Colored point cloud registration都依赖于一个粗略的初始矩阵

Extract geometric feature

提取几何特征
我们向下采样点云,估计法线,然后为每个点计算FPFH特征。FPFH功能是一个33维矢量,它描述了点的局部几何特性。33维空间中最近的邻居查询可以返回具有相似局部几何结构的点。有关详细信息,请参见[Rasu2009]。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
def preprocess_point_cloud(pcd, voxel_size):  # 传入参数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  # kdtree参数,用于估计法线的半径,一般设为体素大小的2倍
    print(":: Estimate normal with search radius %.3f." % radius_normal)
    pcd_down.estimate_normals(
        o3d.geometry.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30))  # 估计法线的1个参数,使用混合型的kdtree,半径内取最多30个邻居

    radius_feature = voxel_size * 5  # kdtree参数,用于估计特征的半径,设为体素大小的5倍
    print(":: Compute FPFH feature with search radius %.3f." % radius_feature)
    pcd_fpfh = o3d.registration.compute_fpfh_feature(
        pcd_down,
        o3d.geometry.KDTreeSearchParamHybrid(radius=radius_feature, max_nn=100))  # 计算特征的2个参数,下采样的点云数据,搜索方法kdtree
    return pcd_down, pcd_fpfh  # 返回降采样的点云、fpfh特征

Input

准备输入数据集

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
def prepare_dataset(voxel_size):
    print(":: Load two point clouds and disturb initial pose.")  # 加载2点云并打乱初始的位置
    source = o3d.io.read_point_cloud("../../TestData/ICP/cloud_bin_0.pcd")
    target = o3d.io.read_point_cloud("../../TestData/ICP/cloud_bin_1.pcd")
    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,体素大小设定为0.05m
source, target, source_down, target_down, source_fpfh, target_fpfh = prepare_dataset(voxel_size)  # 调用函数获取这些值

RANSAC

我们使用RANSAC进行全球注册。在每个RANSAC迭代中,从源点云中提取ransac_n个随机点。通过查询33维FPFH特征空间中最近的邻居,可以检测到它们在目标点云中的对应点。修剪步骤需要使用快速修剪算法来快速及早拒绝错误匹配。
有以下函数:
CorrespondenceCheckerBasedOnDistance检查对齐的点云是否接近(小于指定的阈值)。
CorrespondenceCheckerBasedOnEdgeLength检查从源和目标对应中分别绘制的任意两个任意边(由两个对应顶点形成的线)的长度是否相似。
CorrespondenceCheckerBasedOnNormal考虑任何对应的顶点法线相似度。它计算两个法线向量的点积。阈值采用弧度值。
RANSACConvergenceCriteria。它定义了RANSAC迭代的最大数量和验证步骤的最大数量

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
def execute_global_registration(source_down, target_down, source_fpfh,
                                target_fpfh, voxel_size):  # 传入2的点云的降采样,两个点云的特征、体素大小
    distance_threshold = voxel_size * 1.5  # 设定距离阈值为体素的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)
    # 2个降采样的点云,两个点云的特征,距离阈值,一个函数,4,一个list[0.9的两个对应点的线段长度阈值,两个点的距离阈值],一个函数设定最大迭代次数和最大验证次数
    result = o3d.registration.registration_ransac_based_on_feature_matching(
        source_down, target_down, source_fpfh, target_fpfh, distance_threshold,
        o3d.registration.TransformationEstimationPointToPoint(False), 4, [
            o3d.registration.CorrespondenceCheckerBasedOnEdgeLength(0.9),
            o3d.registration.CorrespondenceCheckerBasedOnDistance(
                distance_threshold)
        ], o3d.registration.RANSACConvergenceCriteria(4000000, 500))
    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) # 源点云旋转平移到目标点云

Local refinement

上面的全局点云配准生成的结果还不够好,需要再精细配准(例如point-to-plane配准)

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
# 传入参数:2个点云,2个点云的特征,体素大小
def refine_registration(source, target, source_fpfh, target_fpfh, voxel_size):
    distance_threshold = voxel_size * 0.4  # 距离阈值是体素大小的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.registration.registration_icp(
        source, target, distance_threshold, result_ransac.transformation,
        o3d.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.
registration::RegistrationResult with fitness=6.210275e-01, inlier_rmse=6.565175e-03, and correspondence_set size of 123482
Access transformation to get result.
'''

Fast global registration

下面方法没有模型建议和评价,会加快配准,节省时间。

Input

跟上面的一样,调用prepare_dataset

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

Baseline

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
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.085 sec.

registration::RegistrationResult with fitness=6.760504e-01, inlier_rmse=2.596653e-02, and correspondence_set size of 3218
Access transformation to get result.
'''

Fast global registration

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
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.registration.registration_fast_based_on_feature_matching(
        source_down, target_down, source_fpfh, target_fpfh,
        o3d.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.128 sec.

registration::RegistrationResult with fitness=5.054622e-01, inlier_rmse=1.743545e-02, and correspondence_set size of 2406
Access transformation to get result.
'''

通过适当的配置,快速全局注册的准确性甚至可以与ICP媲美。更多实验结果请参考[Zhou2016]。