Open3D Ransac点云配准算法（粗配准）

目录

一、概述

1.1简介

1.2RANSAC在点云粗配准中的应用步骤

二、代码实现

2.1关键函数

2.2完整代码

2.3代码解析

2.3.1计算FPFH

1. 法线估计

2. 计算FPFH特征

2.3.2 全局配准

1.函数：execute_global_registration

2.距离阈值

3.registration_ransac_based_on_feature_matching函数

三、实现效果

3.1原始点云

3.2配准后点云

---

一、概述

1.1简介

        RANSAC（Random Sample Consensus）是一种迭代方法，用于从一组包含离群点的数据中估计模型参数。它特别适合处理包含大量噪声或离群点的数据。RANSAC在点云配准、图像拼接、3D重建等领域中广泛应用。

        RANSAC（Random Sample Consensus）在点云粗配准中的应用主要包括以下步骤：特征提取、特征匹配、模型拟合、内点验证和迭代优化。

1.2RANSAC在点云粗配准中的应用步骤

1. 特征提取：从点云中提取特征点及其特征描述符。常用的特征描述符有FPFH（Fast Point Feature Histograms）等。
2. 特征匹配：将源点云和目标点云的特征描述符进行匹配，得到初始对应关系。这些匹配关系可能包含很多误匹配。
3. 随机采样：从匹配的特征对中随机选择一定数量的样本，用于模型拟合。样本的数量应足以确定一个唯一的变换模型。
4. 模型拟合：使用选定的样本对估计一个变换模型（通常是一个刚体变换，包括旋转和平移）。
5. 内点验证：使用拟合的模型对所有匹配对进行验证，计算匹配点对之间的距离，判断是否符合模型。符合模型的点对被称为内点。
6. 评估模型：计算当前模型的内点数量，并与之前的最佳模型进行比较。如果当前模型的内点数量更多，则更新最佳模型。
7. 迭代优化：重复上述步骤一定次数（迭代次数根据数据中的离群点比例和期望成功概率来确定），最终选择内点数量最多的模型作为最佳模型。

二、代码实现

2.1关键函数

def registration_ransac_based_on_feature_matching(source, target, 
          source_feature, target_feature, 
          max_correspondence_distance,
          estimation_method=None, *args, **kwargs):

• source：源点云
• target：目标点云
• source_feature：源点云的特征描述，目前版本的open3d主要是FPFH特征描述
• target_feature：目标点云的特征描述，目前版本的open3d主要是FPFH特征描述
• max_correspondence_distance：对应点之间的最大距离
• estimation_method：默认采用的是点到点的方法。有如下三种方式：(TransformationEstimationPointToPoint, TransformationEstimationPointToPlane, TransformationEstimationForColoredICP)
• ransac_n：随机选取匹配点对的个数，也即RANSAC算法随机选取点的个数。默认值是4。
• checkers：使用快速修剪算法来提早拒绝错误匹配。，有如下3种修剪算法：(CorrespondenceCheckerBasedOnEdgeLength, CorrespondenceCheckerBasedOnDistance, CorrespondenceCheckerBasedOnNormal)
• criteria：定义RANSAC迭代的最大次数和验证的最大次数。这两个值越大,结果越准确,但要花费更多的时间。默认值是：max_iteration=100000, and max_validation=100。

2.2完整代码

import time
import open3d as o3d
import copy


# -------------传入点云数据，计算FPFH------------
def FPFH_Compute(pcd):
    radius_normal = 0.01  # kdtree参数，用于估计法线的半径，
    print(":: Estimate normal with search radius %.3f." % radius_normal)
    pcd.estimate_normals(
        o3d.geometry.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30))
# 估计法线的1个参数，使用混合型的kdtree，半径内取最多30个邻居
    radius_feature = 0.02  # kdtree参数，用于估计FPFH特征的半径
    print(":: Compute FPFH feature with search radius %.3f." % radius_feature)
    pcd_fpfh = o3d.pipelines.registration.compute_fpfh_feature(pcd,
    o3d.geometry.KDTreeSearchParamHybrid(radius=radius_feature, max_nn=100))  # 计算FPFH特征,搜索方法kdtree
    return pcd_fpfh  # 返回FPFH特征


# ----------------RANSAC配准--------------------
def execute_global_registration(source, target, source_fpfh,
                                target_fpfh):  # 传入两个点云和点云的特征
    distance_threshold = 10  # 设定距离阈值
    print("we use a liberal distance threshold %.3f." % distance_threshold)
# 2个点云，两个点云的特征，距离阈值，一个函数，4，
# 一个list[0.9的两个对应点的线段长度阈值，两个点的距离阈值]，
# 一个函数设定最大迭代次数和最大验证次数
    result = o3d.pipelines.registration.registration_ransac_based_on_feature_matching(
        source, target, source_fpfh, target_fpfh, True, distance_threshold,
        o3d.pipelines.registration.TransformationEstimationPointToPoint(False),
        4, [
            o3d.pipelines.registration.CorrespondenceCheckerBasedOnEdgeLength(
                0.9),
            o3d.pipelines.registration.CorrespondenceCheckerBasedOnDistance(
                distance_threshold)
        ], o3d.pipelines.registration.RANSACConvergenceCriteria(4000000, 500))
    return result


# ---------------可视化配准结果----------------
def draw_registration_result(source, target, transformation):
    source_temp = copy.deepcopy(source)         # 由于函数transformand paint_uniform_color会更改点云，
    target_temp = copy.deepcopy(target)         # 因此调用copy.deepcoy进行复制并保护原始点云。
    source_temp.paint_uniform_color([1, 0, 0])  # 点云着色
    target_temp.paint_uniform_color([0, 1, 0])
    source_temp.transform(transformation)
    # o3d.io.write_point_cloud("trans_of_source.pcd", source_temp)#保存点云
    o3d.visualization.draw_geometries([source_temp, target_temp],width=1200, height=900)


# ----------------读取点云数据--------------
source = o3d.io.read_point_cloud("..//..//standford_cloud_data//hand_trans.pcd")
target = o3d.io.read_point_cloud("..//..//standford_cloud_data//hand.pcd")

source=source.uniform_down_sample(every_k_points=10)
target=target.uniform_down_sample(every_k_points=10)
# -----------------计算的FPFH---------------
source_fpfh=FPFH_Compute(source)
target_fpfh=FPFH_Compute(target)
# ---------------调用RANSAC执行配准------------
start = time.time()
result_ransac = execute_global_registration(source, target,
                                            source_fpfh, target_fpfh)
print("Global registration took %.3f sec.\n" % (time.time() - start))
print(result_ransac)  # 输出RANSAC配准信息
Tr = result_ransac.transformation
draw_registration_result(source, target, Tr) 

2.3代码解析

2.3.1计算FPFH

        这段代码的主要作用是为输入的点云（pcd对象）进行法线估计和FPFH特征计算。法线估计通过指定半径和最大最近邻点数来计算每个点的法向量，而FPFH特征计算则基于这些法向量和局部邻域信息，生成描述点云局部几何特征的特征向量。这些特征在点云处理中常用于配准、识别和分割等任务，有助于捕捉点云的几何结构和局部特征信息

# -------------传入点云数据，计算FPFH------------
def FPFH_Compute(pcd):
    radius_normal = 0.01  # kdtree参数，用于估计法线的半径，
    print(":: Estimate normal with search radius %.3f." % radius_normal)
    pcd.estimate_normals(
        o3d.geometry.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30))
# 估计法线的1个参数，使用混合型的kdtree，半径内取最多30个邻居
    radius_feature = 0.02  # kdtree参数，用于估计FPFH特征的半径
    print(":: Compute FPFH feature with search radius %.3f." % radius_feature)
    pcd_fpfh = o3d.pipelines.registration.compute_fpfh_feature(pcd,
    o3d.geometry.KDTreeSearchParamHybrid(radius=radius_feature, max_nn=100))  # 计算FPFH特征,搜索方法kdtree
    return pcd_fpfh  # 返回FPFH特征

1. 法线估计

radius_normal = 0.01
pcd.estimate_normals(
    o3d.geometry.KDTreeSearchParamHybrid(radius=radius_normal, max_nn=30))

作用：

• 法线估计是指在点云中为每个点计算法向量，法向量在计算机视觉和几何处理中很重要，用于描述点云表面的方向和曲率。
• KD树是一种数据结构，用于快速查询最近邻的点。KDTreeSearchParamHybrid参数指定了法线估计算法中的搜索半径和最大最近邻点数。

2. 计算FPFH特征

radius_feature = 0.02
pcd_fpfh = o3d.pipelines.registration.compute_fpfh_feature(
    pcd, o3d.geometry.KDTreeSearchParamHybrid(radius=radius_feature, max_nn=100))

作用：

• FPFH特征（Fast Point Feature Histograms）是一种用于描述点云局部几何结构的特征描述符。它对点云中每个点的邻域进行建模，捕捉了点云的表面形状和曲率信息。
• compute_fpfh_feature函数计算点云的FPFH特征，需要指定点云对象和用于搜索的KD树参数（搜索半径和最大最近邻点数）。

2.3.2 全局配准

        这段代码实现了全局点云配准的过程，使用了RANSAC（Random Sample Consensus）算法基于特征匹配来寻找最优的刚体变换（旋转和平移），使得源点云与目标点云尽可能对齐。

# ----------------RANSAC配准--------------------
def execute_global_registration(source, target, source_fpfh,
                                target_fpfh):  # 传入两个点云和点云的特征
    distance_threshold = 10  # 设定距离阈值
    print("we use a liberal distance threshold %.3f." % distance_threshold)
# 2个点云，两个点云的特征，距离阈值，一个函数，4，
# 一个list[0.9的两个对应点的线段长度阈值，两个点的距离阈值]，
# 一个函数设定最大迭代次数和最大验证次数
    result = o3d.pipelines.registration.registration_ransac_based_on_feature_matching(
        source, target, source_fpfh, target_fpfh, True, distance_threshold,
        o3d.pipelines.registration.TransformationEstimationPointToPoint(False),
        4, [
            o3d.pipelines.registration.CorrespondenceCheckerBasedOnEdgeLength(
                0.9),
            o3d.pipelines.registration.CorrespondenceCheckerBasedOnDistance(
                distance_threshold)
        ], o3d.pipelines.registration.RANSACConvergenceCriteria(4000000, 500))
    return result

1.函数：execute_global_registration

def execute_global_registration(source, target, source_fpfh, target_fpfh):

参数：

• source：源点云对象。
• target：目标点云对象。
• source_fpfh：源点云的FPFH特征。
• target_fpfh：目标点云的FPFH特征。

2.距离阈值

distance_threshold = 10
print("we use a liberal distance threshold %.3f." % distance_threshold)

距离阈值：

• distance_threshold：设定的距离阈值，用于判断匹配点对的有效性。

3.registration_ransac_based_on_feature_matching函数

result = o3d.pipelines.registration.registration_ransac_based_on_feature_matching(
    source, target, source_fpfh, target_fpfh, True, distance_threshold,
    o3d.pipelines.registration.TransformationEstimationPointToPoint(False),
    4, [
        o3d.pipelines.registration.CorrespondenceCheckerBasedOnEdgeLength(0.9),
        o3d.pipelines.registration.CorrespondenceCheckerBasedOnDistance(
            distance_threshold)
    ], o3d.pipelines.registration.RANSACConvergenceCriteria(4000000, 500))

参数解释：

• source 和 target：待配准的源点云和目标点云。
• source_fpfh 和 target_fpfh：源点云和目标点云的FPFH特征。
• True：表示进行全局配准。
• distance_threshold：距离阈值，用于判断匹配点对的有效性。
• TransformationEstimationPointToPoint(False)：使用点到点的变换估计方法。
• 4：RANSAC算法的迭代次数。
• [...]：用于检查对应关系的有效性的列表，包括基于边长和距离的检查器。
• RANSACConvergenceCriteria(4000000, 500)：设定RANSAC的最大迭代次数和最大验证次数。

三、实现效果

3.1原始点云

3.2配准后点云