(七) 三维点云课程---ICP应用

(七)三维点云课程—ICP应用

在实际的ICP应用中，由于初始位姿的不确定，需要采用**Feature detection + description + RANSAC **进行初始估计，最后在采用ICP进行精确的位姿估计。

大致流程：

1.代码解读

1.1滤波

# 加载源点云
pcd_source = io.read_point_cloud_bin(os.path.join(input_dir, 'point_clouds', '{}.bin'.format(idx_source)))
pcd_source, idx_inliers = pcd_source.remove_radius_outlier(nb_points=4, radius=radius) #进行滤波
search_tree_source = o3d.geometry.KDTreeFlann(pcd_source)     #构建KD树

#加载目标点云
pcd_target = io.read_point_cloud_bin(os.path.join(input_dir, 'point_clouds', '{}.bin'.format(idx_target)))
pcd_target, idx_inliers = pcd_target.remove_radius_outlier(nb_points=4, radius=radius) #进行滤波
search_tree_target = o3d.geometry.KDTreeFlann(pcd_target)     #构建KD树

说明： 通过o3d下的remove_radius_outlier进行半径内滤波，并在此时建立KD搜树。

1.2 计算特征向量

#ISS特征点的提取(id,x,y,z,lambda_0,lambda_1,lambda_2)
keypoints_source = detect(pcd_source, search_tree_source, radius)   #维度 (N*7)
keypoints_target = detect(pcd_target, search_tree_target, radius)

#获得每一个特征点的描述子，特征向量
pcd_source_keypoints = pcd_source.select_by_index(keypoints_source['id'].values)
fpfh_source_keypoints = o3d.registration.compute_fpfh_feature( pcd_source_keypoints, o3d.geometry.KDTreeSearchParamHybrid(radius=5*radius, max_nn=100)).data #维度 (33*N)

pcd_target_keypoints = pcd_target.select_by_index(keypoints_target['id'].values)
fpfh_target_keypoints = o3d.registration.compute_fpfh_feature(pcd_target_keypoints, o3d.geometry.KDTreeSearchParamHybrid(radius=5*radius, max_nn=100)).data

说明: 通过ISS算法得到源和目标点云的特征点集，维数$(N\times 7)$,其中N为特征点的个数，7为$id,x,y,z,{\lambda }_0,{\lambda }_1,{\lambda }_2$类别。在通过o3d内置的fpfh算法得到每一个特征点的描述子，维数为$(33\times N)$

1.3 特征向量的配准

调用main函数中的ransac_match函数

def ransac_match(pcd_source, pcd_target, feature_source,feature_target,ransac_params, checker_params):
    #确定潜在的匹配关系
    matches = get_potential_matches(feature_source, feature_target)

    #在目标点云上建立搜索树，因为目标点云默认是不动的
    search_tree_target = o3d.geometry.KDTreeFlann(pcd_target)

    N, _ = matches.shape
    idx_matches = np.arange(N)

    T = None
    
    #提议分布的生成器，相当于C++中的迭代器(指针),节省内存，速度更快。
    #物理意义为建立一个指针，指针的每一次指向都指向matches中随机4个匹配
    proposal_generator = (matches[np.random.choice(idx_matches, ransac_params.num_samples, replace=False)] for _ in iter(int, 1))

    validator = lambda proposal: is_valid_match(pcd_source, pcd_target, proposal, checker_params)

    #map(f,[1,2,3,4])等价于并行的执行f(1),f(2),f(3),f(4),其中f为函数
    for T in map(validator, proposal_generator):
        print(T)
        if not (T is None):
            break

    #开线程进行计算。
    # with concurrent.futures.ThreadPoolExecutor(max_workers=ransac_params.max_workers) as executor:
    #     for T in map(validator, proposal_generator):
    #         print(T)
    #         if not (T is None):
    #             break

    .......

说明: 其中get_potential_matches函数就是返回源和目标点云的匹配对应的点云集，具体请见下文。这里作者采用python下的生成器来优化代码，使得运行速度更快，同时也可以开线程并行进行T的求解。

1.3.1 get_potential_matches函数

## 输入
#  feature_source：源点云生成的源特征描述子       是open3d.registration.Feature类型
#  feature_target：目标点云生成的目标特征描述子   是open3d.registration.Feature类型
## 输出
#  match：在KNN的领域内，源点云中的点相对目标点云中的点，可能产生的配准，是numpy.ndarray类型
def get_potential_matches(feature_source, feature_target):
    # 在目标的特征描述子上建立搜索树
    search_tree = o3d.geometry.KDTreeFlann(feature_target)

    # 源的点云中在产生最近领域的匹配
    _, N = feature_source.shape
    matches = []
    for i in range(N):
        query = feature_source[:, i]
        _, idx_nn_target, _ = search_tree.search_knn_vector_xd(query, 1)
        matches.append([i, idx_nn_target[0]])   #上式的1表示只找一个，返回为array形式

    #物理意义为，第一列表示源点云中的特征点的排序，第二列表示对应的目标点云的匹配索引
    matches = np.asarray(matches)
    return matches

说明: 采用open3d下的dsearch_knn_vector_xd方法进行配准，matches返回维度是$(N\times 2)$，其中第一列为源点云中特征点的索引，按照从0-N进行排序，第二列就是源点云中对应的目标点云中的索引，顺序是不固定的。

1.3.2 is_valid_match函数

##功能:使用快速剪枝算法检查提案有效性
##输入： pcd_source 源点云特征点
#       pcd_target 目标点云特征点
#       proposal： 源与目标匹配点云集，为4个
#       checker_params:配置参数
#输出： 变换矩阵T
def is_valid_match(pcd_source, pcd_target,proposal,checker_params ):
    idx_source, idx_target = proposal[:,0], proposal[:,1]

    # TODO: this checker should only be used for pure translation
    if not checker_params.normal_angle_threshold is None:
        # get corresponding normals:
        normals_source = np.asarray(pcd_source.normals)[idx_source]
        normals_target = np.asarray(pcd_target.normals)[idx_target]

        # a. normal direction check:
        normal_cos_distances = (normals_source*normals_target).sum(axis = 1)
        is_valid_normal_match = np.all(normal_cos_distances >= np.cos(checker_params.normal_angle_threshold)) 

        if not is_valid_normal_match:
            return None

    #获取相应的点
    points_source = np.asarray(pcd_source.points)[idx_source]      #维度 4*3
    points_target = np.asarray(pcd_target.points)[idx_target]

    #b.边长比检查
    #pdist表示两行两行之间进行二范数求解
    pdist_source = pdist(points_source)              #维度(6,)
    pdist_target = pdist(points_target)
    #np.all表示数组中是否都为True，只要有一个为False，则返回False
    #np.logical_and表示数组与数组之间进行求与操作，得到的还是一个数组
    is_valid_edge_length = np.all(
        np.logical_and(
            pdist_source > checker_params.max_edge_length_ratio * pdist_target,
            pdist_target > checker_params.max_edge_length_ratio * pdist_source
        )
    )

    if not is_valid_edge_length:
        return None

    #c.快速相似距离的检查
    T = solve_icp(points_source, points_target)
    R, t = T[0:3, 0:3], T[0:3, 3]
    #注意这里就是求||p_source - R*p_target-t||的大小，但是由于p_source,p_target的x,y,z都是放在行上的，故代码和公式不太一样，但意义都一样的。
    deviation = np.linalg.norm(points_target - np.dot(points_source, R.T) - t,axis = 1)
    is_valid_correspondence_distance = np.all(deviation <= checker_params.max_correspondence_distance)

    return T if is_valid_correspondence_distance else None

说明: 通过上述matchs中的随机ransac_params.num_samples组点对(这里参数为4)，采用快速剪枝的算法对变换矩阵做一个粗略的估计，其中关于变换矩阵T的有效性，代码已经给出了详细的注释。

1.4 RANSAC进行提存

def ransac_match(pcd_source, pcd_target, feature_source,feature_target,ransac_params, checker_params):
    ....
    # 设置基础的匹配结果
    print('[RANSAC ICP]: Get first valid proposal. Start registration...')
    best_result = exact_match(pcd_source, pcd_target, search_tree_target,T,ransac_params.max_correspondence_distance, ransac_params.max_refinement)

    # RANSAC:
    num_validation = 0
    for i in range(ransac_params.max_iteration):
        # 生成器的方法，相当于迭代器中的指针自动下移
        T = validator(next(proposal_generator))

        # 检测有效性
        if (not (T is None)) and (num_validation < ransac_params.max_validation):
            num_validation += 1

            #优化所有关键点的估计
            result = exact_match(pcd_source, pcd_target, search_tree_target,T,ransac_params.max_correspondence_distance, ransac_params.max_refinement)
            
            #更新最好的结果
            best_result = best_result if best_result.fitness > result.fitness else result

            if num_validation == ransac_params.max_validation:
                break

    return best_result

说明: 在RANSAC进行提存前，先确定一个基础的匹配关系best_result,注意此时exact_match函数传入的是特征点的集合，并不是滤波后的点云。

1.4.1 exact_match函数

##功能：对给定的点云对执行精确匹配
##输入:pcd_source:源点云，可以特征点，也可以是稠密的点
#      pcd_target:目标点云，可以特征点，也可以是稠密的点
#      T：变换矩阵
#      max_correspondence_distance：源点云与目标点对应的最大距离
#      max_iteration：对应的最大跌打次数
##输出:更加精确的配准结果
def exact_match(pcd_source, pcd_target, search_tree_target,T,max_correspondence_distance, max_iteration):
    N = len(pcd_source.points)

    #评估提前迭代的相对变化
    result_prev = result_curr = o3d.registration.evaluate_registration(pcd_source, pcd_target, max_correspondence_distance, T)

    for _ in range(max_iteration):
        #转换实际上需要进行深度复制，否则结果将是错误的
        pcd_source_current = copy.deepcopy(pcd_source)
        #将源点云通过T变换到目标点云，即Rp+t
        pcd_source_current = pcd_source_current.transform(T)
        
        #发现源和目标对应的匹配关系
        matches = []
        for n in range(N):
            query = np.asarray(pcd_source_current.points)[n]
            _, idx_nn_target, dis_nn_target = search_tree_target.search_knn_vector_3d(query, 1)

            if dis_nn_target[0] <= max_correspondence_distance:
                matches.append([n, idx_nn_target[0]])
        matches = np.asarray(matches)

        if len(matches) >= 4:
            #求解ICP
            P = np.asarray(pcd_source.points)[matches[:,0]]
            Q = np.asarray(pcd_target.points)[matches[:,1]]
            T = solve_icp(P, Q)

            #再次进行评估
            result_curr = o3d.registration.evaluate_registration(pcd_source, pcd_target, max_correspondence_distance, T)

            #如果没有显著改善就提前终止了
            if shall_terminate(result_curr, result_prev):
                print('[RANSAC ICP]: Early stopping.')
                break

    return result_curr

1.5 ICP进行精确的估计

 # 精确估算的ICP，源点云，目标点云，搜索树，变换矩阵
final_result = exact_match(pcd_source, pcd_target, search_tree_target,init_result.transformation,distance_threshold_final, 60)

说明: 注意此时调用的依然是exact_result函数，但此时输入的是滤波后的点云，是相比特征点而言是稠密的点云，并且迭代次数也从30提高到了60。

2.完整代码

关于完整的代码参考大佬的代码，注意在运行大佬的代码时需要安装progressbar这个库，pip安装注意是pip install progressbar2。我在大佬的代码上进行注释和修改，以下就是我的主要的修改的代码，下载路径为自己修改的代码，密码7875。关于数据集的下载，由于数据集较大，请加我QQ:2822902808

2.1main.py

import os
import argparse
import progressbar

import open3d as o3d

# IO utils:
import utils.io as io
import utils.visualize as visualize

# detector:
from detection.iss import detect
# descriptor:
from description.fpfh import describe
# matcher:
from association.ransac_icp import RANSACParams, CheckerParams, ransac_match, exact_match


def main(input_dir, radius, bins, num_evaluations):
    #读取点云bin文件的路径
    registration_results = io.read_registration_results(os.path.join(input_dir, 'reg_result.txt'))

    df_output = io.init_output()

    #progressbar为进度条
    for i, r in progressbar.progressbar(list(registration_results.iterrows())):
        if i >= num_evaluations:
            print ("")
            print ("over interactive")
            #exit(0)
            break
        
        # 获得源和目标的点云索引
        idx_target = int(r['idx1'])
        idx_source = int(r['idx2'])

        # 加载源点云
        pcd_source = io.read_point_cloud_bin(os.path.join(input_dir, 'point_clouds', '{}.bin'.format(idx_source)))
        pcd_source, idx_inliers = pcd_source.remove_radius_outlier(nb_points=4, radius=radius)   #进行滤波
        search_tree_source = o3d.geometry.KDTreeFlann(pcd_source)     #构建KD树

        #加载目标点云
        pcd_target = io.read_point_cloud_bin(os.path.join(input_dir, 'point_clouds', '{}.bin'.format(idx_target)))
        pcd_target, idx_inliers = pcd_target.remove_radius_outlier(nb_points=4, radius=radius)   #进行滤波
        search_tree_target = o3d.geometry.KDTreeFlann(pcd_target)     #构建KD树

        #ISS特征点的提取(id,x,y,z,lambda_0,lambda_1,lambda_2)
        keypoints_source = detect(pcd_source, search_tree_source, radius)   #维度 (N*7)
        keypoints_target = detect(pcd_target, search_tree_target, radius)

        #获得每一个特征点的描述子，特征向量
        pcd_source_keypoints = pcd_source.select_by_index(keypoints_source['id'].values)
        fpfh_source_keypoints = o3d.registration.compute_fpfh_feature( pcd_source_keypoints, o3d.geometry.KDTreeSearchParamHybrid(radius=5*radius, max_nn=100)).data #维度 (33*N)

        pcd_target_keypoints = pcd_target.select_by_index(keypoints_target['id'].values)
        fpfh_target_keypoints = o3d.registration.compute_fpfh_feature(pcd_target_keypoints, o3d.geometry.KDTreeSearchParamHybrid(radius=5*radius, max_nn=100)).data

        # 一些参数的配准
        distance_threshold_init = 1.5 * radius
        distance_threshold_final = 1.0 * radius

        ransac_params = RANSACParams(max_workers=5, num_samples=4, max_correspondence_distance=distance_threshold_init,
                                     max_iteration=200000, max_validation=500, max_refinement=30)

                          #最大对应距离,最大边长 ,法向角阈值
        checker_params = CheckerParams(max_correspondence_distance=distance_threshold_init,max_edge_length_ratio=0.9,normal_angle_threshold=None)
        # RANSAC进行初始化估计，源点云特征点，目标点云特征点，源点云的描述子，目标点云的描述子
        init_result = ransac_match(pcd_source_keypoints, pcd_target_keypoints, fpfh_source_keypoints, fpfh_target_keypoints,ransac_params = ransac_params,checker_params =checker_params)

        # 精确估算的ICP，源点云，目标点云，搜索树，变换矩阵
        final_result = exact_match(pcd_source, pcd_target, search_tree_target,init_result.transformation,distance_threshold_final, 60)

        # 可视化，其中correspondence_set表示源点云与目标点云配准后的点的索引，transformation表示变换矩阵T
        visualize.show_registration_result(pcd_source_keypoints, pcd_target_keypoints, init_result.correspondence_set,pcd_source, pcd_target, final_result.transformation)

        # add result:
        io.add_to_output(df_output, idx_target, idx_source, final_result.transformation)

    # write output:
    io.write_output(os.path.join(input_dir, 'reg_result_yaogefad.txt'),df_output)

def get_arguments():
    """ 
    Get command-line arguments

    """
    # init parser:
    parser = argparse.ArgumentParser("Point cloud registration.")

    # add required and optional groups:
    required = parser.add_argument_group('Required')
    optional = parser.add_argument_group('Optional')

    # add required:
    required.add_argument(
        "-i", dest="input", help="Input path of point cloud registration dataset.",
        required=True
    )
    required.add_argument(
        "-r", dest="radius", help="Radius for nearest neighbor search.",
        required=True, type=float
    )
    required.add_argument(
        "-b", dest="bins", help="Number of feature descriptor bins.",
        required=True, type=int
    )

    # add optional:
    optional.add_argument(
        '-n', dest='num_evaluations', help="Number of pairs to be processed for interactive estimation-evaluation.",
        required=False, type=int, default=3
    )

    # parse arguments:
    return parser.parse_args()


if __name__ == '__main__':
    input="E:\资料\三维点云课程\\3D-Point-Cloud-Analytics-master\\3D-Point-Cloud-Analytics-master\\workspace\\data\\registration_dataset"
    radius=0.5
    bins=11             #参数没有用到
    num_evaluations=6   #取6组数据
    main(input,radius,bins,num_evaluations)

2.2 ransac.py

import collections
import copy
import concurrent.futures

import numpy as np
from scipy.spatial.distance import pdist
#from scipy.spatial.transform import Rotation
import open3d as o3d

# RANSAC configuration:
RANSACParams = collections.namedtuple(
    'RANSACParams',
    [
        'max_workers',
        'num_samples', 
        'max_correspondence_distance', 'max_iteration', 'max_validation', 'max_refinement'
    ]
)

# fast pruning algorithm configuration:
CheckerParams = collections.namedtuple(
    'CheckerParams', 
    ['max_correspondence_distance', 'max_edge_length_ratio', 'normal_angle_threshold']
)

## 输入
#  feature_source：源点云生成的源特征描述子       是open3d.registration.Feature类型
#  feature_target：目标点云生成的目标特征描述子   是open3d.registration.Feature类型
## 输出
#  match：在KNN的领域内，源点云中的点相对目标点云中的点，可能产生的配准，是numpy.ndarray类型
def get_potential_matches(feature_source, feature_target):
    # 在目标的特征描述子上建立搜索树
    search_tree = o3d.geometry.KDTreeFlann(feature_target)

    # 源的点云中在产生最近领域的匹配
    _, N = feature_source.shape
    matches = []
    for i in range(N):
        query = feature_source[:, i]
        _, idx_nn_target, _ = search_tree.search_knn_vector_xd(query, 1)
        matches.append([i, idx_nn_target[0]])   #上式的1表示只找一个，返回为array形式

    #物理意义为，第一列表示源点云中的特征点的排序，第二列表示对应的目标点云的匹配索引
    matches = np.asarray(matches)
    return matches

##功能:进行ICP的求解
##输入:P 源点云数据集
#      Q 目标点云数据集
#输出  T 旋转矩阵
def solve_icp(P, Q):
    # compute centers:
    up = P.mean(axis = 0)
    uq = Q.mean(axis = 0)

    # move to center:
    P_centered = P - up
    Q_centered = Q - uq

    U, s, V = np.linalg.svd(np.dot(Q_centered.T, P_centered), full_matrices=True, compute_uv=True)
    R = np.dot(U, V)
    t = uq - np.dot(R, up)

    # format as transform:
    T = np.zeros((4, 4))
    T[0:3, 0:3] = R
    T[0:3, 3] = t
    T[3, 3] = 1.0

    return T

##功能:使用快速剪枝算法检查提案有效性
##输入：pcd_source 源点云特征点
#       pcd_target 目标点云特征点
#       proposal： 源与目标匹配点云集，为4个
#       checker_params:配置参数
#输出： 变换矩阵T
def is_valid_match(pcd_source, pcd_target,proposal,checker_params ):
    idx_source, idx_target = proposal[:,0], proposal[:,1]

    # TODO: this checker should only be used for pure translation
    if not checker_params.normal_angle_threshold is None:
        # get corresponding normals:
        normals_source = np.asarray(pcd_source.normals)[idx_source]
        normals_target = np.asarray(pcd_target.normals)[idx_target]

        # a. normal direction check:
        normal_cos_distances = (normals_source*normals_target).sum(axis = 1)
        is_valid_normal_match = np.all(normal_cos_distances >= np.cos(checker_params.normal_angle_threshold)) 

        if not is_valid_normal_match:
            return None

    #获取相应的点
    points_source = np.asarray(pcd_source.points)[idx_source]      #维度 4*3
    points_target = np.asarray(pcd_target.points)[idx_target]

    #b.边长比检查
    #pdist表示两行两行之间进行二范数求解
    pdist_source = pdist(points_source)              #维度(6,)
    pdist_target = pdist(points_target)
    #np.all表示数组中是否都为True，只要有一个为False，则返回False
    #np.logical_and表示数组与数组之间进行求与操作，得到的还是一个数组
    is_valid_edge_length = np.all(
        np.logical_and(
            pdist_source > checker_params.max_edge_length_ratio * pdist_target,
            pdist_target > checker_params.max_edge_length_ratio * pdist_source
        )
    )

    if not is_valid_edge_length:
        return None

    #c.快速相似距离的检查
    T = solve_icp(points_source, points_target)
    R, t = T[0:3, 0:3], T[0:3, 3]
    #注意这里就是求||p_source - R*p_target-t||的大小，但是由于p_source,p_target的x,y,z都是放在行上的，故代码和公式不太一样，但意义都一样的。
    deviation = np.linalg.norm(points_target - np.dot(points_source, R.T) - t,axis = 1)
    is_valid_correspondence_distance = np.all(deviation <= checker_params.max_correspondence_distance)

    return T if is_valid_correspondence_distance else None

def shall_terminate(result_curr, result_prev):
    #fitness计算重叠区域（内点对应关系/目标点数）,越高越好。inlier_rmse计算所有内在对应关系的均方根误差RMSE。越低越好。
    relative_fitness_gain = result_curr.fitness / result_prev.fitness - 1

    return relative_fitness_gain < 0.01

##功能：对给定的点云对执行精确匹配
##输入:pcd_source:源点云，可以特征点，也可以是稠密的点
#      pcd_target:目标点云，可以特征点，也可以是稠密的点
#      T：变换矩阵
#      max_correspondence_distance：源点云与目标点对应的最大距离
#      max_iteration：对应的最大跌打次数
##输出:更加精确的配准结果
def exact_match(pcd_source, pcd_target, search_tree_target,T,max_correspondence_distance, max_iteration):
    N = len(pcd_source.points)

    #评估提前迭代的相对变化
    result_prev = result_curr = o3d.registration.evaluate_registration(pcd_source, pcd_target, max_correspondence_distance, T)

    for _ in range(max_iteration):
        #转换实际上需要进行深度复制，否则结果将是错误的
        pcd_source_current = copy.deepcopy(pcd_source)
        #将源点云通过T变换到目标点云，即Rp+t
        pcd_source_current = pcd_source_current.transform(T)
        
        #发现源和目标对应的匹配关系
        matches = []
        for n in range(N):
            query = np.asarray(pcd_source_current.points)[n]
            _, idx_nn_target, dis_nn_target = search_tree_target.search_knn_vector_3d(query, 1)

            if dis_nn_target[0] <= max_correspondence_distance:
                matches.append([n, idx_nn_target[0]])
        matches = np.asarray(matches)

        if len(matches) >= 4:
            #求解ICP
            P = np.asarray(pcd_source.points)[matches[:,0]]
            Q = np.asarray(pcd_target.points)[matches[:,1]]
            T = solve_icp(P, Q)

            #再次进行评估
            result_curr = o3d.registration.evaluate_registration(pcd_source, pcd_target, max_correspondence_distance, T)

            #如果没有显著改善就提前终止了
            if shall_terminate(result_curr, result_prev):
                print('[RANSAC ICP]: Early stopping.')
                break

    return result_curr

## 输入
#  pcd_source：源点云      是open3d.geometry.PointCloud类型
#  pcd_target：目标点云    是open3d.geometry.PointCloud类型
#  feature_source：源点云的特征描述
#  feature_target：目标点云的特征描述
#  ransac_params：RANSAC参数
#  checker_params：快速减枝算法
##输出：
#  best_result：RANSAC ICP的最佳匹配结果，是open3d.registration.RegistrationResult类型
def ransac_match(pcd_source, pcd_target, feature_source, feature_target,ransac_params, checker_params):
    #确定潜在的匹配关系
    matches = get_potential_matches(feature_source, feature_target)

    #在目标点云上建立搜索树，因为目标点云默认是不动的
    search_tree_target = o3d.geometry.KDTreeFlann(pcd_target)

    N, _ = matches.shape
    idx_matches = np.arange(N)

    T = None
    
    #提议分布的生成器，相当于C++中的迭代器(指针),节省内存，速度更快。
    #物理意义为建立一个指针，指针的每一次指向都指向matches中随机4个匹配
    proposal_generator = (matches[np.random.choice(idx_matches, ransac_params.num_samples, replace=False)] for _ in iter(int, 1))

    validator = lambda proposal: is_valid_match(pcd_source, pcd_target, proposal, checker_params)

    #map(f,[1,2,3,4])等价于并行的执行f(1),f(2),f(3),f(4),其中f为函数
    for T in map(validator, proposal_generator):
        print(T)
        if not (T is None):
            break

    #开线程进行计算。
    # with concurrent.futures.ThreadPoolExecutor(max_workers=ransac_params.max_workers) as executor:
    #     for T in map(validator, proposal_generator):
    #         print(T)
    #         if not (T is None):
    #             break

    # 设置基础的匹配结果
    print('[RANSAC ICP]: Get first valid proposal. Start registration...')
    best_result = exact_match(pcd_source, pcd_target, search_tree_target,T,ransac_params.max_correspondence_distance, ransac_params.max_refinement)

    # RANSAC:
    num_validation = 0
    for i in range(ransac_params.max_iteration):
        # 生成器的方法，相当于迭代器中的指针自动下移
        T = validator(next(proposal_generator))

        # 检测有效性
        if (not (T is None)) and (num_validation < ransac_params.max_validation):
            num_validation += 1

            #优化所有关键点的估计
            result = exact_match(pcd_source, pcd_target, search_tree_target,T,ransac_params.max_correspondence_distance, ransac_params.max_refinement)
            
            #更新最好的结果
            best_result = best_result if best_result.fitness > result.fitness else result

            if num_validation == ransac_params.max_validation:
                break

    return best_result

2.3 visualize.py

import numpy as np
import open3d as o3d 


def show_inlier_outlier(cloud, ind):
    """
    Visualize inliers and outliers

    """
    inlier_cloud = cloud.select_by_index(ind)
    outlier_cloud = cloud.select_by_index(ind, invert=True)

    outlier_cloud.paint_uniform_color([1, 0, 0])
    inlier_cloud.paint_uniform_color([0.5, 0.5, 0.5])
    o3d.visualization.draw_geometries([inlier_cloud, outlier_cloud])

#获得点云的最大最小边界
def get_point_cloud_diameter(pcd):
    diameter = np.linalg.norm(pcd.get_max_bound() - pcd.get_min_bound())

    return diameter

##功能:显示所有匹配的结果
##输入:pcd_source_keypoints 源点云的特征点
#      pcd_target_keypoints 目标点云的特征点
#      association 源与目标最好匹配的点云索引
#      pcd_source_dense 滤波后源点云
#      pcd_target_dense 滤波后目标点云
#      transformation 变换矩阵
def show_registration_result(pcd_source_keypoints, pcd_target_keypoints, association,pcd_source_dense, pcd_target_dense, transformation):
    #添加坐标系
    FOR1 = o3d.geometry.TriangleMesh.create_coordinate_frame(size=10, origin=[0, 0, 0])
    # group 01 -- 匹配的结果:
    pcd_source_dense.transform(transformation)

    #将配准的结果移到源位置
    center_source, _ = pcd_source_dense.compute_mean_and_covariance()
    center_target, _ = pcd_target_dense.compute_mean_and_covariance()

    translation = 0.5 * (center_source + center_target)

    pcd_source_dense_centered = pcd_source_dense.translate(-translation)
    pcd_target_dense_centered = pcd_target_dense.translate(-translation)

    #将稠密的源和目标点云进行上色
    pcd_source_dense_centered.paint_uniform_color([1, 0.706, 0])        #黄色
    pcd_target_dense_centered.paint_uniform_color([0, 0.651, 0.929])    #天蓝色

    # group 02 -- 画出关键点:
    # 获取源和目标的直径
    diameter_source = get_point_cloud_diameter(pcd_source_dense)
    diameter_target = get_point_cloud_diameter(pcd_target_dense)

    #移动对应的点云集
    diameter = max(diameter_source, diameter_target)
    pcd_source_keypoints_shifted = pcd_source_keypoints.translate(
        -translation + np.asarray([diameter, -diameter, 0.0])
    )
    pcd_target_keypoints_shifted = pcd_target_keypoints.translate(
        -translation + np.asarray([diameter, +diameter, 0.0])
    )
    #画两个拼接的点云
    pcd_source_keypoints_shifted.paint_uniform_color([1, 0.706, 0])
    pcd_target_keypoints_shifted.paint_uniform_color([0, 0.651, 0.929])

    #画线，取匹配的前20个结果
    association = np.asarray(association)[:20,:]
    N, _ = association.shape
    points = np.vstack(
        (
            np.asarray(pcd_source_keypoints_shifted.points)[association[:, 0]],
            np.asarray(pcd_target_keypoints_shifted.points)[association[:, 1]]
        )
    )
    correspondences = np.asarray(
        [
            [i, i + N] for i in np.arange(N)
        ]
    )
    colors = [
        [0.0, 0.0, 0.0] for i in range(N)
    ]
    correspondence_set = o3d.geometry.LineSet(
        points = o3d.utility.Vector3dVector(points),
        lines = o3d.utility.Vector2iVector(correspondences),
    )
    correspondence_set.colors = o3d.utility.Vector3dVector(colors)

    #特征点显示
    np.asarray(pcd_source_keypoints_shifted.colors)[association[:, 0], :] = [1.0, 0.0, 0.0]
    np.asarray(pcd_target_keypoints_shifted.colors)[association[:, 1], :] = [1.0, 0.0, 0.0]


    o3d.visualization.draw_geometries(
        [   FOR1,
            pcd_source_dense_centered, pcd_target_dense_centered,
            pcd_source_keypoints_shifted, pcd_target_keypoints_shifted,correspondence_set
        ]
    )

3.仿真结果

说明: 上面的蓝色的表示目标特征点集合，黄色的表示源特征点集合，仔细观看，为了方便显示，在代码中只显示了前20个特征点，用红色表示。下面的是滤波后的点云进行配准的结果。