[笔记]三维激光SLAM学习——LiDAR里程计原理推导&代码实现

• [笔记]三维激光SLAM学习——LiDAR里程计原理推导&代码实现
• 前言
• 一、LiDAR里程计原理
• • 1、坐标系定义
  • 2、特征点提取
  • 3、特征点匹配
  • • 3.1、计算对应特征的距离
  • 4 、用非线性优化方法进行运动估计
  • • 4.1、帧间运动的雅克比J的推导
• 二、LOAM的LiDAR里程计代码分析
• • 1、ROS订阅和发布
  • 2、初始化
  • 3、点云处理——点云配准与运动估计
  • • 3.1、边缘特征上的点配准
    • 3.2、平面特征上的点配准
    • 3.3、构建Jaccobian并进行运动估计求解
  • 4、坐标转换

[笔记]三维激光SLAM学习——LiDAR里程计原理推导&代码实现

前言

最近看回来之前发的博客，发现了埋了很多坑，其中也有很多错误的地方，所以现在重新写一遍，一个个坑补回来。

一、LiDAR里程计原理

简述：目前用得比较多的还是LOAM中提出的方法，以一个粗糙度$c$值来区分边缘和平面，然后边缘点、平面点分别与边缘线、平面匹配，从而得到一个两帧之间的LiDAR位姿。

简单地说，就是提取锐利边缘特征点和平面特征点，并分别将特征点与边缘线段和平面曲面片进行匹配。

参考文献：Zhang J , Singh S . Low-drift and Real-time Lidar Odometry and Mapping[J]. Autonomous Robots, 2017, 41(2):401-416.

1、坐标系定义

• 激光雷达坐标系$L$是3D坐标系，其原点位于激光雷达的几何中心。 $x$轴指向左侧，$y$轴指向上方，$z$轴指向前方。
• 世界坐标系$W$是在初始位置与$L$一致的3D坐标系。

2、特征点提取

LOAM论文中选择在锐利边缘和平面/曲面的特征点。设$i$为PkP_kPk​中的一个点，i∈Pki∈P_ki∈Pk​，设$S$为LiDAR在同一扫描中返回的$i$的连续点集。定义一个“粗糙度”$c$来评估局部曲面的平滑度，用于边缘特征点、平面特征点和普通点的分类。
（这个粗糙度可以粗暴的理解为点$i$到邻点的距离之和）
c=1∣S∣⋅∥X(k,i)L∥∥∑j∈S,j≠i(X(k,i)L−X(k,j)L)∥c=\frac1{\left|\boldsymbol S\right|\cdot\left\|\boldsymbol X_{\left(k,i\right)}^L\right\|}\left\|\sum_{j\in\boldsymbol S,j\neq i}(\boldsymbol X_{(k,i)}^{\boldsymbol L}-\boldsymbol X_{(k,j)}^{\boldsymbol L})\right\|c=∣S∣⋅∥∥∥​X(k,i)L​∥∥∥​1​∥∥∥∥∥∥​j∈S,j​=i∑​(X(k,i)L​−X(k,j)L​)∥∥∥∥∥∥​

• 扫描中的点根据$c$值进行排序，然后用最大$c$值（即边缘点）和最小$c$值（即平面点）选择特征点。
• 一次扫描分为四个均等的区域。每个区域中最多可提供2个边缘点和4个平面点。

两点约束：

• 不能位于与激光束大致平行的曲面片上
• 不能位于遮挡区域的边界上
  
  因为这些点通常被认为是不可靠的。

3、特征点匹配

设tkt_ktk​为扫描$k$的开始时间，在开始时，PkP_{k}Pk​是一个空集，在扫描过程中随着接收到更多点而增加。在每次扫描结束时，在扫描过程中感知到的点云PkP_kPk​重新投影到时间戳tk+1t_{k+1}tk+1​，如上图所示。然后将重新投影的点云表示为P‾k{\overline P}_{k}Pk​。在下一个扫描$k+1$期间，P‾k{\overline P}_{k}Pk​与新接收的点云Pk+1P_{k + 1}Pk+1​一起估计激光雷达的运动。
假设P‾k{\overline P}_{k}Pk​和Pk+1P_{k+1}Pk+1​现在都可用，并开始寻找两个激光雷达点云之间的对应关系。从Pk+1P_{k+1}Pk+1​中找到边缘特征点和平面特征点，设Ek+1E_{k+1}Ek+1​和Hk+1H_{k+1}Hk+1​分别为边缘特征点和平面特征点的集合。我们将从P‾k{\overline P}_{k}Pk​中找边缘线作为Ek+1E_{k+1}Ek+1​中的边缘特征点对应，以及从P‾k{\overline P}_{k}Pk​中找平面块作为Hk+1H_{k+1}Hk+1​中的平面特征点对应。
在扫描$k+1$开始时，Pk+1P_{k+1}Pk+1​是一个空集，在扫描过程中随着接收到更多点而增加。激光雷达里程计递归地估计扫描过程中的6自由度运动，并随着Pk+1P_{k+1}Pk+1​的增加逐渐包含更多的点。在每次迭代中，使用当前估计的变换，将Ek+1E_{k+1}Ek+1​和Hk+1H_{k+1}Hk+1​重新投影到扫描的开始。将重新投影的点集设为E~k+1{\widetilde E}_{k+1}Ek+1​和H~k+1{\widetilde H}_{k+1}Hk+1​。对于E~k+1{\widetilde E}_{k+1}Ek+1​和H~k+1{\widetilde H}_{k+1}Hk+1​中的每个点，我们将在P‾k{\overline P}_{k}Pk​中找到最近的相邻点。这里，P‾k{\overline P}_{k}Pk​存储在3D KDtree中，用于快速索引。

边缘特征点与边缘线对应
上图（a）表示寻找边缘线作为边缘特征点的对应关系的过程。设$i$为E~k+1，i∈E~k+1{\widetilde E}_{k+1}，i∈{\widetilde E}_{k+1}Ek+1​，i∈Ek+1​中的点。边缘线由两点表示。设$j$为$i$在P‾k{\overline P}_{k}Pk​中的最近邻，j∈P‾kj∈{\overline P}_{k}j∈Pk​，设$l$为$i$在连续两次扫描中$i$的最近邻。$(j，l)$形成了$i$的对应关系。然后，为了验证$j$和$l$都是边缘点，我们根据$c$值检查局部表面的光滑度。在这里，我们特别要求$j$和$l$来自不同的扫描，因为单个扫描不能包含来自同一边缘线的多个特征点。边缘线在扫描平面上只有一个例外。但是，如果是这样，则边缘线将退化并在扫描平面上显示为直线，并且边缘线的特征点不应首先提取。
平面特征点与平面曲面块对应
上图（b）表示寻找平面曲面块作为平面特征点的对应关系的过程。设$i$为H~k+1{\widetilde H}_{k+1}Hk+1​中的点，i∈H~k+1i∈{\widetilde H}_{k+1}i∈Hk+1​。平面斑块由三个点表示。与上一部分相似，我们在P‾k{\overline P}_{k}Pk​找到$i$的最近邻，表示为$j$。然后，我们找到另外两个点$l$和$m$，它们是$i$的最近邻，一个点与$j$在同一扫描中，另一个在两次连续扫描中直到$j$的扫描中。这保证了这三个点是非共线的。为了验证$j$，$l$和$m$均为平面点，我们基于$c$值再次检查局部表面的光滑度。

3.1、计算对应特征的距离

利用找到的特征点的对应关系，现在我们导出表达式以计算从特征点到其对应关系的距离。下面将通过最小化特征点的总距离来估计激光雷达运动。
边缘特征点距离计算：
对于点i∈E~k+1i∈{\widetilde E}_{k+1}i∈Ek+1​，如果$(j，l)$是相应的边缘线j，l∈P‾kj，l∈{\overline P}_{k}j，l∈Pk​，则点到线的距离可以计算为

其中，X~(k+1,i)L{\widetilde X}_{(k+1,i)}^{\boldsymbol L}X(k+1,i)L​，X‾(k,j)L{\overline X}_{(k,j)}^{\boldsymbol L}X(k,j)L​和X‾(k,l)L{\overline X}_{(k,l)}^{\boldsymbol L}X(k,l)L​分别是$L$中点$i$，$j$和$l$的坐标。
直观理解：公式的分子是两个向量的叉乘，而叉乘后求模就变成了求两个向量构成的三角形的面积。公式的分母是向量的模相当于三角形底边的长。三角形面积除以一条边就可以求出该边到对应顶点的距离，也就是边角点到边角线的距离。直观上的理解如下图所示：

平面特征点距离计算:
对于点i∈H~k+1i∈{\widetilde H}_{k+1}i∈Hk+1​，如果$(j，l，m)$是对应的平面曲面块j，l，m∈P‾kj，l，m∈{\overline P}_{k}j，l，m∈Pk​，则点到平面的距离为

其中，X‾(k,m)L{\overline X}_{(k,m)}^{\boldsymbol L}X(k,m)L​是$L$中点$m$的坐标。
直观理解：公式的分子包括两部分，上边是获得一个向量，下边也是获得一个向量，但两个向量叉乘再取模就表示的求下边得到三角形面积，上面表示立方体的高，两者相乘就表示立方体体积。而分母表示立方体底面三角形的面积，得到的高就是平面点到平面的距离。直观上的理解如下图所示:

4 、用非线性优化方法进行运动估计

回想一下，上式计算E~k+1{\widetilde E}_{k+1}Ek+1​和H~k+1{\widetilde H}_{k+1}Hk+1​中的点之间的距离及其对应关系。结合边缘特征点距离计算式和上边的式子，我们可以得出Ek+1E_{k + 1}Ek+1​中的边缘点与相应边缘线之间的几何关系，

类似地，结合平面特征点距离计算式和上边的式子，我们可以在Hk+1H_{k + 1}Hk+1​中的一个平面特征点和相应的平面块之间建立另一个几何关系，

最后，利用非线性优化的方法求解激光雷达的运动。对于Ek+1E_{k + 1}Ek+1​和Hk+1H_{k + 1}Hk+1​中的每个特征点fEf_EfE​与fHf_HfH​相加，我们得到一个非线性函数。

其中$f$的每一行对应一个特征点，$d$包含相应的距离。我们计算相对于Tk+1L{T}_{k+1}^{\boldsymbol L}Tk+1L​的$f$的雅可比矩阵，记为$J$，其中J=∂f/∂Tk+1LJ =∂f/∂{T}_{k+1}^{\boldsymbol L}J=∂f/∂Tk+1L​。然后将$d$最小化为零，通过使用高斯-牛顿法（GN法）JTJX=−JTfJ^TJX=-J^TfJTJX=−JTf迭代来求解上式。这里涉及到一个帧间运动的雅克比矩阵$J$，下面我们来推导一下。

4.1、帧间运动的雅克比J的推导

对于一帧点云数据，有第一个点psp_sps​和最后一个点pep_epe​,以及任意时刻的点ptp_tpt​。
首先，将任意时刻$t$的点重新投影到同一时刻tst_sts​得到点p~st{\widetilde p}_{st}p​st​,则得到以下式子
pt=Rs:tp~st+ts:tp_t=R_{s:t}{\widetilde p}_{st}+t_{s:t}pt​=Rs:t​p​st​+ts:t​p~st=Rs:t−1(pt−ts:t){\widetilde p}_{st}=R_{s:t}^{-1}(p_t-t_{s:t})p​st​=Rs:t−1​(pt​−ts:t​)
根据上边的坐标系定义，使用左乘旋转矩阵，旋转坐标轴顺序Z-X-Y得到旋转矩阵RZXYR_{ZXY}RZXY​
Rs:t=RZXY=RryRrxRrzR_{s:t}=R_{ZXY}=R_{ry}R_{rx}R_{rz}Rs:t​=RZXY​=Rry​Rrx​Rrz​=[cosry0sinry010−sinry0cosry][1000cosrx−sinrx0sinrxcosrx][cosrz−sinrz0sinrzcosrz0001]= \begin{bmatrix} cosry& 0& sinry\\ 0& 1& 0\\ -sinry& 0& cosry \end{bmatrix} \begin{bmatrix} 1& 0& 0\\ 0& cosrx& -sinrx\\ 0& sinrx& cosrx \end{bmatrix} \begin{bmatrix} cosrz& -sinrz& 0\\ sinrz& cosrz& 0\\ 0& 0& 1 \end{bmatrix}=⎣⎡​cosry0−sinry​010​sinry0cosry​⎦⎤​⎣⎡​100​0cosrxsinrx​0−sinrxcosrx​⎦⎤​⎣⎡​cosrzsinrz0​−sinrzcosrz0​001​⎦⎤​=[cry∗crz+sry∗srx∗srzcrz∗sry∗srx−cry∗srzcrx∗srycrx∗srzcrx∗crz−srxcry∗srx∗srz−crz∗srycry∗crz∗srx+sry∗srzcry∗crx]= \begin{bmatrix} cry*crz+sry*srx*srz& crz*sry*srx-cry*srz& crx*sry\\ crx*srz& crx*crz& -srx\\ cry*srx*srz-crz*sry& cry*crz*srx+sry*srz& cry*crx \end{bmatrix} =⎣⎡​cry∗crz+sry∗srx∗srzcrx∗srzcry∗srx∗srz−crz∗sry​crz∗sry∗srx−cry∗srzcrx∗crzcry∗crz∗srx+sry∗srz​crx∗sry−srxcry∗crx​⎦⎤​
下式用$c$代表$cos$函数，$s$代表$sin$函数：
Rs:t−1=[cry∗crz−srx∗sry∗srzcry∗srz+srx∗sry∗crz−sry∗crx−crx∗srzcrx∗crzsrxsry∗crz+srx∗cry∗srzsry∗srz−srx∗cry∗crzcry∗crx]R_{s:t}^{-1} =\begin{bmatrix} cry*crz-srx*sry*srz& cry*srz+srx*sry*crz& -sry*crx\\ -crx*srz& crx*crz& srx\\ sry*crz+srx*cry*srz& sry*srz-srx*cry*crz& cry*crx \end{bmatrix}Rs:t−1​=⎣⎡​cry∗crz−srx∗sry∗srz−crx∗srzsry∗crz+srx∗cry∗srz​cry∗srz+srx∗sry∗crzcrx∗crzsry∗srz−srx∗cry∗crz​−sry∗crxsrxcry∗crx​⎦⎤​
对于Rs:t−1R_{s:t}^{-1}Rs:t−1​有Rs:t−1=RZXY−1=RYXZTR_{s:t}^{-1}=R_{ZXY}^{-1}=R_{YXZ}^TRs:t−1​=RZXY−1​=RYXZT​
雅克比计算：
f(p~st)=f(Rs:t−1(pt−ts:t))=f(Rs:t−1,ts:t)=df({\widetilde p}_{st})=f(R_{s:t}^{-1}(p_t-t_{s:t}))=f(R_{s:t}^{-1},t_{s:t})=df(p​st​)=f(Rs:t−1​(pt​−ts:t​))=f(Rs:t−1​,ts:t​)=d
定义：Rs:t−1R_{s:t}^{-1}Rs:t−1​对应的旋转角分别为$rx,ry,rz$即Rs:t−1=RZXY=RryRrxRrzR_{s:t}^{-1}=R_{ZXY}=R_{ry}R_{rx}R_{rz}Rs:t−1​=RZXY​=Rry​Rrx​Rrz​ts:tt_{s:t}ts:t​分量分别为tx,ty,tzt_x,t_y,t_ztx​,ty​,tz​即ts:t=[tx,ty,tz]Tt_{s:t}=[t_x,t_y,t_z]^Tts:t​=[tx​,ty​,tz​]T
进一步得到：∂f∂Tk+1L=∂f∂(rx,ry,rz,tx,ty,tz)=[∂f∂rx,∂f∂ry,∂f∂rz,∂f∂tx,∂f∂ty,∂f∂tz]\frac{\partial f}{\partial T^L_{k+1} }=\frac{\partial f}{\partial (rx,ry,rz,tx,ty,tz)}=[\frac{\partial f}{\partial rx},\frac{\partial f}{\partial ry},\frac{\partial f}{\partial rz},\frac{\partial f}{\partial tx},\frac{\partial f}{\partial ty},\frac{\partial f}{\partial tz}] ∂Tk+1L​∂f​=∂(rx,ry,rz,tx,ty,tz)∂f​=[∂rx∂f​,∂ry∂f​,∂rz∂f​,∂tx∂f​,∂ty∂f​,∂tz∂f​]

式1∂f∂rx=∂f∂p~st∂p~st∂rx\frac{\partial f}{\partial rx }=\frac{\partial f}{\partial {\widetilde p}_{st}} \frac{\partial {\widetilde p}_{st}}{\partial rx}∂rx∂f​=∂p​st​∂f​∂rx∂p​st​​=[la,lb,lc]∂Rs:t−1∂rx（pt−ts:t）=[la,lb,lc] \frac{\partial R_{s:t}^{-1}}{\partial rx }（p_t-t_{s:t}）=[la,lb,lc]∂rx∂Rs:t−1​​（pt​−ts:t​）=[la,lb,lc]∂Rs:t−1∂rx[pxpypz]−[la,lb,lc]∂Rs:t−1∂rx[txtytz]=[la,lb,lc]\frac{\partial R_{s:t}^{-1}}{\partial rx} \begin{bmatrix} px\\py\\pz\\ \end{bmatrix}-[la,lb,lc]\frac{\partial R_{s:t}^{-1}}{\partial rx}\begin{bmatrix} tx\\ty\\tz\\ \end{bmatrix}=[la,lb,lc]∂rx∂Rs:t−1​​⎣⎡​pxpypz​⎦⎤​−[la,lb,lc]∂rx∂Rs:t−1​​⎣⎡​txtytz​⎦⎤​

式2∂f∂tx=∂f∂p~st∂p~st∂tx\frac{\partial f}{\partial tx }=\frac{\partial f}{\partial {\widetilde p}_{st}} \frac{\partial {\widetilde p}_{st}}{\partial tx}∂tx∂f​=∂p​st​∂f​∂tx∂p​st​​=[la,lb,lc]Rs:t−1[−100]=[la,lb,lc]R_{s:t}^{-1}\begin{bmatrix} -1\\0\\0\\ \end{bmatrix}=[la,lb,lc]Rs:t−1​⎣⎡​−100​⎦⎤​
其中∂f∂p~st\frac{\partial f}{\partial {\widetilde p}_{st}}∂p​st​∂f​为梯度方向，对应直线的垂线方向和平面法向量；ptp_tpt​为当前点。
至此，与代码里的arx、ary、arz、atx、aty、atz计算一致。

附录：
旋转矩阵偏导∂Rs:t−1∂rx\frac{\partial R_{s:t}^{-1}}{\partial rx }∂rx∂Rs:t−1​​计算：
∂Rs:t−1∂rx=∂Rzxy∂rx\frac{\partial R_{s:t}^{-1}}{\partial rx }=\frac{\partial R_{zxy}}{\partial rx}∂rx∂Rs:t−1​​=∂rx∂Rzxy​​=∂[cry∗crz−srx∗sry∗srzcry∗srz+srx∗sry∗crz−sry∗crx−crx∗srzcrx∗crzsrxsry∗crz+srx∗cry∗srzsry∗srz−srx∗cry∗crzcry∗crx]∂rx=\frac {\partial \begin{bmatrix} cry*crz-srx*sry*srz& cry*srz+srx*sry*crz& -sry*crx\\ -crx*srz& crx*crz& srx\\ sry*crz+srx*cry*srz& sry*srz-srx*cry*crz& cry*crx \end{bmatrix}} {\partial rx}=∂rx∂⎣⎡​cry∗crz−srx∗sry∗srz−crx∗srzsry∗crz+srx∗cry∗srz​cry∗srz+srx∗sry∗crzcrx∗crzsry∗srz−srx∗cry∗crz​−sry∗crxsrxcry∗crx​⎦⎤​​=[−crx∗sry∗srzcrx∗sry∗crzsrx∗srysrx∗srz−srx∗crzcrxcrx∗cry∗srz−crx∗cry∗crz−cry∗srx]=\begin{bmatrix} -crx*sry*srz& crx*sry*crz& srx*sry\\ srx*srz& -srx*crz& crx\\ crx*cry*srz& -crx*cry*crz& -cry*srx \end{bmatrix} =⎣⎡​−crx∗sry∗srzsrx∗srzcrx∗cry∗srz​crx∗sry∗crz−srx∗crz−crx∗cry∗crz​srx∗srycrx−cry∗srx​⎦⎤​
旋转矩阵偏导∂Rs:t−1∂ry\frac{\partial R_{s:t}^{-1}}{\partial ry }∂ry∂Rs:t−1​​计算：
∂Rs:t−1∂ry=∂Rzxy∂ry\frac{\partial R_{s:t}^{-1}}{\partial ry }=\frac{\partial R_{zxy}}{\partial ry}∂ry∂Rs:t−1​​=∂ry∂Rzxy​​=∂[cry∗crz−srx∗sry∗srzcry∗srz+srx∗sry∗crz−sry∗crx−crx∗srzcrx∗crzsrxsry∗crz+srx∗cry∗srzsry∗srz−srx∗cry∗crzcry∗crx]∂ry=\frac {\partial \begin{bmatrix} cry*crz-srx*sry*srz& cry*srz+srx*sry*crz& -sry*crx\\ -crx*srz& crx*crz& srx\\ sry*crz+srx*cry*srz& sry*srz-srx*cry*crz& cry*crx \end{bmatrix}} {\partial ry}=∂ry∂⎣⎡​cry∗crz−srx∗sry∗srz−crx∗srzsry∗crz+srx∗cry∗srz​cry∗srz+srx∗sry∗crzcrx∗crzsry∗srz−srx∗cry∗crz​−sry∗crxsrxcry∗crx​⎦⎤​​=[−sry∗crz−srx∗cry∗srz−sry∗srz+srx∗cry∗crz−cry∗crx000cry∗crz−srx∗sry∗srzsry∗srz+srx∗sry∗crz−sry∗crx]=\begin{bmatrix} -sry*crz-srx*cry*srz& -sry*srz+srx*cry*crz& -cry*crx \\ 0& 0& 0\\ cry*crz-srx*sry*srz& sry*srz+srx*sry*crz& -sry*crx \end{bmatrix} =⎣⎡​−sry∗crz−srx∗cry∗srz0cry∗crz−srx∗sry∗srz​−sry∗srz+srx∗cry∗crz0sry∗srz+srx∗sry∗crz​−cry∗crx0−sry∗crx​⎦⎤​
旋转矩阵偏导∂Rs:t−1∂rz\frac{\partial R_{s:t}^{-1}}{\partial rz }∂rz∂Rs:t−1​​计算：
∂Rs:t−1∂ry=∂Rzxy∂rz\frac{\partial R_{s:t}^{-1}}{\partial ry }=\frac{\partial R_{zxy}}{\partial rz}∂ry∂Rs:t−1​​=∂rz∂Rzxy​​=∂[cry∗crz−srx∗sry∗srzcry∗srz+srx∗sry∗crz−sry∗crx−crx∗srzcrx∗crzsrxsry∗crz+srx∗cry∗srzsry∗srz−srx∗cry∗crzcry∗crx]∂rz=\frac {\partial \begin{bmatrix} cry*crz-srx*sry*srz& cry*srz+srx*sry*crz& -sry*crx\\ -crx*srz& crx*crz& srx\\ sry*crz+srx*cry*srz& sry*srz-srx*cry*crz& cry*crx \end{bmatrix}} {\partial rz}=∂rz∂⎣⎡​cry∗crz−srx∗sry∗srz−crx∗srzsry∗crz+srx∗cry∗srz​cry∗srz+srx∗sry∗crzcrx∗crzsry∗srz−srx∗cry∗crz​−sry∗crxsrxcry∗crx​⎦⎤​​=[−cry∗srz−srx∗sry∗crzcry∗crz−srx∗sry∗srz0−crx∗crz−crx∗srz0−sry∗srz−srx∗cry∗crzsry∗crz+srx∗cry∗srz0]=\begin{bmatrix} -cry*srz-srx*sry*crz& cry*crz-srx*sry*srz& 0 \\ -crx*crz& -crx*srz& 0\\ -sry*srz-srx*cry*crz& sry*crz+srx*cry*srz& 0 \end{bmatrix} =⎣⎡​−cry∗srz−srx∗sry∗crz−crx∗crz−sry∗srz−srx∗cry∗crz​cry∗crz−srx∗sry∗srz−crx∗srzsry∗crz+srx∗cry∗srz​000​⎦⎤​

二、LOAM的LiDAR里程计代码分析

主要是两部分：运动补偿和帧间配准。

• 利用相邻两帧的点云数据进行配准，即完成$t$时刻和$t+1$时刻点云数据的关联，并估计LiDAR的相对运动关系。
• 配准工作需要基于边缘点特征、平面特征点云进行：利用scanRegistration分别获得$t$时刻和$t+1$时刻点云中的特征点，然后建立这两部分点云的一一对应关系。

一些细节：

• 每帧激光都会参与（所以帧率同VLP16的扫描帧率，10hz）
• 通过对每一帧激光的配准,可以得到一个精度较差的odom；
• 帧间匹配的初始pose可以由IMU得到
• 若没有IMU的时候由匀速运动模型得到

1、ROS订阅和发布

先来看看main函数里怎么写的。

int main(int argc, char** argv)
{ros::init(argc, argv, "laserOdometry");ros::NodeHandle nh;ros::Subscriber subCornerPointsSharp = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 2, laserCloudSharpHandler);ros::Subscriber subCornerPointsLessSharp = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_less_sharp", 2, laserCloudLessSharpHandler);ros::Subscriber subSurfPointsFlat = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_flat", 2, laserCloudFlatHandler);ros::Subscriber subSurfPointsLessFlat = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_less_flat", 2, laserCloudLessFlatHandler);ros::Subscriber subLaserCloudFullRes = nh.subscribe<sensor_msgs::PointCloud2> ("/velodyne_cloud_2", 2, laserCloudFullResHandler);ros::Subscriber subImuTrans = nh.subscribe<sensor_msgs::PointCloud2> ("/imu_trans", 5, imuTransHandler);ros::Publisher pubLaserCloudCornerLast = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 2);ros::Publisher pubLaserCloudSurfLast = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 2);ros::Publisher pubLaserCloudFullRes = nh.advertise<sensor_msgs::PointCloud2> ("/velodyne_cloud_3", 2);ros::Publisher pubLaserOdometry = nh.advertise<nav_msgs::Odometry> ("/laser_odom_to_init", 5);nav_msgs::Odometry laserOdometry;laserOdometry.header.frame_id = "/camera_init";laserOdometry.child_frame_id = "/laser_odom";tf::TransformBroadcaster tfBroadcaster;tf::StampedTransform laserOdometryTrans;laserOdometryTrans.frame_id_ = "/camera_init";laserOdometryTrans.child_frame_id_ = "/laser_odom";std::vector<int> pointSearchInd;//搜索到的点序std::vector<float> pointSearchSqDis;//搜索到的点平方距离PointType pointOri, pointSel/*选中的特征点*/, tripod1, tripod2, tripod3/*特征点的对应点*/, pointProj/*unused*/, coeff;//退化标志bool isDegenerate = false;//P矩阵，预测矩阵cv::Mat matP(6, 6, CV_32F, cv::Scalar::all(0));int frameCount = skipFrameNum;ros::Rate rate(100);bool status = ros::ok();while (status) {ros::spinOnce();if (newCornerPointsSharp && newCornerPointsLessSharp && newSurfPointsFlat && newSurfPointsLessFlat && newLaserCloudFullRes && newImuTrans &&fabs(timeCornerPointsSharp - timeSurfPointsLessFlat) < 0.005 &&fabs(timeCornerPointsLessSharp - timeSurfPointsLessFlat) < 0.005 &&fabs(timeSurfPointsFlat - timeSurfPointsLessFlat) < 0.005 &&fabs(timeLaserCloudFullRes - timeSurfPointsLessFlat) < 0.005 &&fabs(timeImuTrans - timeSurfPointsLessFlat) < 0.005) {  //同步作用，确保同时收到同一个点云的特征点以及IMU信息才进入newCornerPointsSharp = false;newCornerPointsLessSharp = false;newSurfPointsFlat = false;newSurfPointsLessFlat = false;newLaserCloudFullRes = false;newImuTrans = false;/***** 一、初始化 *****//***** 二、点云处理 *****//***** 三、坐标转换 *****/}}

订阅器 ：订阅scanRegistration节点发布的消息并从ROS::Msg类型转换成程序可以处理的pcl点云类型；

• subCornerPointsSharp 订阅 /laser_cloud_sharp 调用laserCloudSharpHandler
• subCornerPointsLessSharp 订阅 /laser_cloud_less_sharp 调用laserCloudLessSharpHandler
• subSurfPointsFlat 订阅 pubSurfPointFlat 调用 laserCloudFlatHandler
• subSurfPointLessFlat 订阅 pubSurfPointLessFlat 调用 laserCloudLessFlatHandler
• subLaserCloudFullRes 订阅 /velodyne_cloud_2 调用laserCloudFullResHandler 处理全部点云数据
• subImuTrans 订阅 /imu_trans 调用imuTransHandler 处理IMU数据

发布器 ：其中/laser_odom_to_init消息的发布频率高，其余三个消息每接收到三次scanRegistration节点的消息才发布一次。

• pubLaserCloudCornerLast 发布 /laser_cloud_corner_last
• pubLaserCloudSurfLast 发布 /laser_cloud_surf_last
• pubLaserCloudFullRes 发布 /velodyne_cloud_3
• pubLaserOdometry 发布 /laser_odom_to_init

总体计算过程分为三步：初始化、点云处理、坐标转换。下面一步一步来

2、初始化

       /********* Initialization ********///将第一个点云数据集发送给laserMapping,从下一个点云数据开始处理if (!systemInited) {//将cornerPointsLessSharp与laserCloudCornerLast交换,目的保存cornerPointsLessSharp的值下轮使用pcl::PointCloud<PointType>::Ptr laserCloudTemp = cornerPointsLessSharp;cornerPointsLessSharp = laserCloudCornerLast;laserCloudCornerLast = laserCloudTemp;//将surfPointLessFlat与laserCloudSurfLast交换，目的保存surfPointsLessFlat的值下轮使用laserCloudTemp = surfPointsLessFlat;surfPointsLessFlat = laserCloudSurfLast;laserCloudSurfLast = laserCloudTemp;//使用上一帧的特征点构建kd-treekdtreeCornerLast->setInputCloud(laserCloudCornerLast);//所有的边沿点集合kdtreeSurfLast->setInputCloud(laserCloudSurfLast);//所有的平面点集合//将cornerPointsLessSharp和surfPointLessFlat点也即边沿点和平面点分别发送给laserMappingsensor_msgs::PointCloud2 laserCloudCornerLast2;pcl::toROSMsg(*laserCloudCornerLast, laserCloudCornerLast2);laserCloudCornerLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);laserCloudCornerLast2.header.frame_id = "/camera";pubLaserCloudCornerLast.publish(laserCloudCornerLast2);sensor_msgs::PointCloud2 laserCloudSurfLast2;pcl::toROSMsg(*laserCloudSurfLast, laserCloudSurfLast2);laserCloudSurfLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);laserCloudSurfLast2.header.frame_id = "/camera";pubLaserCloudSurfLast.publish(laserCloudSurfLast2);//记住原点的翻滚角和俯仰角transformSum[0] += imuPitchStart;transformSum[2] += imuRollStart;systemInited = true;continue;}//T平移量的初值赋值为加减速的位移量，为其梯度下降的方向（沿用上次转换的T（一个sweep匀速模型），同时在其基础上减去匀速运动位移，即只考虑加减速的位移量）transform[3] -= imuVeloFromStartX * scanPeriod;transform[4] -= imuVeloFromStartY * scanPeriod;transform[5] -= imuVeloFromStartZ * scanPeriod;/**********************************************/

主要是等待下一时刻的点云再做处理。

3、点云处理——点云配准与运动估计

这部分是整个laserOdometry节点的重中之重。假设你现在已经得到了两坨点云，对他们进行处理之前你首先得保证这些特征点足够多，否则你带了两坨没有任何特征的点，那可就真的爱莫能助了，用程序表达就是设定一个阈值进行判断。

在点云足够多的条件下，终于要开始正式工作了。这里我们设定整个L-M运动估计的迭代次数为25次，以保证运算效率。迭代部分又可分为：对特征边/面上的点进行处理，构建Jaccobian矩阵，L-M运动估计求解。

      if (laserCloudCornerLastNum > 10 && laserCloudSurfLastNum > 100) {std::vector<int> indices;pcl::removeNaNFromPointCloud(*cornerPointsSharp,*cornerPointsSharp, indices);int cornerPointsSharpNum = cornerPointsSharp->points.size();int surfPointsFlatNum = surfPointsFlat->points.size();//Levenberg-Marquardt算法(L-M method)，非线性最小二乘算法，最优化算法的一种//最多迭代25次for (int iterCount = 0; iterCount < 25; iterCount++) {laserCloudOri->clear();coeffSel->clear();/***** 1. 边缘特征上的点配准并构建Jaccobian *****//***** 2. 平面特征上的点配准并构建Jaccobian  *****//***** 3. L-M运动估计求解 *****/}}

代码中是使用Gauss-Newton优化（相比传统的GN法，代码中增加了一个阻尼因子，代码中的s），这里关键在于如何把点云配准和运动估计的问题转换为优化求解的问题。
主要思路：

1. 构建约束方程
2. 约束方程求偏导构建Jaccobian矩阵
3. 求解

下面再一步一步来看：关于构建约束方程的问题就是这节标题中提到的点云配准的问题，其基本思想就是从上一帧点云中找到一些边/面特征点，在当前帧点云中同样找这么一些点，建立他们之间的约束关系。

3.1、边缘特征上的点配准

          /* 边缘特征匹配 *///处理当前点云中的曲率最大的特征点,从上个点云中曲率比较大的特征点中找两个最近距离点，一个点使用kd-tree查找，另一个根据找到的点在其相邻线找另外一个最近距离的点for (int i = 0; i < cornerPointsSharpNum; i++){TransformToStart(&cornerPointsSharp->points[i], &pointSel);//每迭代五次，重新查找最近点if (iterCount % 5 == 0){std::vector<int> indices;pcl::removeNaNFromPointCloud(*laserCloudCornerLast,*laserCloudCornerLast, indices);//kd-tree查找一个最近距离点，边沿点未经过体素栅格滤波，一般边沿点本来就比较少，不做滤波kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);int closestPointInd = -1, minPointInd2 = -1;//寻找相邻线距离目标点距离最小的点//再次提醒：velodyne是2度一线，scanID相邻并不代表线号相邻，相邻线度数相差2度，也即线号scanID相差2if (pointSearchSqDis[0] < 25){//找到的最近点距离的确很近的话closestPointInd = pointSearchInd[0];//提取最近点线号int closestPointScan = int(laserCloudCornerLast->points[closestPointInd].intensity);float pointSqDis, minPointSqDis2 = 25;//初始门槛值5米，可大致过滤掉scanID相邻，但实际线不相邻的值//寻找距离目标点最近距离的平方和最小的点for (int j = closestPointInd + 1; j < cornerPointsSharpNum; j++){//向scanID增大的方向查找if (int(laserCloudCornerLast->points[j].intensity) > closestPointScan + 2.5){//非相邻线break;}pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) * (laserCloudCornerLast->points[j].x - pointSel.x) + (laserCloudCornerLast->points[j].y - pointSel.y) * (laserCloudCornerLast->points[j].y - pointSel.y) + (laserCloudCornerLast->points[j].z - pointSel.z) * (laserCloudCornerLast->points[j].z - pointSel.z);if (int(laserCloudCornerLast->points[j].intensity) > closestPointScan){//确保两个点不在同一条scan上（相邻线查找应该可以用scanID == closestPointScan +/- 1 来做）if (pointSqDis < minPointSqDis2){//距离更近，要小于初始值5米//更新最小距离与点序minPointSqDis2 = pointSqDis;minPointInd2 = j;}}}//同理for (int j = closestPointInd - 1; j >= 0; j--) {//向scanID减小的方向查找if (int(laserCloudCornerLast->points[j].intensity) < closestPointScan - 2.5){break;}pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) * (laserCloudCornerLast->points[j].x - pointSel.x) + (laserCloudCornerLast->points[j].y - pointSel.y) * (laserCloudCornerLast->points[j].y - pointSel.y) + (laserCloudCornerLast->points[j].z - pointSel.z) * (laserCloudCornerLast->points[j].z - pointSel.z);if (int(laserCloudCornerLast->points[j].intensity) < closestPointScan){if (pointSqDis < minPointSqDis2){minPointSqDis2 = pointSqDis;minPointInd2 = j;}}}}//记住组成线的点序pointSearchCornerInd1[i] = closestPointInd;//kd-tree最近距离点，-1表示未找到满足的点pointSearchCornerInd2[i] = minPointInd2;//另一个最近的，-1表示未找到满足的点}if (pointSearchCornerInd2[i] >= 0){//大于等于0，不等于-1，说明两个点都找到了tripod1 = laserCloudCornerLast->points[pointSearchCornerInd1[i]];tripod2 = laserCloudCornerLast->points[pointSearchCornerInd2[i]];//选择的特征点记为O，kd-tree最近距离点记为A，另一个最近距离点记为Bfloat x0 = pointSel.x;float y0 = pointSel.y;float z0 = pointSel.z;float x1 = tripod1.x;float y1 = tripod1.y;float z1 = tripod1.z;float x2 = tripod2.x;float y2 = tripod2.y;float z2 = tripod2.z;//向量OA = (x0 - x1, y0 - y1, z0 - z1), 向量OB = (x0 - x2, y0 - y2, z0 - z2)，向量AB = （x1 - x2, y1 - y2, z1 - z2）//向量OA OB的向量积(即叉乘)为：//|  i      j      k  |//|x0-x1  y0-y1  z0-z1|//|x0-x2  y0-y2  z0-z2|//模为：float a012 = sqrt(((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))* ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))+ ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))* ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))+ ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))* ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)));//两个最近距离点之间的距离，即向量AB的模float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));//点到线的距离，d = |向量OA 叉乘 向量OB|/|AB|float ld2 = a012 / l12;//AB方向的单位向量与OAB平面的单位法向量的向量积在各轴上的分量（d的方向）//x轴分量ifloat la = ((y1 - y2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))+ (z1 - z2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))) / a012 / l12;//y轴分量jfloat lb = -((x1 - x2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))- (z1 - z2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;//z轴分量kfloat lc = -((x1 - x2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))+ (y1 - y2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;//unusedpointProj = pointSel;pointProj.x -= la * ld2;pointProj.y -= lb * ld2;pointProj.z -= lc * ld2;//权重计算，距离越大权重越小，距离越小权重越大，得到的权重范围<=1float s = 1;if (iterCount >= 5){//5次迭代之后开始增加权重因素 fabs返回 x 的绝对值s = 1 - 1.8 * fabs(ld2);}//考虑权重coeff.x = s * la;coeff.y = s * lb;coeff.z = s * lc;coeff.intensity = s * ld2;if (s > 0.1 && ld2 != 0){//只保留权重大的，也即距离比较小的点，同时也舍弃距离为零的laserCloudOri->push_back(cornerPointsSharp->points[i]);coeffSel->push_back(coeff);}}}

3.2、平面特征上的点配准

          /* 平面特征匹配 *///对本次接收到的曲率最小的点,从上次接收到的点云曲率比较小的点中找三点组成平面;1、一个使用kd-tree查找;2、另外一个在同一线上查找满足要求的;3、第三个在不同线上查找满足要求的for (int i = 0; i < surfPointsFlatNum; i++){TransformToStart(&surfPointsFlat->points[i], &pointSel);if (iterCount % 5 == 0){//kd-tree最近点查找，在经过体素栅格滤波之后的平面点中查找，一般平面点太多，滤波后最近点查找数据量小kdtreeSurfLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1;if (pointSearchSqDis[0] < 25){closestPointInd = pointSearchInd[0];int closestPointScan = int(laserCloudSurfLast->points[closestPointInd].intensity);float pointSqDis, minPointSqDis2 = 25, minPointSqDis3 = 25;for (int j = closestPointInd + 1; j < surfPointsFlatNum; j++){if (int(laserCloudSurfLast->points[j].intensity) > closestPointScan + 2.5){break;}pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) *(laserCloudSurfLast->points[j].x - pointSel.x) +(laserCloudSurfLast->points[j].y - pointSel.y) *(laserCloudSurfLast->points[j].y - pointSel.y) +(laserCloudSurfLast->points[j].z - pointSel.z) *(laserCloudSurfLast->points[j].z - pointSel.z);if (int(laserCloudSurfLast->points[j].intensity) <= closestPointScan){//如果点的线号小于等于最近点的线号(应该最多取等，也即同一线上的点)if (pointSqDis < minPointSqDis2){minPointSqDis2 = pointSqDis;minPointInd2 = j;}}else{//如果点处在大于该线上if (pointSqDis < minPointSqDis3){minPointSqDis3 = pointSqDis;minPointInd3 = j;}}}//同理for (int j = closestPointInd - 1; j >= 0; j--){if (int(laserCloudSurfLast->points[j].intensity) < closestPointScan - 2.5){break;}pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) *(laserCloudSurfLast->points[j].x - pointSel.x) +(laserCloudSurfLast->points[j].y - pointSel.y) *(laserCloudSurfLast->points[j].y - pointSel.y) +(laserCloudSurfLast->points[j].z - pointSel.z) *(laserCloudSurfLast->points[j].z - pointSel.z);if (int(laserCloudSurfLast->points[j].intensity) >= closestPointScan){if (pointSqDis < minPointSqDis2){minPointSqDis2 = pointSqDis;minPointInd2 = j;}}else{if (pointSqDis < minPointSqDis3){minPointSqDis3 = pointSqDis;minPointInd3 = j;}}}}pointSearchSurfInd1[i] = closestPointInd;//kd-tree最近距离点,-1表示未找到满足要求的点pointSearchSurfInd2[i] = minPointInd2;//同一线号上的距离最近的点，-1表示未找到满足要求的点pointSearchSurfInd3[i] = minPointInd3;//不同线号上的距离最近的点，-1表示未找到满足要求的点}if (pointSearchSurfInd2[i] >= 0 && pointSearchSurfInd3[i] >= 0){//找到了三个点tripod1 = laserCloudSurfLast->points[pointSearchSurfInd1[i]];//A点tripod2 = laserCloudSurfLast->points[pointSearchSurfInd2[i]];//B点tripod3 = laserCloudSurfLast->points[pointSearchSurfInd3[i]];//C点//向量AB = (tripod2.x - tripod1.x, tripod2.y - tripod1.y, tripod2.z - tripod1.z)//向量AC = (tripod3.x - tripod1.x, tripod3.y - tripod1.y, tripod3.z - tripod1.z)//向量AB AC的向量积（即叉乘），得到的是法向量//x轴方向分向量ifloat pa = (tripod2.y - tripod1.y) * (tripod3.z - tripod1.z) - (tripod3.y - tripod1.y) * (tripod2.z - tripod1.z);//y轴方向分向量jfloat pb = (tripod2.z - tripod1.z) * (tripod3.x - tripod1.x) - (tripod3.z - tripod1.z) * (tripod2.x - tripod1.x);//z轴方向分向量kfloat pc = (tripod2.x - tripod1.x) * (tripod3.y - tripod1.y) - (tripod3.x - tripod1.x) * (tripod2.y - tripod1.y);float pd = -(pa * tripod1.x + pb * tripod1.y + pc * tripod1.z);//法向量的模float ps = sqrt(pa * pa + pb * pb + pc * pc);//pa pb pc为法向量各方向上的单位向量pa /= ps;pb /= ps;pc /= ps;pd /= ps;//点到面的距离：向量OA与与法向量的点积除以法向量的模float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;//unusedpointProj = pointSel;pointProj.x -= pa * pd2;pointProj.y -= pb * pd2;pointProj.z -= pc * pd2;//同理计算权重float s = 1;if (iterCount >= 5){s = 1 - 1.8 * fabs(pd2) / sqrt(sqrt(pointSel.x * pointSel.x+ pointSel.y * pointSel.y + pointSel.z * pointSel.z));}//考虑权重coeff.x = s * pa;coeff.y = s * pb;coeff.z = s * pc;coeff.intensity = s * pd2;if (s > 0.1 && pd2 != 0) {//保存原始点与相应的系数laserCloudOri->push_back(surfPointsFlat->points[i]);coeffSel->push_back(coeff);}}}

3.3、构建Jaccobian并进行运动估计求解

根据上一部分推导的公式，我们有了这两坨点云的约束方程，构建Jaccobian矩阵就是直接对待估参数X~(k,i)L{\widetilde X}_{(k,i)}^{\boldsymbol L}X(k,i)L​求偏导了。
再看对应程序中如何对应的:
arx=∂f∂rx，ary=∂f∂ry，arz=∂f∂rzarx=\frac{\partial f}{\partial rx }，ary=\frac{\partial f}{\partial ry }，arz=\frac{\partial f}{\partial rz }arx=∂rx∂f​，ary=∂ry∂f​，arz=∂rz∂f​atx=∂f∂tx，aty=∂f∂ty，atz=∂f∂tzatx=\frac{\partial f}{\partial tx }，aty=\frac{\partial f}{\partial ty }，atz=\frac{\partial f}{\partial tz }atx=∂tx∂f​，aty=∂ty∂f​，atz=∂tz∂f​
以$arx$为例：
arx=∂f∂rx=[la,lb,lc]∂Rs:t−1∂rx[pxpypz]−[la,lb,lc]∂Rs:t−1∂rx[txtytz]arx=\frac{\partial f}{\partial rx }=[la,lb,lc]\frac{\partial R_{s:t}^{-1}}{\partial rx} \begin{bmatrix} px\\py\\pz\\ \end{bmatrix}-[la,lb,lc]\frac{\partial R_{s:t}^{-1}}{\partial rx}\begin{bmatrix} tx\\ty\\tz\\ \end{bmatrix}arx=∂rx∂f​=[la,lb,lc]∂rx∂Rs:t−1​​⎣⎡​pxpypz​⎦⎤​−[la,lb,lc]∂rx∂Rs:t−1​​⎣⎡​txtytz​⎦⎤​=[lalblc][−crx∗sry∗srzcrx∗sry∗crzsrx∗srysrx∗srz−srx∗crzcrxcrx∗cry∗srz−crx∗cry∗crz−cry∗srx][px−txpy−typz−tz]=\begin{bmatrix} la& lb& lc \end{bmatrix} \begin{bmatrix} -crx*sry*srz& crx*sry*crz& srx*sry\\ srx*srz& -srx*crz& crx\\ crx*cry*srz& -crx*cry*crz& -cry*srx \end{bmatrix} \begin{bmatrix} px-tx\\ py-ty\\ pz-tz \end{bmatrix}=[la​lb​lc​]⎣⎡​−crx∗sry∗srzsrx∗srzcrx∗cry∗srz​crx∗sry∗crz−srx∗crz−crx∗cry∗crz​srx∗srycrx−cry∗srx​⎦⎤​⎣⎡​px−txpy−typz−tz​⎦⎤​
$=(-crx\ast sry\ast srz\ast px+crx\ast crz\ast sry\ast py+srx\ast sry\ast pz$
$+tx\ast crx\ast sry\ast srz-ty\ast crx\ast crz\ast sry-tz\ast srx\ast sry)\ast la$
$+(srx\ast srz\ast px-crz\ast srx\ast py+crx\ast pz$
$+ty\ast crz\ast srx-tz\ast crx-tx\ast srx\ast srz)\ast lb$
$+(crx\ast cry\ast srz\ast px-crx\ast cry\ast crz\ast py-cry\ast srx\ast pz$
$+tz\ast cry\ast srx+ty\ast crx\ast cry\ast crz-tx\ast crx\ast cry\ast srz)\ast lc$

          int pointSelNum = laserCloudOri->points.size();//满足要求的特征点至少10个，特征匹配数量太少弃用此帧数据if (pointSelNum < 10){continue;}/* LM优化过程 QR分解求解T向量*/cv::Mat matA(pointSelNum, 6, CV_32F, cv::Scalar::all(0));cv::Mat matAt(6, pointSelNum, CV_32F, cv::Scalar::all(0));cv::Mat matAtA(6, 6, CV_32F, cv::Scalar::all(0));cv::Mat matB(pointSelNum, 1, CV_32F, cv::Scalar::all(0));cv::Mat matAtB(6, 1, CV_32F, cv::Scalar::all(0));cv::Mat matX(6, 1, CV_32F, cv::Scalar::all(0));//计算matA,matB矩阵for (int i = 0; i < pointSelNum; i++){pointOri = laserCloudOri->points[i];coeff = coeffSel->points[i];float s = 1;float srx = sin(s * transform[0]);float crx = cos(s * transform[0]);float sry = sin(s * transform[1]);float cry = cos(s * transform[1]);float srz = sin(s * transform[2]);float crz = cos(s * transform[2]);float tx = s * transform[3];float ty = s * transform[4];float tz = s * transform[5];float arx = (-s*crx*sry*srz*pointOri.x + s*crx*crz*sry*pointOri.y + s*srx*sry*pointOri.z+ s*tx*crx*sry*srz - s*ty*crx*crz*sry - s*tz*srx*sry) * coeff.x+ (s*srx*srz*pointOri.x - s*crz*srx*pointOri.y + s*crx*pointOri.z+ s*ty*crz*srx - s*tz*crx - s*tx*srx*srz) * coeff.y+ (s*crx*cry*srz*pointOri.x - s*crx*cry*crz*pointOri.y - s*cry*srx*pointOri.z+ s*tz*cry*srx + s*ty*crx*cry*crz - s*tx*crx*cry*srz) * coeff.z;float ary = ((-s*crz*sry - s*cry*srx*srz)*pointOri.x+ (s*cry*crz*srx - s*sry*srz)*pointOri.y - s*crx*cry*pointOri.z+ tx*(s*crz*sry + s*cry*srx*srz) + ty*(s*sry*srz - s*cry*crz*srx)+ s*tz*crx*cry) * coeff.x+ ((s*cry*crz - s*srx*sry*srz)*pointOri.x+ (s*cry*srz + s*crz*srx*sry)*pointOri.y - s*crx*sry*pointOri.z+ s*tz*crx*sry - ty*(s*cry*srz + s*crz*srx*sry)- tx*(s*cry*crz - s*srx*sry*srz)) * coeff.z;float arz = ((-s*cry*srz - s*crz*srx*sry)*pointOri.x + (s*cry*crz - s*srx*sry*srz)*pointOri.y+ tx*(s*cry*srz + s*crz*srx*sry) - ty*(s*cry*crz - s*srx*sry*srz)) * coeff.x+ (-s*crx*crz*pointOri.x - s*crx*srz*pointOri.y+ s*ty*crx*srz + s*tx*crx*crz) * coeff.y+ ((s*cry*crz*srx - s*sry*srz)*pointOri.x + (s*crz*sry + s*cry*srx*srz)*pointOri.y+ tx*(s*sry*srz - s*cry*crz*srx) - ty*(s*crz*sry + s*cry*srx*srz)) * coeff.z;float atx = -s*(cry*crz - srx*sry*srz) * coeff.x + s*crx*srz * coeff.y- s*(crz*sry + cry*srx*srz) * coeff.z;float aty = -s*(cry*srz + crz*srx*sry) * coeff.x - s*crx*crz * coeff.y- s*(sry*srz - cry*crz*srx) * coeff.z;float atz = s*crx*sry * coeff.x - s*srx * coeff.y - s*crx*cry * coeff.z;float d2 = coeff.intensity;matA.at<float>(i, 0) = arx;matA.at<float>(i, 1) = ary;matA.at<float>(i, 2) = arz;matA.at<float>(i, 3) = atx;matA.at<float>(i, 4) = aty;matA.at<float>(i, 5) = atz;matB.at<float>(i, 0) = -0.05 * d2;}cv::transpose(matA, matAt);matAtA = matAt * matA;matAtB = matAt * matB;//求解matAtA * matX = matAtBcv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);if (iterCount == 0){//特征值1*6矩阵 Mat(int rows, int cols, int type, const Scalar& s)cv::Mat matE(1, 6, CV_32F, cv::Scalar::all(0));//特征向量6*6矩阵cv::Mat matV(6, 6, CV_32F, cv::Scalar::all(0));cv::Mat matV2(6, 6, CV_32F, cv::Scalar::all(0));//求解特征值/特征向量cv::eigen(matAtA, matE, matV);matV.copyTo(matV2);isDegenerate = false;//特征值取值门槛float eignThre[6] = {10, 10, 10, 10, 10, 10};for (int i = 5; i >= 0; i--){//从小到大查找if (matE.at<float>(0, i) < eignThre[i]){//特征值太小，则认为处在兼并环境中，发生了退化for (int j = 0; j < 6; j++) {//对应的特征向量置为0matV2.at<float>(i, j) = 0;}isDegenerate = true;}else{break;}}//计算P矩阵matP = matV.inv() * matV2;}if (isDegenerate){//如果发生退化，只使用预测矩阵P计算cv::Mat matX2(6, 1, CV_32F, cv::Scalar::all(0));matX.copyTo(matX2);matX = matP * matX2;}//累加每次迭代的旋转平移量transform[0] += matX.at<float>(0, 0);transform[1] += matX.at<float>(1, 0);transform[2] += matX.at<float>(2, 0);transform[3] += matX.at<float>(3, 0);transform[4] += matX.at<float>(4, 0);transform[5] += matX.at<float>(5, 0);for(int i=0; i<6; i++){if(isnan(transform[i]))//判断是否非数字transform[i]=0;}//计算旋转平移量，如果很小就停止迭代float deltaR = sqrt(pow(rad2deg(matX.at<float>(0, 0)), 2) +pow(rad2deg(matX.at<float>(1, 0)), 2) +pow(rad2deg(matX.at<float>(2, 0)), 2));float deltaT = sqrt(pow(matX.at<float>(3, 0) * 100, 2) +pow(matX.at<float>(4, 0) * 100, 2) +pow(matX.at<float>(5, 0) * 100, 2));if (deltaR < 0.1 && deltaT < 0.1){//迭代终止条件跳出循环break;}

4、坐标转换

计算出了两坨点云间的相对运动，但他们是在这两帧点云的局部坐标系下的，我们需要把它转换到世界坐标系下，因此需要进行转换。
这部分内容较为简单，直接上代码了：

      float rx, ry, rz, tx, ty, tz;//求相对于原点的旋转量,垂直方向上1.05倍修正?AccumulateRotation(transformSum[0], transformSum[1], transformSum[2], -transform[0], -transform[1] * 1.05, -transform[2], rx, ry, rz);float x1 = cos(rz) * (transform[3] - imuShiftFromStartX) - sin(rz) * (transform[4] - imuShiftFromStartY);float y1 = sin(rz) * (transform[3] - imuShiftFromStartX) + cos(rz) * (transform[4] - imuShiftFromStartY);float z1 = transform[5] * 1.05 - imuShiftFromStartZ;float x2 = x1;float y2 = cos(rx) * y1 - sin(rx) * z1;float z2 = sin(rx) * y1 + cos(rx) * z1;//求相对于原点的平移量tx = transformSum[3] - (cos(ry) * x2 + sin(ry) * z2);ty = transformSum[4] - y2;tz = transformSum[5] - (-sin(ry) * x2 + cos(ry) * z2);//根据IMU修正旋转量PluginIMURotation(rx, ry, rz, imuPitchStart, imuYawStart, imuRollStart, imuPitchLast, imuYawLast, imuRollLast, rx, ry, rz);//得到世界坐标系下的转移矩阵transformSum[0] = rx;transformSum[1] = ry;transformSum[2] = rz;transformSum[3] = tx;transformSum[4] = ty;transformSum[5] = tz;//欧拉角转换成四元数geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw(rz, -rx, -ry);//publish四元数和平移量laserOdometry.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);laserOdometry.pose.pose.orientation.x = -geoQuat.y;laserOdometry.pose.pose.orientation.y = -geoQuat.z;laserOdometry.pose.pose.orientation.z = geoQuat.x;laserOdometry.pose.pose.orientation.w = geoQuat.w;laserOdometry.pose.pose.position.x = tx;laserOdometry.pose.pose.position.y = ty;laserOdometry.pose.pose.position.z = tz;pubLaserOdometry.publish(laserOdometry);

至此我们接完成了整个laserOdometry的计算。