hdl_graph_slam源码解析（二）

3. scan_matching_odometry

该部分代码主要实现的是根据连续两帧的激光点云数据，进行点云配准，从而获得连续两帧时间内，激光雷达的运动，其主要功能由以下两段函数完成：

Eigen::Matrix4f pose = matching(cloud_msg->header.stamp, cloud);
publish_odometry(cloud_msg->header.stamp, cloud_msg->header.frame_id, pose);

另外在类的初始化函数里面，还有一段代码比较重要：

registration = select_registration_method(pnh);

这段代码是根据.launch文件中的点云配准算法设置，选择对应的点云配准算法，该源码共支持以下几种点云配准算法：icp, gicp, ndt以及它们的openmp加速版本。接下来将分别对这几种算法进行介绍。

3.1 icp

icp是一种非常经典的点云配准算法，即用来最小化两组点云$T$，$S$之间的距离，在二维与三维重建、机器人定位等很多重要应用中都是十分常用的算法，其算法流程可以总结为如下的形式：

• 对于source cloud中的每一点，在target cloud中查找与其距离最近的点，构成最近点对
• 为每一组点对加权，并根据点对之间的距离滤除野值点对
• 如下式所示，最小化目标函数，得到当前的变换矩阵$T$
  $$T=argmin\sum _{i=1}^N(X_i-T{Y}_i{)}^T{\Omega }_i(X_i-T{Y}_i)$$
  其中，$N$表示最近点对的个数，$X_i$，$Y_i$分别表示的是第i组最近点对中target和source中的对应点，${\Omega }_i$为权重
• 迭代直至收敛或达到最大迭代步数

最原始的icp算法由于几乎每次迭代都需要进行最近点对搜索，算法复杂度极高，同时两组点云中通常有很多最近点对，导致目标函数由很多很多二次型构成，对目标函数的优化也会相应更加耗时。因此原始的icp算法运行十分耗时，且对初始值的选择较为敏感。因此衍生出了很多icp的变种算法，如利用kd树进行加速搜索的icp方法，基于点到线、点到面以及面到面的对应关系等等，且不同的变种在不同的场景下表现也不尽相同。关于各种不同icp变种的比较，可以参考论文¹。
鉴于icp算法十分经典，很多开源的点云处理库都会将该算法包括在内，例如pcl，libpointmatcher中都有icp算法及其很多变种算法的集成。
例如在pcl库中可以按照如下代码声明icp配准算法：

boost::shared_ptr<pcl::IterativeClosestPoint<PointT, PointT>> icp(new pcl::IterativeClosestPoint<PointT, PointT>());

3.2 gicp

gicp，全称generalized_icp，是由斯坦福大学的Aleksandr V. Segal与Dirk Haehnel等人²提出的一种泛化的icp变种算法，同样用来解决点i云之间的配准问题。
相较于3.1中最原始的icp算法，gicp只是修改了3.1中的目标函数，将概率论的框架移植到了点云配准问题中，接下来，笔者按照自己的理解描述一下gicp算法的思想。
假设有两组已经找好对应关系并去除了野值点对的点云$A$和$B$，即$a_i$对应着$b_i$，$i\in [1,N]$。假设激光雷达或深度相机的测量没有误差，那么对于正确的变换矩阵$T^{\ast }$，存在如下关系：
$$d_i=b_i-T^{\ast }{a}_i=0$$
而gicp算法考虑了激光雷达或深度相机的测量误差，并将其测量点$p_i$建模为在高维分布$N({\hat{p}}_i,{\Sigma }_i)$下的采样，其中${\hat{p}}_i$和${\Sigma }_i$分别为该高斯分布的均值和协方差。因此，由高斯分布的运算法则可以得到如下规律：
即$d_i$同样为高斯分布，且均值为：
$${\hat{b}}_i-T^{\ast }{\hat{a}}_i=0$$
方差为：
$$C_i^B+(T^{\ast })C_i^A(T^{\ast }{)}^T$$
又由极大似然估计(MLE)，可以得到下式：
$$T^{\ast }=arg\underset{T}{\max }\prod _{i=1}^{N}p(d_i(T))=arg\underset{T}{\max }\sum _{i=1}^{N}log(p(d_i(T)))$$
根据高斯分布的性质，可以将上式简化为如下形式：
$$T^{\ast }=arg\underset{T}{\max }\sum _{i=1}^N{d}_i(T{)}^T(C_i^B+T{C}_i^A{T}^T)d_i(T)$$
当$C_i^B=I,C_i^A=0$时，上式就可以继续化简为标准ICP的形式：
$$T^{\ast }=arg\underset{T}{\max }\sum _{i=1}^N{d}_i(T{)}^T{d}_i(T)$$
事实上，只需改变$C_i^B,C_i^A$的值，就可以将目标函数变换成point-to-plane和plane-to-plane的形式，详细的推导过程可以参考文献2.

3.3 ndt

篇幅过长，下一篇再介绍。
ndt(normal distribution transformation)是由

---

1. François Pomerleau, Colas F , Siegwart R , et al. Comparing ICP variants on real-world data sets[J]. Autonomous Robots, 2013, 34(3):133-148. ↩︎

2. Generalized-ICP ↩︎