LOAM 原理及代碼實現介紹

paper:《Lidar Odometry and Mapping in Real-time》
LOAM的參考代碼鏈接：
A-LOAM
A-LOAM-Notes
LOAM-notes

使用傳感器介紹：

如果沒有電機旋轉，則雷達自身的掃描是一個平面的180度。如下：

加上電機的旋轉，則該設備能掃描的范圍為雷達前方的半球形。

LOAM技術點

• 二維雷達固定在一個轉軸上，實現3維雷達；一次完整的三維掃描為sweep，雷達平面的一次掃描為scan；
• 將點云分為兩類：邊線點(edge point角點)和平面點(planar point)，分別對應采取點到線及點到面的約束；在實際的測試中，平面點占總點云的80%。
• 位姿變化估計采用高頻粗估計+低頻優化。兩套算法來實現定位。(laser odometry & mapping odometry)
  其中，laser odometry高頻粗估計是在雷達坐標系下，通過點到線及點到面距離，利用LM得到雷達位姿估計；
  mapping odometry低頻位姿精估計，是在世界坐標系下，通過點到線，點到面的距離，得到世界坐標系下的雷達位姿估計優化。
• 低頻位姿精估計是1HZ，與sweep頻率相同。也就是一次完整的sweep會進行一次完整的位姿估計。

LOAM整體框架

將定位與建圖分開運行，高頻位姿估計+低頻優化建圖->實現實時，低計算量，低漂移。

LOAM主要包括四個節點：點云提取(scan registration)、雷達里程計(lidar odometry)、雷達建圖(lidar mapping) 和位姿集成(transform integration)

1. 點云提取及處理（scan registration）：

   根據點的曲率c來將點劃分為不同的類別（邊/面特征或不是特征），在每一個sweep中，根據曲率對點進行排序，來作為評價局部表面平滑性的標準。
   一個sweep指完成一次完整的掃描，一次sweep分為多個scan，每一次sweep的雷達位姿定義為為這一sweep起始時的狀態。

   特征點提取及處理
   LOAM中選用特征點來進行運動估計，特征點選取(edge point角點)和平面點(planar point)，如下黃點為edge point，紅點為planar point。試驗數據表明，大部分點云都是平面點。邊線點起到的約束作用較小。
   
   判斷公式：
   
   P k P_k Pk?第k次整體sweep中得到的點云。 X ( k , i ) L X^L_{(k,i)} X(k,i)L?表示第k個sweep中的i點在雷達坐標系下的坐標。

   設 i ∈ P k i\in P_k i∈Pk?，表示點 i i i是第k次整體sweep中的點。選取 i i i點在同一個scan中相鄰的前后的5個點 X ( k , j ) L , j ∈ S , j ≠ i X^L_{(k,j)}, j \in S, j\ne i X(k,j)L?,j∈S,j?=i，計算 Σ j X ( k , i ) L ? X ( k , j ) L \Sigma _jX^L_{(k,i)}-X^L_{(k,j)} Σj?X(k,i)L??X(k,j)L?。注意，論文中使用的是平面雷達，每個scan掃描區域是半平面，即，空間的平面，雷達一次scan會得到空間平面與雷達掃描平面交線上的點，會得到一條線的點，但是，如果雷達掃過角點，則就是類似平面v字型的點。）
   即：
   如果是平面，前后的5個點與i的距離相加=0，c->0
   如果是邊線角點，則 c > 0 c>0 c>0
   c的計算方程的分母，用于歸一化c值，防止，因為雷達的距離而帶來的c值得過大或小。

   可以參考LOAM論文和程序代碼的解讀中更詳細的解釋。

• 代碼文件：scanRegistration.cpp：
  接收雷達驅動發布的點云，解析驅動點云，計算曲率，并按序排布，按照曲率由高到低，將點云分為sharp，less_sharp，flat，less_flat四種點并發布對應的消息。
  雷達驅動發布的點云topic為：/velodyne_points，其中點云的位置是以x,y,z存儲的。根據xyz計算點云的仰角(angle)，及偏角(ori)，并對應的計算出點云對應的線號，存放在point.intensity = scanID + scanPeriod * relTime。其中scanID是整數，為點的線數(0-15)，relTime代表水平偏轉度(0-1) scanPeriod=0.1。即將偏轉度存為歸一化的小數。線束為整數。
  從而每個點包含的數據包括x,y,z,intensity，intensity則存放是豎直上的線束及水平上的偏轉度。也就是包含位置信息的同時，包含了仰角及偏轉角信息。

2. 位姿粗估計(雷達里程計(lidar odometry))：（高頻）
   符號定義：
   P k P_k Pk?第k次整體sweep中得到的點云；
   根據插值將 P k P_k Pk?映射到k時刻sweep起始坐標系下(點云去掉運動漂移)，記為 P k ~ \widetilde{P_k} Pk?
   ?，則根據插值將 P k + 1 P_{k+1} Pk+1?映射到k+1時刻sweep起始坐標系下(點云去掉運動漂移)，記為 P k + 1 ~ \widetilde{P_{k+1}} Pk+1?
   ?;
   將第k幀掃描到的特征點 P k P_k Pk?映射到第k+1幀的雷達坐標系下，記為 P k + 1 ~ \widetilde{P_{k+1}} Pk+1?
   ?.

   點云匹配：
   該作者的點云匹配是基于將 P k + 1 ~ \widetilde{P_{k+1}} Pk+1?
   ?和 P k + 1 ~ \widetilde{P_{k+1}} Pk+1?
   ?進行匹配。
   即將第k個sweep的點云映射到第k+1個sweep起始，將第k+1個sweep的點云映射到第k+1個sweep的起始。匹配 P k + 1 ~ \widetilde{P_{k+1}} Pk+1?
   ?和 P k + 1 ~ \widetilde{P_{k+1}} Pk+1?
   ?的點云點面關系，計算得到第k+1個sweep中產生的運動。

   點云特征點被分為，邊線點和平面點。
   邊線約束點兩個點確定：設去漂當前幀的點云 i ∈ ξ k + 1 ~ i\in\widetilde{\xi_{k+1}} i∈ξk+1?
   ?，在上一幀點云(映射在k+1時刻坐標系下) ξ k ￣ \overline{\xi_k} ξk??中找到距離最近的點 j j j，并在 ξ k ￣ \overline{\xi_k} ξk??找到與j相鄰scan的點 l l l。
   設想兩個墻面的豎直墻角線，設雷達偏移較小，相鄰的的scan，會出現類似圖a的情況，相鄰scan會測量到墻角線上的點。因為可以在上一幀中找到兩個相鄰幀的墻角線上的點，約束當前幀打到墻角上的點到這兩個點構成的直線上的距離最小，得到雷達運動T。如下圖a所示。

   平面約束點三個點確定：設去漂當前幀的點云 i ∈ H k + 1 ~ i\in\widetilde{\mathcal{H}_{k+1}} i∈Hk+1?
   ?，在上一幀點云(映射在k+1時刻坐標系下) H k ￣ \overline{\mathcal{H}_k} Hk??中找到距離最近的點 j j j，并在 H k ￣ \overline{\mathcal{H}_k} Hk??找到與j同scan的最近點 l l l和相鄰scan的一個點 m m m。

   論文中的雷達是二維平面雷達加旋轉得到，每一次scan都是一次平面半圓，一個sweep由加在雷達上的電機轉動180度，構成半球。一次scan打到對面一個平面上，就是一排直線排列的點。

   橙色線表示點 j j j所在的scan，藍色線表示與橙色線相鄰的兩次scan的線。使用velonedy雷達，每個FOV對應的多線構成一個豎直scan，但是點云約束類似。
   

   使用kdtree存儲點云信息。

   將第k幀掃描到的特征點 P k P_k Pk?映射到第k+1幀的雷達坐標系下，記為 P k ￣ \overline{P_k} Pk??：，將 P k ￣ \overline{P_k} Pk??與第k+1幀的掃描點云 P k + 1 P_{k+1} Pk+1?進行點云匹配。在LOAM代碼中，計算 P k ￣ \overline{P_k} Pk??是使用TransformToEnd()將 P k {P_k} Pk?映射到本k時刻sweep的結束，即k+1時刻sweep的開始來得到的。
   而當前時刻k+1時刻的點云映射到k+1時刻的初始是使用TransformToStart()實現。
   假設k+1時刻的sweep中與前一k時刻的sweep的運動一致 T k + 1 L T^L_{k+1} Tk+1L?。使用位置插值方法得到整個sweep點云對應的 T k + 1 , i L T^L_{k+1,i} Tk+1,iL?，從而利用TransformToStart()，TransformToEnd()可將點云映射到sweep初始和結束時的坐標系下。

   位姿插值
   論文中，作者采用的是二維雷達加了一個電機旋轉，每一次scan得到的點云的xyz是基于雷達的自身的坐標系，就是已經旋轉后的雷達坐標系。
   設雷達勻速旋轉及勻速偏移，通過插值的方法，得到一個完整的sweep中，每一個scan對應的T(q，t)，并將點云坐標變換到每一次的sweep初始坐標系。消除點云在sweep中的因雷達運動而產生的誤差，得到undistorted 的點云 P ~ k \widetilde P_k P
   k?。
   詳細實現在laserOdometry.cpp TransformToStart函數中。
   

   對于邊線點 i ∈ P k + 1 i\in P_{k+1} i∈Pk+1?，對應找到 P k ￣ \overline{P_k} Pk??相鄰scan中最近的中兩個點(j, l)，則兩個點則確定了邊線，計算變換矩陣T能使，點i到邊線(j, l)的距離最小。j為最近點，然后再j所在scan的相鄰scan中，找到l點。

   距離約束：
   點到線的距離計算公式如下：原理是目標點到兩個原始點組成的兩個向量構成的平行四邊形面積/底邊長度。
   
   對于平面點 i ∈ P k + 1 i\in P_{k+1} i∈Pk+1?，對應找到 P k ~ \widetilde{P_k} Pk?
   ?相鄰scan中最近的中三個點(j, l，m)，則三個點則確定了平面，計算變換矩陣T能使，點i到平面的距離(j, l，m)最小。
   
   點到面距離計算公式如下：原理：目標點到三個原始點組成的三個向量構成的斜方體/底面面積。
   分子：第二行兩個向量的叉乘結果值等于j，l，m三個點構成的三角形面積，方向垂直于平面向上。
   結果點乘分子第一行，則可以得到i，j，l，m構成的斜三棱柱的體積。
   分母：等同于分子第二行。
   如下圖所示：
   即分別對應了點云匹配中的點到線及點到面的算法。

   向量a×向量b=
   　　| i j k |
   　　|a1 b1 c1|
   　　|a2 b2 c2|
   　　=(b1c2-b2c1，c1a2-a1c2，a1b2-a2b1)

距離公式解釋參見LOAM 論文及原理分析

這里列出平面點的距離約束計算對應代碼：

// tripod1，tripod2，tripod3對應公式的j,l,m三點
tripod1 = _lastSurfaceCloud->points[_pointSearchSurfInd1[i]];
tripod2 = _lastSurfaceCloud->points[_pointSearchSurfInd2[i]];
tripod3 = _lastSurfaceCloud->points[_pointSearchSurfInd3[i]];

//pa,pb,pc為公式中的分母項各分量(利用叉乘的公式得到)，pd為分母項的值
float pa = (tripod2.y - tripod1.y) * (tripod3.z - tripod1.z)
   - (tripod3.y - tripod1.y) * (tripod2.z - tripod1.z);
float pb = (tripod2.z - tripod1.z) * (tripod3.x - tripod1.x)
   - (tripod3.z - tripod1.z) * (tripod2.x - tripod1.x);
float 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 /= ps;
pb /= ps;
pc /= ps;
pd /= ps;

// pointSel對應公式的{\widetilde{X}}_{(k+1,i)}^L
float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;

pointSel對應公式的 X ~ ( k + 1 , i ) L {\widetilde{X}}_{(k+1,i)}^L X
(k+1,i)L?

注：向量外積在數值上等于這兩個向量構成的平行四邊形的面積。
其中:
X ￣ k = R X k ? 1 + T \overline X_k=RX_{k-1}+T Xk?=RXk?1?+T X ~ k = R i n t e X k ? 1 + T i n t e \widetilde X_k=R_{inte}X_{k-1}+T_{inte} X
k?=Rinte?Xk?1?+Tinte?
R i n t e 和 T i n t e R_{inte}和T_{inte} Rinte?和Tinte?為將每個sweep中不同的scan的雷達坐標變換，用于將sweep下每個scan都映射到sweep的起始坐標系。 R i n t e 和 T i n t e R_{inte}和T_{inte} Rinte?和Tinte?由 R 和 T R和T R和T插值得到。

而后，利用LM方法計算得到 T k + 1 L T^L_{k+1} Tk+1L?，得到該時刻的定位值(雷達里程計)

融合高頻粗略的運動估計和優化后的位姿估計，得到準確位姿。根據位姿及得到的點云進行建圖，創建點云地圖。

• 代碼文件：laserOdometry.cpp
  ALOAM中實現代碼比較清晰。根據scanRegistration發布的點云信息，尋找角點與平面點的匹配點，并構建點到線及點到面的約束方程，使用ceres進行求解。

  點云使用pcl下的kdtree存儲。先將點云都變換到sweep起始坐標系，然后在存放上一幀點云的kdtree中查找點云的最鄰近點，在找到的最鄰近點的相鄰幀找到鄰近點的最鄰近的點。并構造距離方程。

  通過迭代得到：q_last_curr，t_last_curr，即上一個sweep其實坐標系相對與這一個sweep的起始坐標系的變換。
  累積便可的odom的輸出。

3. 雷達建圖(lidar mapping): (低頻) 一次完整sweep執行一次
   地圖創建
   雷達地圖創建采用點云地圖，通過迭代得到的每一次sweep對應的雷達的世界坐標系下的位姿變換矩陣，得到點云的世界坐標坐標。
   位姿精優化(mapping odometry)
   使用的點云數量是高頻odom輸出的10倍，使用分塊(cude)存儲點云，但同時處理頻率是odom的1/10。
   將當前雷達所在位置為中心cube，submap為當前中心cube相鄰水平左右2兩個cube，豎直上下兩個cube，深度前后1個cube中的點云(一個方向上5個cube)，將submap中的點云進行降采樣，并篩除不在當前相機視野下的點云，作為target，以當前幀降采樣后的特征點云為source的ICP過程。優化變量為里程計的運動估計誤差矩陣 T o d o m 2 m a p T_{odom2map} Todom2map?

   精優化的點到線約束代碼實現過程(A-LOAM)：

   1. 選取點最近鄰5個點，進行協方差矩陣特征值、特征向量計算，若其中一個特征值遠大于其他特征值，則說明該點是邊線點，其中最大的特征值對應的特征向量就是該線的方向向量。這個判斷方法同NICP算法的平面法向量計算。解釋參考
   2. 利用得到的直線的方向向量在該點附近構造2個鄰近點，同odom使用同樣點到線的距離約束方程進行約束。

   精優化的點到面約束代碼實現過程(A-LOAM):

   1. 選取最近鄰5個點 ，設平面方程為ax + by + cz + 1 = 0，求解平面法向量X=(a,b,c)，將最近鄰5個點帶入平面方程，得到 A X = B AX=B AX=B的方程，其中A為有5個點云構成的5*3的矩陣， B = [ ? 1 , ? 1 , ? 1 , ? 1 , ? 1 ] T B=[-1,-1,-1,-1,-1]^T B=[?1,?1,?1,?1,?1]T，使用QR分解得到平面法向量X=(a,b,c)。并對向量進行歸一化得到(a’, b’, c’)。norm = matA0.colPivHouseholderQr().solve(matB0);
   2. 點( x 0 , y 0 , z 0 x_0,y_0,z_0 x0?,y0?,z0?)到平面的計算公式： a b s ( a x 0 + b y 0 + c z 0 + 1 ) / ( a 2 + b 2 + c 2 ) = a ′ x 0 + b ′ y 0 + c ′ z 0 + 1 / n o r m abs(ax_0 + by_0 + cz_0 + 1) /\sqrt{(a^2 + b^2 + c^2)}=a’x_0 + b’y_0 + c’z_0 + 1/norm abs(ax0?+by0?+cz0?+1)/(a2+b2+c2)
      ?=a′x0?+b′y0?+c′z0?+1/norm

   這個點(角點/平面點)特征的區分不同于雷達里程計的輸入的點云提取，是因為，在雷達里程計的計算中，舍去了參考意義小或重復的點云。

   下圖為雷達里程計的輸出和laser mapping下優化位姿變換的輸出的關系：
   雷達里程計的輸出是在雷達坐標系下，高頻，有漂移；
   地圖最后輸出位姿變化是全局的，高精度，低頻。
   

• 代碼文件：laserMapping.cpp
  接收odom發來的odom定位估計以及去誤差的點云 P k P_k Pk?映射到世界坐標系下，記為 Q k Q_k Qk?，并將下采用后的點云存儲在cubes中，就類似三維柵格，利用cubes將點云分塊存放，每個cubes的點云。cubes的預留個數： （Width 21）*（Height 11）*（Depth 21）;

LOAM中Mapping線程中的幀與submap的特征匹配，用到的submap就是上圖中的黃色區域，submap中的corner特征和surf特征在匹配中作為target(篩除無效點后存放在kdtree[Corner/Surf]FromMap)；而當前幀的單幀點云中的兩種特征在匹配中作為source(代碼中存放在laserCloudxxxStack2中)。
[centerCubeI,centerCubeJ,centerCubeK]為當前幀點云(source)的cube坐標。因為保證索引的非負性，所以計算時要加上laserCloudCenWidth[/Height/Depth]。

此外要保證【3 < centerCubeI < 18， 3 < centerCubeJ < 8, 3 < centerCubeK < 18】，因為要保證【submap為當前中心cube相鄰水平左右2兩個cube，豎直上下兩個cube，深度前后1個cube中的點云(一個方向上5個cube)】，
如果[centerCubeI,centerCubeJ,centerCubeK]不滿足以上條件，如centerCubeI<3時，則對應調節預留的cubes的分布位置，保證submap的正確提取。

以下圖片來自https://www.cnblogs.com/wellp/p/8877990.html

MAP的ICP也是將點云分為邊線點(edge/corner)和平面點(planner/sur)，來一起添加約束方程
邊線點判斷。邊線點和平面點的分類，是通過計算點與周圍點簇的協方差矩陣的特征值來判斷的。
當最大的特征值遠大于其他特征值，說明該點位于邊線上；
當最小的特征值遠小于其他特征值，說明該點位于平面上。

點云的曲率計算與odom一樣，點云數量比odom多；
通過分析點云簇 S ′ S’ S′的協方差矩陣，分析邊線及平面的方向；
A-LOAM 的laserMapping.cpp集成LOAM的transformMaintenance.cpp中的odom輸出的高頻及map輸出的低頻集成高頻的定位。q_wmap_wodom ，t_wmap_wodom 來消除odom相對于map的偏差。
當odom定位輸出后，如果q_wmap_wodom ，t_wmap_wodom有輸出則更新，如何沒有，則按先前的q_wmap_wodom ，t_wmap_wodom更新odom的輸出，從而實現高頻的map定位輸出。

• 代碼文件：lidarFactor.hpp
  定義ceres的代價結構體及仿函數
  LidarEdgeFactor
  odom和map中點到線約束結構體及仿函數：使用2個臨近點(j,l)確定直線，點到直線的距離作為約束

  LidarPlaneFactor
  odom點到面的約束結構體及仿函數：當前點p，使用3個鄰近點(j,l,m)確定平面，使用(lp – lpj).dot(ljm)，NICP思路

  LidarPlaneNormFactor
  map精優化的點到面的約束及仿函數，就是點到面的計算公式： a b s ( a x 0 + b y 0 + c z 0 + 1 ) / ( a 2 + b 2 + c 2 ) = a ′ x 0 + b ′ y 0 + c ′ z 0 + 1 / n o r m abs(ax_0 + by_0 + cz_0 + 1) /\sqrt{(a^2 + b^2 + c^2)}=a’x_0 + b’y_0 + c’z_0 + 1/norm abs(ax0?+by0?+cz0?+1)/(a2+b2+c2)
  ?=a′x0?+b′y0?+c′z0?+1/norm

  LidarDistanceFactor未用

退化問題

定位問題分為兩步：1、位姿預測；2、位姿優化
1、位姿預測( x p x_p xp?)：使用imu預測姿態，或前端點云計算得到位姿；
2、位姿優化( x u x_u xu?)：使用非線性最小二乘優化方法得到的位姿。
退化問題解決思路：
退化問題主要出現在位姿優化，當出現退化現象時，舍棄退化方向的值，使用預測值(由其他傳感器估計或者模型估計計算得到)來填充退化方向上的位姿解。

求解問題如下：
可使用如下方法求解：
將 A x = b Ax=b Ax=b兩邊左乘 A T A^T AT，得到 A T A x = A T b A^TAx=A^Tb ATAx=ATb，保證 A T A A^TA ATA滿秩，即可逆，則可根據 x = ( A T A ) ? 1 A T b x=(A^TA)^{-1}A^Tb x=(ATA)?1ATb解出 x x x。

作者定義退化因子 D = λ m i n + 1 \mathcal{D}=\lambda_{min}+1 D=λmin?+1，
其中 λ m i n \lambda_{min} λmin?為 A T A A^TA ATA的特征值，特征值很小時，則表明該特征值對應的方向存在退化現象。
退化現象的表現：
(a)表示求解約束較好的情況，(b)退化情況，在解在藍色方向上存在退化，表明，藍色箭頭方向解的不確定性大。

令 v 1 , . . . , v m v_1,…,v_m v1?,...,vm? 對應的特征值小于閾值，因此被視為退化方向向量。
v m + 1 , . . . , v n v_{m+1},…,v_n vm+1?,...,vn?對應的特征值大于閾值，因此被視為非退化方向向量。
則：
其中：
由以下公式得到上式：
V p x p V_px_p Vp?xp?將預測值(由傳感器或者模型預測得到)映射到退化方向上；
V u x u V_ux_u Vu?xu?將計算值映射到非退化方向上，其實就是舍棄退化部分；
因此，退化部分使用預測值和計算優化部分的非退化部分。

解釋： V f x f = V p x p + V u x u V_fx_f=V_px_p+V_ux_u Vf?xf?=Vp?xp?+Vu?xu?

設預估值為 x p x_p xp?，更新值為 x u x_u xu?，更新值 x u x_u xu?在特征向量 v 1 v_1 v1?方向退化，將預估值 x p x_p xp?映射到 v 1 v_1 v1?方向，將更新值 x u x_u xu?映射到 v 2 v_2 v2?方向。最后合成為 x f x_f xf?。

V p x p V_px_p Vp?xp?為 x p x_p xp?與 V p V_p Vp?中的各個特征向量的投影*對應向量的模

而在LOAM中，使用imu作為姿態預測，使用雷達點云的點到線、點到面來優化姿態角，并計算得到偏移。或者三前端計算得到位姿，后端批量優化。
令 Δ x u \Delta x_u Δxu?為后端優化相對于前端預測值，得到的位姿偏差。此時：(與論文中的偽代碼不同，包括代碼中的matP都應為matU), V p V_p Vp?應為 V u V_u Vu?，即提出 Δ x u \Delta x_u Δxu?的非退化部分。

代碼結構

#loam_velodyne.launch
  <node pkg="loam_velodyne" type="scanRegistration" name="scanRegistration" output="screen"/>
  <node pkg="loam_velodyne" type="laserOdometry" name="laserOdometry" output="screen" respawn="true">
  </node>
  <node pkg="loam_velodyne" type="laserMapping" name="laserMapping" output="screen"/>
  <node pkg="loam_velodyne" type="transformMaintenance" name="transformMaintenance" output="screen"/>
  <group if="$(arg rviz)">
    <node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find loam_velodyne)/rviz_cfg/loam_velodyne.rviz" />
  </group>

scanRegistration：點云提取，特征點分類
laserOdometry：點到線 點到面匹配，得到初始定位信息，高頻輸出10hz
laserMapping：將點云分塊處理，尋找最近鄰的匹配點，且用于計算點云簇的法向量，類似point-to-plain方法，實現點到線及點到面的約束，最后得到去漂的優化定位值。低頻輸出。
transformMaintenance：將odom的高頻粗匹配與map的低頻高精度定位值進行集成，輸出高頻高精度的定位信息。

aloam 集成代碼中的實現在laserMapping.cpp，當得到odom粗匹配則利用map的定位輸出進行校正一下，然后發布校正后的定位信息。map校正是通過計算map與odom之間的坐標系關系匹配的。

loam_velodyne源碼解析
A-LOAM代碼解析

每個cpp文件對應LOAM框架重的一個節點。
推薦兩篇講述論文的博客及文檔
LOAM筆記及A-LOAM源碼閱讀
loam_velodyne
參考：
https://zhuanlan.zhihu.com/p/57351961
LOAM筆記及A-LOAM源碼閱讀
3D 激光SLAM ->loam_velodyne論文與代碼解析Lidar Odometry and Mapping
LOAM論文和程序代碼的解讀
https://blog.csdn.net/shoufei403/article/details/103664877

總結

以上是生活随笔為你收集整理的LOAM 原理及代码实现介绍[通俗易懂]的全部內容，希望文章能夠幫你解決所遇到的問題。

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