[筆記]三維激光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中提出的方法，以一個粗糙度ccc值來區分邊緣和平面，然后邊緣點、平面點分別與邊緣線、平面匹配，從而得到一個兩幀之間的LiDAR位姿。

簡單地說，就是提取銳利邊緣特征點和平面特征點，并分別將特征點與邊緣線段和平面曲面片進行匹配。

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

1、坐標系定義

• 激光雷達坐標系L{L}L是3D坐標系，其原點位于激光雷達的幾何中心。 xxx軸指向左側，yyy軸指向上方，zzz軸指向前方。
• 世界坐標系W{W}W是在初始位置與L{L}L一致的3D坐標系。

2、特征點提取

LOAM論文中選擇在銳利邊緣和平面/曲面的特征點。設iii為PkP_kPk?中的一個點，i∈Pki∈P_ki∈Pk?，設SSS為LiDAR在同一掃描中返回的iii的連續點集。定義一個“粗糙度”ccc來評估局部曲面的平滑度，用于邊緣特征點、平面特征點和普通點的分類。
（這個粗糙度可以粗暴的理解為點iii到鄰點的距離之和）
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?)∥∥∥∥∥∥?

• 掃描中的點根據ccc值進行排序，然后用最大ccc值（即邊緣點）和最小ccc值（即平面點）選擇特征點。
• 一次掃描分為四個均等的區域。每個區域中最多可提供2個邊緣點和4個平面點。

兩點約束：

• 不能位于與激光束大致平行的曲面片上
• 不能位于遮擋區域的邊界上
  
  因為這些點通常被認為是不可靠的。

3、特征點匹配

設tkt_ktk?為掃描kkk的開始時間，在開始時，PkP_{k}Pk?是一個空集，在掃描過程中隨著接收到更多點而增加。在每次掃描結束時，在掃描過程中感知到的點云PkP_kPk?重新投影到時間戳tk+1t_{k+1}tk+1?，如上圖所示。然后將重新投影的點云表示為P￣k{\overline P}_{k}Pk?。在下一個掃描k+1k+1k+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{k+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）表示尋找邊緣線作為邊緣特征點的對應關系的過程。設iii為E~k+1，i∈E~k+1{\widetilde E}_{k+1}，i∈{\widetilde E}_{k+1}Ek+1?，i∈Ek+1?中的點。邊緣線由兩點表示。設jjj為iii在P￣k{\overline P}_{k}Pk?中的最近鄰，j∈P￣kj∈{\overline P}_{k}j∈Pk?，設lll為iii在連續兩次掃描中iii的最近鄰。(j，l)(j，l)(j，l)形成了iii的對應關系。然后，為了驗證jjj和lll都是邊緣點，我們根據ccc值檢查局部表面的光滑度。在這里，我們特別要求jjj和lll來自不同的掃描，因為單個掃描不能包含來自同一邊緣線的多個特征點。邊緣線在掃描平面上只有一個例外。但是，如果是這樣，則邊緣線將退化并在掃描平面上顯示為直線，并且邊緣線的特征點不應首先提取。
平面特征點與平面曲面塊對應
上圖（b）表示尋找平面曲面塊作為平面特征點的對應關系的過程。設iii為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?找到iii的最近鄰，表示為jjj。然后，我們找到另外兩個點lll和mmm，它們是iii的最近鄰，一個點與jjj在同一掃描中，另一個在兩次連續掃描中直到jjj的掃描中。這保證了這三個點是非共線的。為了驗證jjj，lll和mmm均為平面點，我們基于ccc值再次檢查局部表面的光滑度。

3.1、計算對應特征的距離

利用找到的特征點的對應關系，現在我們導出表達式以計算從特征點到其對應關系的距離。下面將通過最小化特征點的總距離來估計激光雷達運動。
邊緣特征點距離計算：
對于點i∈E~k+1i∈{\widetilde E}_{k+1}i∈Ek+1?，如果(j，l)(j，l)(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{L}L中點iii，jjj和lll的坐標。
直觀理解：公式的分子是兩個向量的叉乘，而叉乘后求模就變成了求兩個向量構成的三角形的面積。公式的分母是向量的模相當于三角形底邊的長。三角形面積除以一條邊就可以求出該邊到對應頂點的距離，也就是邊角點到邊角線的距離。直觀上的理解如下圖所示：

平面特征點距離計算:
對于點i∈H~k+1i∈{\widetilde H}_{k+1}i∈Hk+1?，如果(j，l，m)(j，l，m)(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{L}L中點mmm的坐標。
直觀理解：公式的分子包括兩部分，上邊是獲得一個向量，下邊也是獲得一個向量，但兩個向量叉乘再取模就表示的求下邊得到三角形面積，上面表示立方體的高，兩者相乘就表示立方體體積。而分母表示立方體底面三角形的面積，得到的高就是平面點到平面的距離。直觀上的理解如下圖所示:

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?相加，我們得到一個非線性函數。

其中fff的每一行對應一個特征點，ddd包含相應的距離。我們計算相對于Tk+1L{T}_{k+1}^{\boldsymbol L}Tk+1L?的fff的雅可比矩陣，記為JJJ，其中J=?f/?Tk+1LJ =?f/?{T}_{k+1}^{\boldsymbol L}J=?f/?Tk+1L?。然后將ddd最小化為零，通過使用高斯-牛頓法（GN法）JTJX=?JTfJ^TJX=-J^TfJTJX=?JTf迭代來求解上式。這里涉及到一個幀間運動的雅克比矩陣JJJ，下面我們來推導一下。

4.1、幀間運動的雅克比J的推導

對于一幀點云數據，有第一個點psp_sps?和最后一個點pep_epe?,以及任意時刻的點ptp_tpt?。
首先，將任意時刻ttt的點重新投影到同一時刻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????
下式用ccc代表coscoscos函數，sss代表sinsinsin函數：
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,rzrx,ry,rzrx,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里程計代碼分析

主要是兩部分：運動補償和幀間配準。

• 利用相鄰兩幀的點云數據進行配準，即完成ttt時刻和t+1t+1t+1時刻點云數據的關聯，并估計LiDAR的相對運動關系。
• 配準工作需要基于邊緣點特征、平面特征點云進行：利用scanRegistration分別獲得ttt時刻和t+1t+1t+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?
以arxarxarx為例：
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?sry?srz?px+crx?crz?sry?py+srx?sry?pz=(-crx*sry*srz*px + crx*crz*sry*py + srx*sry*pz=(?crx?sry?srz?px+crx?crz?sry?py+srx?sry?pz
+tx?crx?sry?srz?ty?crx?crz?sry?tz?srx?sry)?la+ tx*crx*sry*srz - ty*crx*crz*sry - tz*srx*sry) * la+tx?crx?sry?srz?ty?crx?crz?sry?tz?srx?sry)?la
+(srx?srz?px?crz?srx?py+crx?pz+ (srx*srz*px - crz*srx*py + crx*pz+(srx?srz?px?crz?srx?py+crx?pz
+ty?crz?srx?tz?crx?tx?srx?srz)?lb+ ty*crz*srx - tz*crx - tx*srx*srz) * lb+ty?crz?srx?tz?crx?tx?srx?srz)?lb
+(crx?cry?srz?px?crx?cry?crz?py?cry?srx?pz+(crx*cry*srz*px - crx*cry*crz*py - cry*srx*pz+(crx?cry?srz?px?crx?cry?crz?py?cry?srx?pz
+tz?cry?srx+ty?crx?cry?crz?tx?crx?cry?srz)?lc+ tz*cry*srx + ty*crx*cry*crz - tx*crx*cry*srz) * lc+tz?cry?srx+ty?crx?cry?crz?tx?crx?cry?srz)?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的計算。

總結

以上是生活随笔為你收集整理的[笔记]三维激光SLAM学习——LiDAR里程计原理推导代码实现的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。