1 基本原理

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

2trimmed ICP在pcl中的实现

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

        inline void
        align (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 correspondences
          pcl::Correspondences full_src_to_tgt (num_source_points), trimmed_src_to_tgt (num_trimmed_source_points);

          // Some variables for the closest point search
          pcl::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 correspondences
            for ( int i = 0 ; i < num_source_points ; ++i )
            {
              // Transform the i-th source point based on the current transform matrix
              aux::transform (guess_and_result, source_points.points[i], transformed_source_point);

              // Perform the closest point search
              kdtree_.nearestKSearch (transformed_source_point, 1, target_index, sqr_dist_to_target);

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