RF2O是一種快速而精確的方法，用於從連續範圍掃描中估計激光雷達的平面運動。對於每個掃描點，可以根據傳感器速度制定範圍流約束方程，並最小化所得幾何約束的魯棒函數以獲得運動估計。與傳統方法相反，該方法不搜索對應關係，而是以密集的3D視覺測距法的方式基於掃描梯度執行密集掃描對準。最小化問題以粗到精方案解決以應對大位移，並且基於估計的協方差的平滑濾波器用於處理無約束情景（例如走廊）中的不確定性。
里程計是機器人定位的重要組成部分，大家普遍採用的是編碼器里程計或者是視覺里程計。依靠電機編碼的里程計在低速情況下比較可靠，但不可避免地因爲輪子打滑等因素造成的漂移問題，而視覺里程計對於運動估計問題來說是一個比較靈活的解決方案，能適用不同的機器人，但是計算量大，佔用較大資源空間，對傳感器要求較高。激光里程計綜合上述兩種方法的優點，能快速而精確地從連續範圍掃描中估計激光雷達的平面運動，且運算成本低，表現取決於激光的分辨率和環境因素，轉換評估準確，但是由於激光雷達自身的旋轉，會導致odom旋轉不準確，這可以通過和IMU的傳感器數據融合來解決。
2D激光掃描速度的描述
假設環境是靜態和剛性的。 設R（t，α）爲範圍掃描，其中t是時間，α∈[0，N）⊂R是掃描座標，N是掃描的大小。 任何點P相對於連接到傳感器的局部參考系的位置由其極座標（r，θ）給出（見圖1）。
與某物相交的激光掃描圖表示。 經歷時間間隔Δt之後，觀察點P相對於掃描儀移動到P0。
如果從激光雷達掃描點P，則將在與P的角座標直接相關的掃描座標α處觀察到：
其中FOV是掃描儀的視野。 類似於光學流動約束方程，可以從兩個掃描對的幾何一致性的一般表達式導出線性約束。 假設R的可微性，第二次掃描的任何點的範圍可以表示爲泰勒展開
其中Δt是連續掃描之間的時間間隔，Δα表示所考慮的點的掃描座標的變化。 忽略二階/更高階項，除以Δt，我們可以得到一個簡單的表達式，它將掃描梯度與區間[t，t +Δt]期間點的範圍和掃描座標的變化聯繫起來：
如果我們認爲˙r=ΔR/Δt和˙α=Δα/Δt是區間[t，t +Δt]期間範圍內點和掃描座標的平均速度，則我們可以得到：
方程（4）首先由Gonzalez＆Gutierrez [2]引入，隨後在[16]中推廣並命名爲“範圍流約束方程”。 爲了描述所有點相對於相同矢量基礎的速度，我們將徑向和方位角速度（˙r，˙θ）轉換爲笛卡爾表示（˙x，˙y），如圖1所示：
作爲最後一步，我們需要強制每個明顯的運動實際上是由激光雷達平移和/或旋轉引起的。換句話說，我們假設每個點都相對於傳感器移動，好像它是剛體的一部分，其速度相同但與傳感器的符號相反：
得到ξs=（vx，s，vy，s，ωs）2D Twist（傳感器速度）和（x，y）笛卡爾座標P。如果笛卡爾速度（5）（6）代入（4）和 強加了剛度假設（7），我們可以將範圍流約束方程轉換爲激光雷達速度的約束：
總得來說，每個掃描點對傳感器運動都施加了限制，因此，3個線性獨立的限制在理論上足以估計它。
速度估計
在實踐中，激光雷達運動不能僅用三個獨立的限制來估計，因爲一般來說，（8）由於距離測量的噪聲，線性近似（3）或運動物體的存在所造成的誤差而不精確（ 非靜態環境）。 因此，我們提出了一種密集的公式，其中掃描的所有點都有助於運動估計。 對於每個點，我們將幾何殘差ρ（ξ）定義爲給定Twist ξ的範圍流約束（8）的評估：
爲了獲得準確的估計，通過最小化魯棒函數F內的所有幾何殘差來計算傳感器運動：
函數F是Cauchy M-estimator，k是可調參數。 與L2或L1規範的更常見選擇相反，該函數降低了具有非常高殘差的那些點的相關性，並且代表了處理異常值的有效且自動的方式。 迭代重加權最小二乘法（IRLS）解決了優化問題，其中與Cauchy M-estimator相關的權重爲：
利用IRLS，通過重新計算殘差和權重來迭代求解系統直到收斂。如前所述，有一些因素可能導致（8）不準確，主要是剛性假設（7）的不完整填充和（3）中的線性近似的偏差。儘管Cauchy M-estimator估計可以減輕它們對整體運動估計的影響，但它並沒有完全消除它。在解決運動之前很難檢測到運動物體的存在，因此，我們依靠Cauchy M-estimator估計器在最小化過程中減小它們的權重。另一方面，可以預先檢測與（3）中採用的線性近似的偏差，這有助於加速（10）中的收斂並且還導致更準確的結果。爲此，我們提出了一種預加權策略，以減輕那些範圍函數是非線性或甚至不可微分的點的殘差。我們稱之爲“預加權”，因爲它是在最小化問題（10）解決之前應用的。爲了量化與（2）的線性化相關的誤差，我們將泰勒級數展開爲二階：
可以注意到，忽略高階項，R2o中的二階導數（Δt，˙α）可用於檢測線性偏差。 一個特殊情況是關於時間（Rtt）的二階導數，其不能在粗到細方案中計算，因爲扭曲圖像是永恆的，因此，第二時間導數的概念沒有意義（粗 - 第V節中描述了to-fi ne。 此外，重要的是檢測掃描區域，其中範圍函數不僅是非線性的而且是不可微分的。 這些區域主要是觀察到的不同物體的邊緣，並且通常以非常高的一階導數值（Rt和/或Rα）爲特徵。 爲了懲罰這兩種效應，非線性和不連續性，我們爲每個掃描點定義了以下預加權函數：
參數Kd標誌着一階和二階導數的相對重要性，並且
是避免奇異情況的小常數。 因此，我們最初計算一組預加權殘差
因此，我們最初計算一組預加權殘差
隨後根據（10）（11）將其最小化。 雖然我們沒有在論文中顯示比較，但這種策略提供了比標準IRLS最小化更好的結果，沒有預加權和收斂更快（大約2倍）。
粗到細方案和掃描彎曲
（3）中給出的線性化要麼適用於連續掃描之間的小位移，要麼適用於具有恆定範圍梯度的區域（在激光雷達的情況下，對於非常不尋常的幾何形狀會發生：阿基米德螺旋）。爲了克服這個限制，我們估計了粗糙度方案中的運動，其中較粗糙的水平提供粗略估計，隨後在較高水平上得到改善。 Battiti等人介紹了粗到細方案。 [17]解決大位移的光學問題，並且自[18] [19]以來一直被採用。設R0，R1爲兩次連續激光掃描。最初，通過連續下采樣（通常爲2）原始掃描R0，R1來創建兩個高斯金字塔。通常，高斯掩模應用於下采樣RGB或灰度圖像，但是在範圍數據的情況下，標準高斯濾波器不是最佳選擇，因爲它在濾波掃描上產生僞像。作爲替代方案，我們採用雙邊濾波器[20]，它不會混合可能屬於場景的不同對象的遠點。一旦構建了金字塔，速度估計問題就會從最粗糙到嵌套級別迭代求解。在每次過渡到一個水平時，兩個輸入掃描中的一個必須根據先前水平（ξp）中估計的總體速度相互扭曲。這種變形過程總是分爲兩個步驟，並且在我們的公式中，應用於第二次掃描R1。首先，使用與Twist ξp相關的剛體運動對在R1中觀察到的每個點P進行空間變換：
其次，必須將轉換後的點重新投影到R1上以構建彎曲掃描Rw 1，以便：
可以將幾個點翹曲到相同的座標αw，在這種情況下保留最接近的點（其他點將被遮擋）。 如果ξp收斂到實際速度，那麼彎曲掃描Rw 1將比原始R1更接近第一掃描R0，這允許我們在（2）中以更精細的分辨率應用線性近似。
實施
573/5000
我們的算法特別注意範圍梯度的計算。 通常，使用固定的離散公式來近似掃描或圖像梯度。 在範圍數據的情況下，該策略導致對象邊界處的梯度的非常高的值，其不表示這些對象上的真實梯度。 作爲替代方案，我們使用一種考慮環境幾何形狀的自適應公式。 此公式使用連續觀察點（點）之間的2D距離對掃描中的前向和後向導數進行加權：
因此，最近鄰居總是對梯度計算貢獻更多，而遠點幾乎不影響它。在兩個鄰居近似等距的情況下，所呈現的公式等效於居中的有限差分近似。關於梯度計算的更多細節可以在[19]中找到。最後，重要的是要注意到存在一些環境的幾何配置，傳感器運動無法從中恢復。當激光雷達僅觀察到牆壁時，最常見的情況就出現了。在這種情況下，平行於牆的運動是不確定的，因此求解器將爲它提供任意解（不僅是我們的方法，而且是純粹基於幾何的任何方法）。爲了緩解這個問題，我們在速度ξ的本徵空間中應用低通濾波器，其工作原理如下所述。首先，分析IRLS解的協方差矩陣Σ∈R3×3的特徵值，以檢測哪些運動（或運動的組合）是不確定的，哪些是完全約束的。在特徵向量的空間中，由（10）提供的速度ξtM與前一時間間隔ξt-1的速度ξtM加權，以獲得新的濾波速度ξt：
其中E是包含特徵值和k1的對角矩陣，ke是濾波器的參數。 具體地說，kl在來自求解器的解與先前估計之間施加恆定加權，同時確定特徵值如何影響最終估計。 這些參數設置爲以下值：
其中l是金字塔等級，範圍從1（最粗）到所考慮的等級數。 有關此過濾器及其應用方式的更詳細說明，請參閱[19]。