简介
trimmed-icp相较于传统的ICP算法，提升了对噪声的鲁棒性，同时允许两个点云集合具有较差的变换参数初始值。由于我只是在某程序中用到了trimmed-icp做点云匹配，没有研究它的代码实现和论文的实验细节部分，而且我的理解可能有很多错误的地方，原汁原味的论文在这里： 《Robust Euclidean alignment of 3D point sets: the trimmed iterative closest point algorithm》。
在介绍trimmed-icp之前我想说说我对ICP算法的一些理解。
ICP（迭代最近点算法）
给定两个点云集合，两个集合中有一部分重合点，ICP的目的是求出这两个点云集合的最优的旋转矩阵 $R$ 和 平移矢量 $t$，很容易举个例子：比如用两个RGB-D相机对同一物体在不同角度下拍摄（当然要有重叠的部分），利用获取的信息生成点云后，我们想把同一时刻的两帧点云拼接在一起，得到比较完整的物体的三维模型，这时候就需要知道这两帧点云之间的变换矩阵，ICP可以帮助我们实现这个目标。
算法概述
可以查看三维点云数据拼接中ICP及其改进算法综述获取更多的了解，下面是我从论文中摘取一些我觉得（对我来说）重要的部分。
ICP算法对待拼接的2片点云，首先根据一定的准则确立对应点集 $P$ 与 $Q$ ，其中对应点对的个数为 $n$ 。然后通过最小二乘法迭代计算最优的坐标变换 ，即旋转矩阵$R$和平移矢量$t$，使得误差函数
$$ E(\boldsymbol{R}, \boldsymbol{t})=\frac{1}{n} \sum_{k=1}^{n}\left|q_{k}-\left(\boldsymbol{R} p_{k}+\boldsymbol{t}\right)\right|^{2} $$
最小。
ICP算法计算简便直观且可使拼接具有较好的精度，但是算法的运行速度以及向全局最优的收敛性却在很大程度上依赖于给定的初始变换估计以及在迭代过程中对应关系的确立。各种粗拼接技术可为ICP算法提供较好的初始位置，所以迭代过程中确立正确对应点集以避免迭代陷入局部极值成为各种改进算法的关键 ，决定了算法的收敛速度与最终的拼接精度。
算法流程
文中将ICP算法流程分为四部分：
1 . 对原始点云数据进行采样
常用的是均匀采样法，随机采样法，法矢采样法，其中第三种鲁棒性最好。
2 . 确定初始对应点集
确定对应点集的方法，点到点 (point—to—point)、点到投影(point—to-projection)和点到面(point—to—surfaee)。
3 . 去除错误对应点对
我粗略看了可以利用 RANSAC 去除错误点，关于RANSAC的介绍戳这里
4 . 坐标变换的求解
ICP采用最小二乘法迭代求解出两个点云集的变换矩阵 $P$ 后，通过SVD分解，单位四元数等方法求出 $R$ 和 $t$ 来。
trimmed-icp
trimmed-icp 和 传统ICP算法最大的不同，在于trim-icp对于由以上前三步得到的对应点，并不是直接采用最小二乘法来拟合误差函数，而是采用了一个叫LTS(the least trimmed squares)的方法来拟合误差函数，LTS的思路是，如果直接采用最小二乘法，选取的对应点集中如果存在少数异常值，最后拟合的误差函数受这些异常值影响较大，那为什么不把这些异常值排除在外呢？怎么做到不把这些异常值算进误差函数里面？trim-icp采用的LTS方法是对每组对应点求得的残差做一个升值排序，只截取前面比例为 $\epsilon$ 的对应点拟合误差函数，然后通过迭代让误差函数最小求解 $R$ 和 $t$ 。在论文的细节部分给出了 $\epsilon$ 的自适应方法，不过我没有看，想了解可以戳文章开始的论文链接。
算法思路
定义：
$$ \mathbf{p}_{i}(\mathbf{R}, \mathbf{t})=\mathbf{R} \mathbf{p}_{i}+\mathbf{t}, \quad \mathscr{P}(\mathbf{R}, \mathbf{t})={\mathbf{p}_{i}(\mathbf{R}, \mathbf{t})}_{1}^{N_{p}} $$
其中$\mathbf{p}_{i}(\mathbf{R},\mathbf{t})$为点云集合$\mathscr{P}$ 中某一点转换到点云集合$\mathscr{U}$的对应点。$\mathscr{P}(\mathbf{R},\mathbf{t})$ 是转换后的对应点的集合。需要为$\mathscr{P}(\mathbf{R},\mathbf{t})$中每一点在$\mathscr{U}$中找到对应点，对应点是这样找的：
$$ \mathbf{m}_{\mathrm{cl}}(i, \mathbf{R}, \mathbf{t})=\arg \min _{\mathbf{m} \in \mathscr{M}}\left|\mathbf{m}-\mathbf{p}_{i}(\mathbf{R}, \mathbf{t})\right| $$
公式说的是距离最近的点为对应点，找到对应点就可以计算每一组对应点的残差了：
$$ d_{i}(\mathbf{R}, \mathbf{t})=\left|\mathbf{m}_{\mathrm{cl}}(i, \mathbf{R}, \mathbf{t})-\mathbf{p}_{i}(\mathbf{R}, \mathbf{t})\right| $$
LTS做的是，对得到的 $d_{i}^{2}(\mathbf{R}, \mathbf{t})$ 做升值排序，然后截取 $N_{\mathrm{po}}=\xi N_{p}$ 个点求和得到误差函数，然后对优化误差函数，最终求解$R$和$t$。
算法流程
1 . 为点集 $\mathscr{P}$ 上每一个点找到它在点集 $\mathscr{U}$ 上的对应点，计算其残差的平方 $d_{i}^{2}$。
2 . 对$d_{i}^{2}$ 做升值排序，选择前面的 $N_{\mathrm{po}}$ 点的对应值，求和得到 $S_{\mathrm{TS}}$。
3 . 如果满足终止条件，退出迭代，否则把$S_{\mathrm{TS}}$设置为上一轮的对应点数目$S_{\mathrm{TS}}^{\prime}$，开始新一轮迭代。
4 . 通过最小化$S_{\mathrm{TS}}$计算一个最优的$(\mathbf{R},\mathbf{t})$。
5 . 由得到的 $(\mathbf{R},\mathbf{t})$做对应点的转换，然后回到第1步。
迭代的终止条件是：
1 . 达到了设定的最大迭代次数。
2 . trimmed MSE $e=\frac{S_{\mathrm{TS}}}{N_{\mathrm{po}}}$足够小了。
3 . trimmed MSE 的前后两次的变换量$\left|e^{\prime}-e\right|$足够小了。
完整的算法流程如上，这样trimmed-icp的大体思路就介绍完了，后面若有机会理解代码实现再补充细节。