泡泡點雲時空，帶你精讀點雲領域頂級會議文章
標題：Planar Odometry from a Radial Laser Scanner. A Range Flow-based Approach
作者：Mariano Jaimez, Javier G. Monroy and Javier Gonzalez-Jimenez
來源：ICRA2016
編譯：于振洋
審核：zhiyong
歡迎個人轉發朋友圈；其他機構或自媒體如需轉載，後臺留言申請授權
在本文中，我們提出了一種快速，精確的方法，可以從連續的距離掃描中估算雷射雷達的平面運動。對於每個掃描點，我們根據傳感器速度制定範圍流約束方程，並最小化所得幾何約束的魯棒函數以獲得運動估計。與傳統方法相反，此方法不進行相關性搜索，而是受稠密3D視覺測距法啟發，根據掃描梯度執行稠密掃描對齊。最小化問題通過一種從粗到精的方案來解決，以應對大位移，並且一種基於速度估計值的協方差平滑濾波器用於處理無約束場景（例如走廊）中的不確定性。通過仿真和真實實驗，我們比較了該方法與兩個著名的掃描匹配器和裡程計。定量和定性結果都證明了我們方法的優越性能，其極低的計算成本（單個CPU內核上為0.9毫秒），使其非常適合那些需要平面測距的機器人應用。為此，我們還提供了代碼，以便機器人社區可以從中受益。
視覺配準方法通常分為特徵點法和直接法（光流）兩種，而點雲匹配法大多是類似特徵點法的ICP、GICP等方法，而沒有利用點雲梯度的信息。本文受光流法的啟發，提出了測距流（range flow）的方法，並利用速度估計值的方法，提高了在無約束場景下的精度
設R(t, α)為掃描函數，其中 t 代表時間， α為掃描點的坐標，圖1中掃描點 P 在雷射雷達的坐標下極坐標為 P(r, θ)，其掃描點坐標表示如下，其中FOV為雷射雷達的掃描角度，N為雷射雷達的掃描點數
P'為連續兩次掃描間隔時間 Δt 後的掃描點。假設 R 可導，則在第二次掃描的任意點的掃描函數可通過泰勒展開表示為
捨棄掉高階項，則當掃描範圍和點的掃描坐標在 [t, t+Δt] 內改變時，掃描函數的梯度近似為
為了表示所有掃描範圍內點的速度，將上述速度（r, θ）改寫雷射雷達的笛卡爾坐標系下 (x,y)
最後假設環境為靜態的剛體，則所有掃描點的運動均由雷射雷達自身的運動引起，所以雷射雷達的速度和掃描點的速度大小相同，方向相反
將每個掃描點的角度和坐標帶入上式，即可以求解出雷射雷達的速度
由於雷射雷達的測量噪聲和線性估計引入的誤差，上一步中求解出的速度值誤差會較大，因此將掃描範圍內所有的點都帶入計算估計雷射雷達的速度並對結果進行優化（與視覺裡程計的後端優化類似）。進行優化要先定義殘差，在一個速度下掃描範圍內的每個點，定義集幾何殘差如下
為了較好地處理離群點（比如環境中的運動物體），這裡採用更加魯棒的 Cauchy M-estimator 核函數代替2範數，並且在每次迭代後會重新計算一次權重如下
為了加快收斂速度並減小掃描函數不可微的點引起的誤差，採用一種「預先計算權重」的方法。首先為了減小使線性化的誤差，將推導過程中的式子保留到2階項：
這樣做可以使系統更快收斂並減小線性化估計對結果產生的誤差
作者提出了一種新穎的名為RF2O的方法，通過在連續掃描對上施加測距流約束方程來估計雷射雷達的平面運動。廣泛的實驗證明了我們的方法的準確性，並且在不同的場景和幀頻下與PL-ICP，Polar Scan Matching 和標準車輪測距法進行了比較。結果表明，RF2O為平移和旋轉提供了最準確的估計，除了非靜態環境（PL-ICP略勝一籌）。運行時間僅為1毫秒，幾乎可以在不增加計算成本的情況下輕鬆估計平面運動，這使得該方法對許多對計算有嚴格要求並且需要實時性能的機器人應用具有吸引力。
本文受光流法的啟發，提出了測距流（range flow）的方法，並且取得了很好的跟蹤精度，為雷射雷達的匹配以及航跡推算提供了一種全新的思路，可以好好地借鑑一下。
In this paper we present a fast and precise methodto estimate the planar motion of a lidar from consecutiverange scans. For every scanned point we formulate the rangeflow constraint equation in terms of the sensor velocity, andminimize a robust function of the resulting geometric constraints to obtain the motion estimate. Conversely to traditionalapproaches, this method does not search for correspondencesbut performs dense scan alignment based on the scan gradients,in the fashion of dense 3D visual odometry. The minimizationproblem is solved in a coarse-to-fine scheme to cope with largedisplacements, and a smooth filter based on the covariance ofthe estimate is employed to handle uncertainty in unconstraintscenarios (e.g. corridors). Simulated and real experiments havebeen performed to compare our approach with two prominentscan matchers and with wheel odometry. Quantitative andqualitative results demonstrate the superior performance ofour approach which, along with its very low computationalcost (0.9 milliseconds on a single CPU core), makes it suitablefor those robotic applications that require planar odometry.For this purpose, we also provide the code so that the roboticscommunity can benefit from it
如果你對本文感興趣，想要下載完整文章進行閱讀，可以關注【泡泡機器人SLAM】公眾號。
歡迎來到泡泡論壇，這裡有大牛為你解答關於SLAM的任何疑惑。
有想問的問題，或者想刷帖回答問題，泡泡論壇歡迎你！
泡泡網站：www.paopaorobot.org
泡泡論壇：http://paopaorobot.org/bbs/
泡泡機器人SLAM的原創內容均由泡泡機器人的成員花費大量心血製作而成，希望大家珍惜我們的勞動成果，轉載請務必註明出自【泡泡機器人SLAM】微信公眾號，否則侵權必究！同時，我們也歡迎各位轉載到自己的朋友圈，讓更多的人能進入到SLAM這個領域中，讓我們共同為推進中國的SLAM事業而努力！
泡泡機器人SLAM的原創內容均由泡泡機器人的成員花費大量心血製作而成，希望大家珍惜我們的勞動成果，轉載請務必註明出自【泡泡機器人SLAM】微信公眾號，否則侵權必究！同時，我們也歡迎各位轉載到自己的朋友圈，讓更多的人能進入到SLAM這個領域中，讓我們共同為推進中國的SLAM事業而努力！
商業合作及轉載請聯繫paopaorobot@163.com