这篇文章先根据论文思路分析一下里程计算法原理，代码部分后面再补上。
看这个节点的名字应该就可以推测出来：它的主要功能就是**利用相邻两帧的点云数据进行配准，即完成t时刻和t+1时刻点云数据的关联，并估计lidar的相对运动关系。**我们先回顾一下scanRegistration的工作。它让我们获得了各种特征的点云，那配准工作就可以很简单的进行了：我们利用scanRegistration分别获得t时刻和t+1时刻点云中的特征点，然后建立这两部分点云的一一对应关系，配准工作不就完成了！
- 点云配准
假设你现在已经得到了两坨点云，对他们进行处理之前你首先得保证这些特征点足够多，否则你带了两坨没有任何特征的点，那可就真的爱莫能助了，用程序表达就是设定一个阈值进行判断。
在点云足够多的条件下，终于要开始正式工作了。这里我们设定整个L-M运动估计的迭代次数为25次，以保证运算效率。迭代部分又可分为：对特征边/面上的点进行处理，构建Jaccobian矩阵，L-M运动估计求解。
L-M方法其实就是非线性最小二乘，是Gauss-Newton优化的一种改进（增加了一个阻尼因子，代码中的s），所以关键在于如何把点云配准和运动估计的问题转换为L-M优化求解的问题。主要思路就是：构建约束方程 -> 约束方程求偏导构建Jaccobian矩阵 -> L-M求解。很简单吧！所以不要看代码有千八百行，但实际上也没那么复杂。下面再一步一步来看：关于构建约束方程的问题就是这节标题中提到的点云配准的问题，其基本思想就是从上一帧点云中找到一些边/面特征点，在当前帧点云中同样找这么一些点，建立他们之间的约束关系。
找t+1时刻的某个特征边/面上的点在t时刻下对应的配准点，论文作者给出如上图的思路。特征线：利用KD树找点i在t时刻点云中最近的一点j，并在j周围（上下几条线的范围内）找次近点l，于是我们把（j，l）称为点i在t时刻点云中的对应。特征面：与特征线类似，先找最近点j，在j周围找l，在j周围找m，将（j，l，m）称为点i在t时刻点云中的对应。
实际上这个点云配准过程与ICP算法原理类似，只不过ICP算法是point-to-point，这里是point-to-line和point-to-plane。
- 运动估计
找到不同时刻两堆点云之间的约束关系后，也就是构建好了Jaccobian矩阵之后，将所有对应到的点求到直线的距离到面的距离之和最短然后按照Levenberg-Marquardt算法迭代计算，得到两帧之间的变换，最后通过累计计算odom（假设：雷达的运动是连续的）。
算出了两坨点云间的相对运动，但他们是在这两帧点云的局部坐标系下的，我们需要把它转换到世界坐标系下，因此需要进行转换。
至此我们接完成了整个laserOdometry的计算，剩下的只需要把这些计算结果发布出去就好了。最后贴一张论文里这块内容的伪码图帮助大家更好的理解这块内容的一个框架：
未完待续…后面还需补充的部分有什么是Jaccobian矩阵、如何构建，Levenberg-Marquardt算法。