留個筆記自用
Efficient LiDAR Odometry for Autonomous Driving
做什麼
Lidar Odometry雷射雷達裡程計，裡程計作為移動機器人相對定位的有效傳感器，為機器人提供了實時的位姿資訊。移動機器人裡程計模型決定于移動機器人結構和運動方式，即移動機器人運動學模型。
簡單來說，裡程計是一種利用從移動傳感器獲得的資料來估計物體位置随時間的變化而改變的方法
用建圖的方式來了解，要實作機器人的定位與導航，就需要知道機器人走了多遠，往哪走，也就是初始位姿和終點位姿，隻有知道了裡程計，才能準确将機器人掃描出來的資料進行建構。
做了什麼
這裡利用了非地面的球面距離圖像和地面的鳥瞰圖結合的方法，并且提出了一種自适應法向量估計方法，解決了投影圖像時存在的相似點失去高度資訊的問題。
首先先要了解一下什麼叫地面點，這裡的雷射雷達裡程計是基于自動駕駛的
以汽車為基礎的雷達就會出現一個問題，往往方向都是朝向地面的，是以大部分掃描的點雲都是屬于路面的，這會導緻姿态估計的偏移，而本文就是将這兩種點進行有效融合
怎麼做
首先是任務設計，這裡将任務設計成了一個非線性最小二乘最小化問題，雷射雷達裡程計的任務是通過輸入點雲Lt和Lt-1，希望得到位姿轉換Ttt-1∈SE（3）。。
這裡構造了兩個不同的cost，地面cost和非地面cost并進行分别處理，其實也可以了解成這倆是兩個loss
這裡的Es是非地面cost，EG是地面cost
很顯然上面這個式子是一個個非線性最小二乘最小化問題，可以進行最小化優化
總的來說，整體結構分為四步，輸入是原始3D點雲集，第一步首先通過柱面投影的方式将點雲投影至2D平面，以快速得到分割結果和非地面cost。第二步然後通過得到的平面分割出地面點，将其進一步投影到2D鳥瞰圖上形成地面cost。第三步通過距離自适應算法計算2D平面的法向量圖，然後通過ICP計算位姿轉換。第四步最後通過投影map和鳥瞰map更新地圖。
首先是第一步，也就是所有LO裡都有的點雲輸入處理問題，這裡也采用了柱面投影的方式，這裡展示了一下前面所述的常見投影方式會遇到的問題
在這裡，G1、G2、G3是地面點，F1、F2、F3是表面點，也就是所說的非地面點。
上排的Range Image就是柱面投影的方式，柱面投影存在的問題很明顯，缺少了點雲的高度資訊，它們會将投影在相近列中，就算包含了點的角度等資訊，也會容易存在備援重複的現象。除了非地面點遇到的問題，地面點G也存在問題，由于三個地面點其實角度上差距較小，投影容易較為接近，就算它們之間的像素距離可能相差特别大。
這裡就采用了鳥瞰圖的方法來包含處理點雲的地面資訊
雖然在這裡F也就是非地面點的高度問題沒有解決，但很好地解決了地面資訊也就是G地面點的問題，使地面點之間的距離關系最大程度地進行了保留，至于F的高度問題則在後面部分進行融合解決。
然後是投影的具體公示
這裡的（x，y，z）是點的三維坐标r表示點的範圍，θ表示方位角，φ代表仰角分量，這三個都是點的特征，w和h是人為給定的呃參數，用來限制投影出來的平面大小，f是雷達的探測範圍大小
至此得到2D平面，每個點（x，y，z）索引至坐标（u，v）
然後是第二步，地面cost Es，這個loss是由2D投影平面計算得到，可以了解成為了那些非地面點而進行的優化
這裡采用的方式是scan to model的方式，VD在scan層次上頂點的對應：R2->R3，相應的法向量圖對應是ND，這在後面有用到。VM是model層次上頂點的對應，NM是相應的法向量圖對應
其中
u即是屬于VD中的一個頂點，這裡的Π是柱面投影函數，vu是屬于VM的一個頂點，nu是一個法向量
簡單來說，這裡的ES的意思就是常見的對比loss，将點根據得到的位姿轉換T進行轉換，希望投影誤差盡可能減小，以此估計姿态相對變換，是一個幀與模型的配準問題
然後是第三步，通過一個自适應的方法計算平面法向量圖
常見的平面可以用一個函數進行代表，nxx+nyy+nzz-d=0，于是可以定義各部分的法向量誤差e
這裡的pi是3D點坐标，是總點雲集的一個小子集（由于法向量是需要局部平面進行計算），n是（nx，ny，nz）也就是該平面的法向量，d是平面偏移量
為了優化這個值，采用SVD計算局部點的協方差矩陣
這個3×3特征矩陣的最小特征值對應的特征向量就作為這個局部的法向量，也可以采用一點權重的因素
接下來是設計前面的局部區域pi鄰域的大小，這也會影響逐點的法向量計算
這裡的ws和hs是投影圖檔的分辨率，l和y是規定的local大小。
至此，使用自适應法向量計算得到了非地面點的cost ES
然後是第四步，根據地面分割和鳥瞰點計算地面點cost EG
首先是地面點分割，也就是為了知道點裡有哪些是地面點，直覺了解中，所謂的地面點，在鄰域中應存在兩個特性，第一個是地面點和雷達中心的距離應該較大，第二個就是相鄰點之間的豎直角度差應該較小，因為都是地面平行的
是以第一個規定
這裡的hp是點的z軸高度，hg是雷射雷達的中心高度，後面的兩個是人為規定的參數，意思就是過濾掉一些在雷射雷達掃描中心的非地面點
然後是第二個規定
這裡借用了平面進行輔助，p是柱面上某一點的坐标，pup和pdown分别是p點的鄰域，也就是相對于點（u，v）的
然後要求這裡的兩個θ都要小于規定的門檻值 δ
簡單來說，這裡就是控制相鄰點的角度差，如果小的話說明可能是地面點
至此，通過投影平面得到了地面點的分割圖
然後是BEV俯瞰圖
這裡的W和B和S均是人為規定的參數，代表了俯瞰圖的橫縱大小範圍和分辨率
然後就可以定義地面cost EG了
同樣，這也是個配準問題，也是scan-to-model的做法，BG是一個R2->R3的映射函數，可視為快速最近鄰搜尋的2D搜尋表，其中每個像素包含3D地面點，BM是model層次上的
這裡的做法跟前面的非地面點非常相似，都類似于對比LOSS進行一個配準
最後就是優化之後的模型更新方案，應用了前面的投影柱面和俯瞰圖
對于非地面的柱面投影，維護一張頂點圖VM和法向量圖NM，這都是計算非地面cost層次中model層次的。更新的方法是對于3D點用位姿轉換T進行轉換，然後再進行投影，計算兩個投影的距離。
對于地面點隻維護model層次的頂點圖BM，跟上面方法類似。
總結
1.全程看下來很多地方看的不是很懂，感覺這種模型更新方法很怪，希望有機會能閱讀以下這篇的代碼
2.感覺較有收獲的點還是處理資料的方式，柱面投影的問題在這裡得到了一種簡單解決，鳥瞰圖判斷地面點确實能做到這一點