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 T T ，S S S 之间的距离，在二维与三维重建、机器人定位等很多重要应用中都是十分常用的算法，其算法流程可以总结为如下的形式：
- 对于source cloud中的每一点，在target cloud中查找与其距离最近的点，构成最近点对
- 为每一组点对加权，并根据点对之间的距离滤除野值点对
- 如下式所示，最小化目标函数，得到当前的变换矩阵T
T T
T =argmin∑ N i= 1 ( Xi −TY i )TΩ i ( Xi −TY i ) T= ar gmin i=1 ∑ N (X i − T Y i ) T Ω i (X i − T Y i )
其中，N N 表示最近点对的个数，X i X i ，Y i Y i 分别表示的是第i组最近点对中target和source中的对应点，Ω i Ω i 为权重 - 迭代直至收敛或达到最大迭代步数
最原始的icp算法由于几乎每次迭代都需要进行最近点对搜索，算法复杂度极高，同时两组点云中通常有很多最近点对，导致目标函数由很多很多二次型构成，对目标函数的优化也会相应更加耗时。因此原始的icp算法运行十分耗时，且对初始值的选择较为敏感。因此衍生出了很多icp的变种算法，如利用kd树进行加速搜索的icp方法，基于点到线、点到面以及面到面的对应关系等等，且不同的变种在不同的场景下表现也不尽相同。关于各种不同icp变种的比较，可以参考论文
1。
鉴于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等人2提出的一种泛化的icp变种算法，同样用来解决点i云之间的配准问题。
相较于3.1中最原始的icp算法，gicp只是修改了3.1中的目标函数，将概率论的框架移植到了点云配准问题中，接下来，笔者按照自己的理解描述一下gicp算法的思想。
假设有两组已经找好对应关系并去除了野值点对的点云A A 和B B ，即a i a i 对应着b i b i ，i ∈[1,N] i∈ [1,N ] 。假设激光雷达或深度相机的测量没有误差，那么对于正确的变换矩阵T
∗ T ∗ ，存在如下关系：
d i =b i −T ∗ a i =0 d i = b i − T ∗ a i = 0
而gicp算法考虑了激光雷达或深度相机的测量误差，并将其测量点p i p i 建模为在高维分布N ( ˆ p i ,Σ i ) N( p ^ i , Σ i ) 下的采样，其中ˆ p i p ^ i 和 Σ i Σ i 分别为该高斯分布的均值和协方差。因此，由高斯分布的运算法则可以得到如下规律：
即didi同样为高斯分布，且均值为：
ˆ b i − T ∗ ˆ a i = 0 b ^ i − T ∗ a ^ i = 0
方差为：
C B i + ( T ∗ ) C A i ( T ∗ ) T C i B + ( T ∗ ) C i A ( T ∗ ) T
又由极大似然估计(MLE)，可以得到下式：
Unexpected text node: ' ' T ∗ = a
r g T max i = 1 ∏ N p ( d i ( T ) ) = a r g T max i = 1 ∑ N l o g ( p ( d i ( T ) ) )
根据高斯分布的性质，可以将上式简化为如下形式：
Unexpected text node: ' ' T ∗ = a r g T max i = 1 ∑ N d i ( T ) T ( C i B + T C i A T T
) d i ( T )
当 C B i = I , C A i = 0 C i B = I , C i A = 0 时，上式就可以继续化简为标准ICP的形式：
Unexpected text node: ' ' T ∗ = a r g T max i = 1 ∑ N d i ( T ) T d i ( T )
事实上，只需改变 C B i , C A i C i B , C i
A 的值，就可以将目标函数变换成point-to-plane和plane-to-plane的形式，详细的推导过程可以参考文献2.
3.3 ndt
篇幅过长，下一篇再介绍。 ndt(normal distribution transformation)是由
