(七)三维点云课程—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 × 7 ) (N \times 7) (N×7),其中N为特征点的个数，7为 i d , x , y , z , λ 0 , λ 1 , λ 2 id,x,y,z,\lambda_0,\lambda_1,\lambda_2 id,x,y,z,λ0​,λ1​,λ2​类别。在通过o3d内置的fpfh算法得到每一个特征点的描述子，维数为 ( 33 × N ) (33 \times N) (33×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.shapeidx_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.shapematches = []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 × 2 ) (N \times 2) (N×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 translationif 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*3points_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 = 0for 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 resultif num_validation == ransac_params.max_validation:breakreturn 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+tpcd_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:#求解ICPP = 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.')breakreturn 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 progressbarimport 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_matchdef 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 * radiusdistance_threshold_final = 1.0 * radiusransac_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表示变换矩阵Tvisualize.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.5bins=11             #参数没有用到num_evaluations=6   #取6组数据main(input,radius,bins,num_evaluations)

2.2 ransac.py

import collections
import copy
import concurrent.futuresimport 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.shapematches = []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 - upQ_centered = Q - uqU, 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] = RT[0:3, 3] = tT[3, 3] = 1.0return 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 translationif 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*3points_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 Nonedef shall_terminate(result_curr, result_prev):#fitness计算重叠区域（内点对应关系/目标点数）,越高越好。inlier_rmse计算所有内在对应关系的均方根误差RMSE。越低越好。relative_fitness_gain = result_curr.fitness / result_prev.fitness - 1return 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+tpcd_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:#求解ICPP = 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.')breakreturn 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.shapeidx_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 = 0for 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 resultif num_validation == ransac_params.max_validation:breakreturn 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.shapepoints = 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个特征点，用红色表示。下面的是滤波后的点云进行配准的结果。

(七) 三维点云课程---ICP应用相关推荐

1. (一) 三维点云课程---PCA介绍

   三维点云课程-PCA介绍 三维点云课程---PCA介绍 三维点云课程---PCA介绍 1. 什么是PCA 2.知识铺垫 2.1 SVD分解(奇异值分解 ) 2.1 谱定理 2.2 Rayleigh商 ...

2. (四) 三维点云课程---PointNet-Pytorch运行

   三维点云课程-PointNet-Pytorch运行 三维点云课程---PointNet-Pytorch运行 三维点云课程---PointNet-Pytorch运行 1.分类---Classificat ...

3. 三维点云课程（七）——特征点描述

   1. 什么是特征点 1.1 图像特征点 ORB slam 1.2 点云特征点 点云配准 :ICP要求有足够好的初始平移旋转矩阵,且有一定的重合率 2. 怎么提取特征点 2.1 图像提取特征点 2.1. ...

4. 深蓝学院的三维点云课程：第一章

   一.前言 为什么现在点云应用这么广泛,就是因为他有深度信息. 像人脸识别用来解锁手机,比如Iphnoe手机在前边有一个深度摄像头,所以它产生的点云真的是一个三维点云:然后很多手机他可能就没有深度摄像头 ...

5. 三维点云课程（六）——三维目标检测

   目录 1.  图像目标检测 ​ 1.1 评价检测好坏 1.2 物体检测的方法 1.2.1 Two-Stage 1.2.2 One-Stage 2. 点云目标检测 2.1 VoxelNet ​2.2 P ...

6. 三维点云课程第一章：应用

   核PCA的核心思想就是我们可以把高维空间的运算转化成低维空间的一个核函数,这个思想叫做Kernel track.也就是说我们在学习SVM支持向量机的时候,也会经常的用到这个核的方法.  应用1:如何去 ...

7. 三维点云学习（9）5-实现RANSAC Registration配准

   三维点云学习(9)5-实现RANSAC Registration配准 参考博客: 机器视觉之 ICP算法和RANSAC算法 三维点云配准 ICP点云配准原理及优化 本章因个人能力有限,大部分代码摘自g ...

8. azure kinect三维点云_万众期待的 【三维点云处理】 课程来啦！

   三维点云是最重要的三维数据表达方式之一. 从技术角度看,SLAM.三维重建.机器人感知等领域,点云都是最简单且最普遍的表达方式:相对于图像,点云有其不可替代的优势-深度,也就是说三维点云直接提供了三维 ...

9. 干货 | 三维点云配准：ICP 算法原理及推导

   编者荐语 点云配准可以分为粗配准(Coarse Registration)和精配准(Fine Registration)两步.粗配准指的是在两幅点云之间的变换完全未知的情况下进行较为粗糙的配准,目的主 ...

最新文章

1. 设计模式：状态模式（State Pattern）
2. MySQL优化篇：IN VS EXISTS
3. ORACLE 几个我忍了他很多年的问题
4. 聚能聊每周精选 第二十三期
5. 获取日志$6到$NF的字段
6. 【Util】 时间天数增加，时间比较。
7. powershell XML操作
8. QRCode二维码生成方案及其在带LOGO型二维码中的应用（2）
9. java itext word_Java使用iText生成word文件的解决方案 | 学步园
10. Unity C# 设计模式（五）建造者模式
11. 云服务器如何选型？可以从这几个方面来考虑
12. 2018.12.12 第九章虚拟内存
13. Android酷炫有用的开源框架
14. 编码基本功：工作中，大多数人不会举一反三
15. 云原生是一个时代下践行者们的故事
16. Windows勒索病毒补丁下载
17. MySQL数据库基础
18. “本是青灯不归客，却因浊酒留风尘，星光不问赶路人，岁月不负有心人”，你是怎么理解的？
19. word将空格替换为逗号
20. 负载均衡器-Citrix

热门文章

1. 《Mybatis 手撸专栏》第9章：细化XML语句构建器，完善静态SQL解析
2. LM小型可编程控制器软件（基于CoDeSys）笔记二十：plc通过驱动器控制步进电机
3. 一个离轴抛物面反射镜的几何场追迹
4. C#中线程和进程的区别
5. Java EE与Jakarta EE区别
6. 三步使用cpolar发布一个独立网站（windows版）
7. 触发器及触发器的作用
8. 新安装的Ubuntu20.04 5.13上没有WIFI 看这一篇就够了
9. macOS 无法读取移动硬盘（不使用chkdsk，有一台linux时）
10. 苹果笔记本包_扔掉丑不拉几电脑包！这款笔记本包轻薄、商务范，随身带没负担！...