LOAM 原理及代碼實(shí)現(xiàn)介紹

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

使用傳感器介紹：

如果沒(méi)有電機(jī)旋轉(zhuǎn)，則雷達(dá)自身的掃描是一個(gè)平面的180度。如下：

加上電機(jī)的旋轉(zhuǎn)，則該設(shè)備能掃描的范圍為雷達(dá)前方的半球形。

LOAM技術(shù)點(diǎn)

• 二維雷達(dá)固定在一個(gè)轉(zhuǎn)軸上，實(shí)現(xiàn)3維雷達(dá)；一次完整的三維掃描為sweep，雷達(dá)平面的一次掃描為scan；
• 將點(diǎn)云分為兩類：邊線點(diǎn)(edge point角點(diǎn))和平面點(diǎn)(planar point)，分別對(duì)應(yīng)采取點(diǎn)到線及點(diǎn)到面的約束；在實(shí)際的測(cè)試中，平面點(diǎn)占總點(diǎn)云的80%。
• 位姿變化估計(jì)采用高頻粗估計(jì)+低頻優(yōu)化。兩套算法來(lái)實(shí)現(xiàn)定位。(laser odometry & mapping odometry)
  其中，laser odometry高頻粗估計(jì)是在雷達(dá)坐標(biāo)系下，通過(guò)點(diǎn)到線及點(diǎn)到面距離，利用LM得到雷達(dá)位姿估計(jì)；
  mapping odometry低頻位姿精估計(jì)，是在世界坐標(biāo)系下，通過(guò)點(diǎn)到線，點(diǎn)到面的距離，得到世界坐標(biāo)系下的雷達(dá)位姿估計(jì)優(yōu)化。
• 低頻位姿精估計(jì)是1HZ，與sweep頻率相同。也就是一次完整的sweep會(huì)進(jìn)行一次完整的位姿估計(jì)。

LOAM整體框架

將定位與建圖分開(kāi)運(yùn)行，高頻位姿估計(jì)+低頻優(yōu)化建圖->實(shí)現(xiàn)實(shí)時(shí)，低計(jì)算量，低漂移。

LOAM主要包括四個(gè)節(jié)點(diǎn)：點(diǎn)云提取(scan registration)、雷達(dá)里程計(jì)(lidar odometry)、雷達(dá)建圖(lidar mapping) 和位姿集成(transform integration)

1. 點(diǎn)云提取及處理（scan registration）：

   根據(jù)點(diǎn)的曲率c來(lái)將點(diǎn)劃分為不同的類別（邊/面特征或不是特征），在每一個(gè)sweep中，根據(jù)曲率對(duì)點(diǎn)進(jìn)行排序，來(lái)作為評(píng)價(jià)局部表面平滑性的標(biāo)準(zhǔn)。
   一個(gè)sweep指完成一次完整的掃描，一次sweep分為多個(gè)scan，每一次sweep的雷達(dá)位姿定義為為這一sweep起始時(shí)的狀態(tài)。

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

   設(shè) i ∈ P k i\in P_k i∈Pk?，表示點(diǎn) i i i是第k次整體sweep中的點(diǎn)。選取 i i i點(diǎn)在同一個(gè)scan中相鄰的前后的5個(gè)點(diǎn) 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ì)算 Σ 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?。注意，論文中使用的是平面雷達(dá)，每個(gè)scan掃描區(qū)域是半平面，即，空間的平面，雷達(dá)一次scan會(huì)得到空間平面與雷達(dá)掃描平面交線上的點(diǎn)，會(huì)得到一條線的點(diǎn)，但是，如果雷達(dá)掃過(guò)角點(diǎn)，則就是類似平面v字型的點(diǎn)。）
   即：
   如果是平面，前后的5個(gè)點(diǎn)與i的距離相加=0，c->0
   如果是邊線角點(diǎn)，則 c > 0 c>0 c>0
   c的計(jì)算方程的分母，用于歸一化c值，防止，因?yàn)槔走_(dá)的距離而帶來(lái)的c值得過(guò)大或小。

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

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

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

   點(diǎn)云匹配：
   該作者的點(diǎn)云匹配是基于將 P k + 1 ~ \widetilde{P_{k+1}} Pk+1?
   ?和 P k + 1 ~ \widetilde{P_{k+1}} Pk+1?
   ?進(jìn)行匹配。
   即將第k個(gè)sweep的點(diǎn)云映射到第k+1個(gè)sweep起始，將第k+1個(gè)sweep的點(diǎn)云映射到第k+1個(gè)sweep的起始。匹配 P k + 1 ~ \widetilde{P_{k+1}} Pk+1?
   ?和 P k + 1 ~ \widetilde{P_{k+1}} Pk+1?
   ?的點(diǎn)云點(diǎn)面關(guān)系，計(jì)算得到第k+1個(gè)sweep中產(chǎn)生的運(yùn)動(dòng)。

   點(diǎn)云特征點(diǎn)被分為，邊線點(diǎn)和平面點(diǎn)。
   邊線約束點(diǎn)兩個(gè)點(diǎn)確定：設(shè)去漂當(dāng)前幀的點(diǎn)云 i ∈ ξ k + 1 ~ i\in\widetilde{\xi_{k+1}} i∈ξk+1?
   ?，在上一幀點(diǎn)云(映射在k+1時(shí)刻坐標(biāo)系下) ξ k ￣ \overline{\xi_k} ξk??中找到距離最近的點(diǎn) j j j，并在 ξ k ￣ \overline{\xi_k} ξk??找到與j相鄰scan的點(diǎn) l l l。
   設(shè)想兩個(gè)墻面的豎直墻角線，設(shè)雷達(dá)偏移較小，相鄰的的scan，會(huì)出現(xiàn)類似圖a的情況，相鄰scan會(huì)測(cè)量到墻角線上的點(diǎn)。因?yàn)榭梢栽谏弦粠姓业絻蓚€(gè)相鄰幀的墻角線上的點(diǎn)，約束當(dāng)前幀打到墻角上的點(diǎn)到這兩個(gè)點(diǎn)構(gòu)成的直線上的距離最小，得到雷達(dá)運(yùn)動(dòng)T。如下圖a所示。

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

   論文中的雷達(dá)是二維平面雷達(dá)加旋轉(zhuǎn)得到，每一次scan都是一次平面半圓，一個(gè)sweep由加在雷達(dá)上的電機(jī)轉(zhuǎn)動(dòng)180度，構(gòu)成半球。一次scan打到對(duì)面一個(gè)平面上，就是一排直線排列的點(diǎn)。

   橙色線表示點(diǎn) j j j所在的scan，藍(lán)色線表示與橙色線相鄰的兩次scan的線。使用velonedy雷達(dá)，每個(gè)FOV對(duì)應(yīng)的多線構(gòu)成一個(gè)豎直scan，但是點(diǎn)云約束類似。
   

   使用kdtree存儲(chǔ)點(diǎn)云信息。

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

   位姿插值
   論文中，作者采用的是二維雷達(dá)加了一個(gè)電機(jī)旋轉(zhuǎn)，每一次scan得到的點(diǎn)云的xyz是基于雷達(dá)的自身的坐標(biāo)系，就是已經(jīng)旋轉(zhuǎn)后的雷達(dá)坐標(biāo)系。
   設(shè)雷達(dá)勻速旋轉(zhuǎn)及勻速偏移，通過(guò)插值的方法，得到一個(gè)完整的sweep中，每一個(gè)scan對(duì)應(yīng)的T(q，t)，并將點(diǎn)云坐標(biāo)變換到每一次的sweep初始坐標(biāo)系。消除點(diǎn)云在sweep中的因雷達(dá)運(yùn)動(dòng)而產(chǎn)生的誤差，得到undistorted 的點(diǎn)云 P ~ k \widetilde P_k P
   k?。
   詳細(xì)實(shí)現(xiàn)在laserOdometry.cpp TransformToStart函數(shù)中。
   

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

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

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

距離公式解釋參見(jiàn)LOAM 論文及原理分析

這里列出平面點(diǎn)的距離約束計(jì)算對(duì)應(yīng)代碼：

// tripod1，tripod2，tripod3對(duì)應(yīng)公式的j,l,m三點(diǎn)
tripod1 = _lastSurfaceCloud->points[_pointSearchSurfInd1[i]];
tripod2 = _lastSurfaceCloud->points[_pointSearchSurfInd2[i]];
tripod3 = _lastSurfaceCloud->points[_pointSearchSurfInd3[i]];

//pa,pb,pc為公式中的分母項(xiàng)各分量(利用叉乘的公式得到)，pd為分母項(xiàng)的值
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對(duì)應(yīng)公式的{\widetilde{X}}_{(k+1,i)}^L
float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;

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

注：向量外積在數(shù)值上等于這兩個(gè)向量構(gòu)成的平行四邊形的面積。
其中:
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?為將每個(gè)sweep中不同的scan的雷達(dá)坐標(biāo)變換，用于將sweep下每個(gè)scan都映射到sweep的起始坐標(biāo)系。 R i n t e 和 T i n t e R_{inte}和T_{inte} Rinte?和Tinte?由 R 和 T R和T R和T插值得到。

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

融合高頻粗略的運(yùn)動(dòng)估計(jì)和優(yōu)化后的位姿估計(jì)，得到準(zhǔn)確位姿。根據(jù)位姿及得到的點(diǎn)云進(jìn)行建圖，創(chuàng)建點(diǎn)云地圖。

• 代碼文件：laserOdometry.cpp
  ALOAM中實(shí)現(xiàn)代碼比較清晰。根據(jù)scanRegistration發(fā)布的點(diǎn)云信息，尋找角點(diǎn)與平面點(diǎn)的匹配點(diǎn)，并構(gòu)建點(diǎn)到線及點(diǎn)到面的約束方程，使用ceres進(jìn)行求解。

  點(diǎn)云使用pcl下的kdtree存儲(chǔ)。先將點(diǎn)云都變換到sweep起始坐標(biāo)系，然后在存放上一幀點(diǎn)云的kdtree中查找點(diǎn)云的最鄰近點(diǎn)，在找到的最鄰近點(diǎn)的相鄰幀找到鄰近點(diǎn)的最鄰近的點(diǎn)。并構(gòu)造距離方程。

  通過(guò)迭代得到：q_last_curr，t_last_curr，即上一個(gè)sweep其實(shí)坐標(biāo)系相對(duì)與這一個(gè)sweep的起始坐標(biāo)系的變換。
  累積便可的odom的輸出。

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

   精優(yōu)化的點(diǎn)到線約束代碼實(shí)現(xiàn)過(guò)程(A-LOAM)：

   1. 選取點(diǎn)最近鄰5個(gè)點(diǎn)，進(jìn)行協(xié)方差矩陣特征值、特征向量計(jì)算，若其中一個(gè)特征值遠(yuǎn)大于其他特征值，則說(shuō)明該點(diǎn)是邊線點(diǎn)，其中最大的特征值對(duì)應(yīng)的特征向量就是該線的方向向量。這個(gè)判斷方法同NICP算法的平面法向量計(jì)算。解釋參考
   2. 利用得到的直線的方向向量在該點(diǎn)附近構(gòu)造2個(gè)鄰近點(diǎn)，同odom使用同樣點(diǎn)到線的距離約束方程進(jìn)行約束。

   精優(yōu)化的點(diǎn)到面約束代碼實(shí)現(xiàn)過(guò)程(A-LOAM):

   1. 選取最近鄰5個(gè)點(diǎn) ，設(shè)平面方程為ax + by + cz + 1 = 0，求解平面法向量X=(a,b,c)，將最近鄰5個(gè)點(diǎn)帶入平面方程，得到 A X = B AX=B AX=B的方程，其中A為有5個(gè)點(diǎn)云構(gòu)成的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)。并對(duì)向量進(jìn)行歸一化得到(a’, b’, c’)。norm = matA0.colPivHouseholderQr().solve(matB0);
   2. 點(diǎn)( x 0 , y 0 , z 0 x_0,y_0,z_0 x0?,y0?,z0?)到平面的計(jì)算公式： 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

   這個(gè)點(diǎn)(角點(diǎn)/平面點(diǎn))特征的區(qū)分不同于雷達(dá)里程計(jì)的輸入的點(diǎn)云提取，是因?yàn)椋诶走_(dá)里程計(jì)的計(jì)算中，舍去了參考意義小或重復(fù)的點(diǎn)云。

   下圖為雷達(dá)里程計(jì)的輸出和laser mapping下優(yōu)化位姿變換的輸出的關(guān)系：
   雷達(dá)里程計(jì)的輸出是在雷達(dá)坐標(biāo)系下，高頻，有漂移；
   地圖最后輸出位姿變化是全局的，高精度，低頻。
   

• 代碼文件：laserMapping.cpp
  接收odom發(fā)來(lái)的odom定位估計(jì)以及去誤差的點(diǎn)云 P k P_k Pk?映射到世界坐標(biāo)系下，記為 Q k Q_k Qk?，并將下采用后的點(diǎn)云存儲(chǔ)在cubes中，就類似三維柵格，利用cubes將點(diǎn)云分塊存放，每個(gè)cubes的點(diǎn)云。cubes的預(yù)留個(gè)數(shù)： （Width 21）*（Height 11）*（Depth 21）;

LOAM中Mapping線程中的幀與submap的特征匹配，用到的submap就是上圖中的黃色區(qū)域，submap中的corner特征和surf特征在匹配中作為target(篩除無(wú)效點(diǎn)后存放在kdtree[Corner/Surf]FromMap)；而當(dāng)前幀的單幀點(diǎn)云中的兩種特征在匹配中作為source(代碼中存放在laserCloudxxxStack2中)。
[centerCubeI,centerCubeJ,centerCubeK]為當(dāng)前幀點(diǎn)云(source)的cube坐標(biāo)。因?yàn)楸ＷC索引的非負(fù)性，所以計(jì)算時(shí)要加上laserCloudCenWidth[/Height/Depth]。

此外要保證【3 < centerCubeI < 18， 3 < centerCubeJ < 8, 3 < centerCubeK < 18】，因?yàn)橐ＷC【submap為當(dāng)前中心cube相鄰水平左右2兩個(gè)cube，豎直上下兩個(gè)cube，深度前后1個(gè)cube中的點(diǎn)云(一個(gè)方向上5個(gè)cube)】，
如果[centerCubeI,centerCubeJ,centerCubeK]不滿足以上條件，如centerCubeI<3時(shí)，則對(duì)應(yīng)調(diào)節(jié)預(yù)留的cubes的分布位置，保證submap的正確提取。

以下圖片來(lái)自https://www.cnblogs.com/wellp/p/8877990.html

MAP的ICP也是將點(diǎn)云分為邊線點(diǎn)(edge/corner)和平面點(diǎn)(planner/sur)，來(lái)一起添加約束方程
邊線點(diǎn)判斷。邊線點(diǎn)和平面點(diǎn)的分類，是通過(guò)計(jì)算點(diǎn)與周?chē)c(diǎn)簇的協(xié)方差矩陣的特征值來(lái)判斷的。
當(dāng)最大的特征值遠(yuǎn)大于其他特征值，說(shuō)明該點(diǎn)位于邊線上；
當(dāng)最小的特征值遠(yuǎn)小于其他特征值，說(shuō)明該點(diǎn)位于平面上。

點(diǎn)云的曲率計(jì)算與odom一樣，點(diǎn)云數(shù)量比odom多；
通過(guò)分析點(diǎn)云簇 S ′ S’ S′的協(xié)方差矩陣，分析邊線及平面的方向；
A-LOAM 的laserMapping.cpp集成LOAM的transformMaintenance.cpp中的odom輸出的高頻及map輸出的低頻集成高頻的定位。q_wmap_wodom ，t_wmap_wodom 來(lái)消除odom相對(duì)于map的偏差。
當(dāng)odom定位輸出后，如果q_wmap_wodom ，t_wmap_wodom有輸出則更新，如何沒(méi)有，則按先前的q_wmap_wodom ，t_wmap_wodom更新odom的輸出，從而實(shí)現(xiàn)高頻的map定位輸出。

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

  LidarPlaneFactor
  odom點(diǎn)到面的約束結(jié)構(gòu)體及仿函數(shù)：當(dāng)前點(diǎn)p，使用3個(gè)鄰近點(diǎn)(j,l,m)確定平面，使用(lp – lpj).dot(ljm)，NICP思路

  LidarPlaneNormFactor
  map精優(yōu)化的點(diǎn)到面的約束及仿函數(shù)，就是點(diǎn)到面的計(jì)算公式： 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未用

退化問(wèn)題

定位問(wèn)題分為兩步：1、位姿預(yù)測(cè)；2、位姿優(yōu)化
1、位姿預(yù)測(cè)( x p x_p xp?)：使用imu預(yù)測(cè)姿態(tài)，或前端點(diǎn)云計(jì)算得到位姿；
2、位姿優(yōu)化( x u x_u xu?)：使用非線性最小二乘優(yōu)化方法得到的位姿。
退化問(wèn)題解決思路：
退化問(wèn)題主要出現(xiàn)在位姿優(yōu)化，當(dāng)出現(xiàn)退化現(xiàn)象時(shí)，舍棄退化方向的值，使用預(yù)測(cè)值(由其他傳感器估計(jì)或者模型估計(jì)計(jì)算得到)來(lái)填充退化方向上的位姿解。

求解問(wèn)題如下：
可使用如下方法求解：
將 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滿秩，即可逆，則可根據(jù) 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的特征值，特征值很小時(shí)，則表明該特征值對(duì)應(yīng)的方向存在退化現(xiàn)象。
退化現(xiàn)象的表現(xiàn)：
(a)表示求解約束較好的情況，(b)退化情況，在解在藍(lán)色方向上存在退化，表明，藍(lán)色箭頭方向解的不確定性大。

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

解釋： 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?

設(shè)預(yù)估值為 x p x_p xp?，更新值為 x u x_u xu?，更新值 x u x_u xu?在特征向量 v 1 v_1 v1?方向退化，將預(yù)估值 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?中的各個(gè)特征向量的投影*對(duì)應(yīng)向量的模

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

代碼結(jié)構(gòu)

#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：點(diǎn)云提取，特征點(diǎn)分類
laserOdometry：點(diǎn)到線 點(diǎn)到面匹配，得到初始定位信息，高頻輸出10hz
laserMapping：將點(diǎn)云分塊處理，尋找最近鄰的匹配點(diǎn)，且用于計(jì)算點(diǎn)云簇的法向量，類似point-to-plain方法，實(shí)現(xiàn)點(diǎn)到線及點(diǎn)到面的約束，最后得到去漂的優(yōu)化定位值。低頻輸出。
transformMaintenance：將odom的高頻粗匹配與map的低頻高精度定位值進(jìn)行集成，輸出高頻高精度的定位信息。

aloam 集成代碼中的實(shí)現(xiàn)在laserMapping.cpp，當(dāng)?shù)玫給dom粗匹配則利用map的定位輸出進(jìn)行校正一下，然后發(fā)布校正后的定位信息。map校正是通過(guò)計(jì)算map與odom之間的坐標(biāo)系關(guān)系匹配的。

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

每個(gè)cpp文件對(duì)應(yīng)LOAM框架重的一個(gè)節(jié)點(diǎn)。
推薦兩篇講述論文的博客及文檔
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

總結(jié)

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

如果覺(jué)得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。