1 基本原理

定义data set（source set，待配准数据）：
P={pi}1Np\mathcal{P}=\left\{\mathbf{p}_{i}\right\}_{1}^{N_{p}}P={pi}1Np
点云数量为NpN_pNp
定义 model set (target set，基准数据)：
M={mi}1Nm\mathcal{M}=\left\{\mathbf{m}_{i}\right\}_{1}^{N_{m}} M={mi}1Nm
点云数量为NmN_mNm。一般情况下，两个点云并不存在100%重叠度，即Np≠NmN_{p} \neq N_{m}Np=Nm。假设重叠度为ξ\xiξ，则重叠点云数量记为Npo=ξNpN_{p o}=\xi N_{p}Npo=ξNp。
假设旋转矩阵RRR平移向量ttt，对待配准点云进行变换：
pi(R,t)=Rpi+t\mathbf{p}_{i}(\mathbf{R}, \mathbf{t})=\mathbf{R} \mathbf{p}_{i}+\mathbf{t} pi(R,t)=Rpi+t
则变换后的待配准点云为:
P(R,t)={pi(R,t)}1Np\quad \mathcal{P}(\mathbf{R}, \mathbf{t})=\left\{\mathbf{p}_{i}(\mathbf{R}, \mathbf{t})\right\}_{1}^{N_{p}} P(R,t)={pi(R,t)}1Np
根据距离定义待配准点云对应在基准点云的最近邻点集合为：（注意这里为1范数，不用平方）
mcl(i,R,t)=arg⁡min⁡m∈M∥m−pi(R,t)∥\mathbf{m}_{c l}(i, \mathbf{R}, \mathbf{t})=\arg \min _{\mathbf{m} \in \mathcal{M}}\left\|\mathbf{m}-\mathbf{p}_{i}(\mathbf{R}, \mathbf{t})\right\| mcl(i,R,t)=argm∈Mmin∥m−pi(R,t)∥
定义individual distances：在一个最近邻匹配点对中，变换后的待配准点与基准点之间的欧式距离，则individual distances：（注意这里为1范数，不用平方）
di(R,t)=∥mcl(i,R,t)−pi(R,t)∥d_{i}(\mathbf{R}, \mathbf{t})=\left\|\mathbf{m}_{c l}(i, \mathbf{R}, \mathbf{t})-\mathbf{p}_{i}(\mathbf{R}, \mathbf{t})\right\| di(R,t)=∥mcl(i,R,t)−pi(R,t)∥
这个individual distance其实就是残差，也就是匹配对的距离。
trimmed ICP主要的贡献就在于利用这个individual distance进行重叠度不是100%的点云配准（即待配准点云和基准点云之间是部分重合）。下面讲一下距离算法，首先给定初值，并给STS′S_{T S}^{\prime}STS′一个很大的值，然后进行如下步骤：
(1)对于待配准点云P\mathcal{P}P中的每一个点而言，寻找其在M\mathcal{M}M的最近邻点（对应点），计算 squared individual distance di2d_i^2di2;
(2)将di2d_i^2di2按照升序（ascending order，从小到大排序），选择前面NpoN_{p o}Npo（重叠数量）个di2d_i^2di2，计算它们的和STSS_{TS}STS;
(3)如果满足终止条件，则退出程序，否则，令STS′=STSS_{T S}^{\prime}=S_{T S}STS′=STS,继续下一步；
(4)利用（2）中筛选出来的前NpoN_{p o}Npo个点云匹配对，（利用最小二乘）求解(R,t)(R,t)(R,t);
(5)利用(R,t)(R,t)(R,t)变换待配准点云P\mathcal{P}P，返回第一步（1）。
本文中给出的终止条件一共有三个：
（1）达到最大迭代次数，终止迭代
（2）定义 trimmed MSE：
e=STSNpoe=\frac{S_{T S}}{N_{p o}} e=NpoSTS
如果eee足够小，终止迭代
（3）如果trimmed MSE之间的改变量∣e′−e∣\left|e^{\prime}-e\right|∣e′−e∣足够小，终止迭代。
上述三种终止方式任选一即可。
总结：trimmedICP对部分重叠点云配准提出一种解决方案，利用了 Least Trimmed Squares（修剪最小二乘）求解ICP问题。文中使用的是点到点的距离度量。其思路很好理解：即在求解旋转和平移量时，删除残差比较大的匹配对（残差越大对求解结果影响越大），文中将残差定义为individual distance。

2trimmed ICP在pcl中的实现

trimmed ICP在PCL的recognition模块中，具体实现在pcl\recognition\ransac_based\的trimmed_icp.h文件中。具体实现代码为align函数：

        inline voidalign (const PointCloud& source_points, int num_source_points_to_use, Matrix4& guess_and_result) const{int num_trimmed_source_points = num_source_points_to_use, num_source_points = static_cast<int> (source_points.size ());if ( num_trimmed_source_points >= num_source_points ){printf ("WARNING in 'TrimmedICP::%s()': the user-defined number of source points of interest is greater or equal to ""the total number of source points. Trimmed ICP will work correctly but won't be very efficient. Either set ""the number of source points to use to a lower value or use standard ICP.\n", __func__);num_trimmed_source_points = num_source_points;}// These are vectors containing source to target correspondencespcl::Correspondences full_src_to_tgt (num_source_points), trimmed_src_to_tgt (num_trimmed_source_points);// Some variables for the closest point searchpcl::PointXYZ transformed_source_point;std::vector<int> target_index (1);std::vector<float> sqr_dist_to_target (1);float old_energy, energy = std::numeric_limits<float>::max ();//          printf ("\nalign\n");do{// Update the correspondencesfor ( int i = 0 ; i < num_source_points ; ++i ){// Transform the i-th source point based on the current transform matrixaux::transform (guess_and_result, source_points.points[i], transformed_source_point);// Perform the closest point searchkdtree_.nearestKSearch (transformed_source_point, 1, target_index, sqr_dist_to_target);// Update the i-th correspondencefull_src_to_tgt[i].index_query = i;full_src_to_tgt[i].index_match = target_index[0];full_src_to_tgt[i].distance = sqr_dist_to_target[0];}// Sort in ascending order according to the squared distancestd::sort (full_src_to_tgt.begin (), full_src_to_tgt.end (), TrimmedICP::compareCorrespondences);old_energy = energy;energy = 0.0f;// Now, setup the trimmed correspondences used for the transform estimationfor ( int i = 0 ; i < num_trimmed_source_points ; ++i ){trimmed_src_to_tgt[i].index_query = full_src_to_tgt[i].index_query;trimmed_src_to_tgt[i].index_match = full_src_to_tgt[i].index_match;energy += full_src_to_tgt[i].distance;}this->estimateRigidTransformation (source_points, *target_points_, trimmed_src_to_tgt, guess_and_result);//            printf ("energy = %f, energy diff. = %f, ratio = %f\n", energy, old_energy - energy, energy/old_energy);}while ( energy/old_energy < new_to_old_energy_ratio_ ); // iterate if enough progress//          printf ("\n");}

讲解一下具体实现：
函数参数：

source_points:待配准点云
num_source_points_to_use：重叠点云数量
guess_and_result：变换矩阵初值

变量old_energy, energy分别对应算法中的STS′，STSS_{T S}^{\prime}，S_{T S}STS′，STS（individual distance 平方之和）
dowhile循环为trimmedICP实现过程：
首先,利用kdtree寻找匹配对，放入full_src_to_tgt,
然后,利用距离平方对匹配点对进行升序排序（sort函数），
然后,筛选前num_source_points_to_use进行真正的匹配对，放入trimmed_src_to_tgt
接着，利用最小二乘求解R,tR,tR,t(estimateRigidTransformation函数(应该是SVD求解，没细看))
直到energy/old_energy大于new_to_old_energy_ratio_(实例化类时设置的参数，默认为0.99)停止迭代，这里迭代终止没有用到原文提到的三种方法，而是PCL自己定义的比值。理想情况下，当二者相等即比例接近1时可以认为终止，不过这种迭代终止条件略显粗糙，可能导致在没有完全收敛时迭代已经终止。

3PCL中trimmedICP的使用

请参考链接1
可能提示找不到aux，需要在trimmed_icp.h加入include<pcl/recognition/ransac_based/auxiliary.h>

trimmed ICP及其在PCL代码解析与使用相关推荐

PCL点云处理之点面ICP配准（附代码，参数设置，实验结果）（六十七）
PCL点云处理之点面ICP配准(附代码,参数设置,实验结果)(六十七) 前言一.点面ICP是什么? 二.使用步骤 1.代码效果总结前言学习点云配准,我辈义不容辞一.点面ICP是什么? 将之 ...
y空间兑换代码_loam代码解析3
本章记录loam的第三部分地图构建,仍然从main函数看起. 首先订阅/laser_cloud_corner_last和/laser_cloud_surf_last特征点云,订阅特征点云对应的全局位姿 ...
hdl_localization代码解析
hdl_localization代码解析简介 hdl_localization是基于UKF滤波框架,融合了ndt点云配准结果,在已经构建的点云地图上实习激光重定位的一种方法.在使用16线激光雷达进行 ...
LOAM学习-代码解析（三）特征点运动估计 laserOdometry
LOAM学习-代码解析(三)特征点运动估计 laserOdometry 前言一.初始化二.去除位移畸变 TransformToStart TransformToEnd 三.去除角度畸变 Plugi ...
matrix_multiply代码解析
matrix_multiply代码解析关于matrix_multiply 程序执行代码里两个矩阵的乘法,并将相乘结果打印在屏幕上. 示例的主要目的是展现怎么实现一个自定义CPU计算任务. 参考:ht ...
CornerNet代码解析——损失函数
CornerNet代码解析--损失函数文章目录 CornerNet代码解析--损失函数前言总体损失 1.Heatmap的损失 2.Embedding的损失 3.Offset的损失前言今天要解 ...
视觉SLAM开源算法ORB-SLAM3 原理与代码解析
来源:深蓝学院,文稿整理者:何常鑫,审核&修改:刘国庆本文总结于上交感知与导航研究所科研助理--刘国庆关于[视觉SLAM开源算法ORB-SLAM3 原理与代码解析]的公开课. ORB-SLA ...
java获取object属性值_java反射获取一个object属性值代码解析
有些时候你明明知道这个object里面是什么,但是因为种种原因,你不能将它转化成一个对象,只是想单纯地提取出这个object里的一些东西,这个时候就需要用反射了. 假如你这个类是这样的: privat ...
python中的doc_基于Python获取docx/doc文件内容代码解析
这篇文章主要介绍了基于Python获取docx/doc文件内容代码解析,文中通过示例代码介绍的非常详细,对大家的学习或者工作具有一定的参考学习价值,需要的朋友可以参考下整体思路: 下载文件并修改后缀 ...
mongoose框架示例代码解析（一）
mongoose框架示例代码解析(一) 参考: Mongoose Networking Library Documentation(Server) Mongoose Networking Librar ...

trimmed ICP及其在PCL代码解析与使用

文章目录

1 基本原理

2trimmed ICP在pcl中的实现

3PCL中trimmedICP的使用

trimmed ICP及其在PCL代码解析与使用相关推荐

最新文章

热门文章