視覺激光融合——VLOAM / LIMO算法解析

• 視覺激光融合——VLOAM / LIMO算法解析
• • 1. VLOAM算法
  • • 1.1 總體框架
    • 1.2 視覺里程計
    • 1.3 激光里程計
    • 1.4 實驗結果
  • 2. LIMO算法
  • • 2.1 總體框架
    • 2.2 尺度估計
    • 2.3 幀間估計與后端優化
    • 2.4 實驗結果

視覺激光融合——VLOAM / LIMO算法解析

在激光SLAM領域，LOAM、Lego-LOAM屬于純激光領域，除此之外還衍生處理視覺激光結合、激光IMU結合，甚至三者結合的算法，本篇博客介紹的VLOAM和LIMO算法就是屬于視覺和激光結合的算法，其中VLOAM算法更偏向于是一種激光SLAM算法，而LIMO算法更偏向于是一種視覺SLAM算法，具體地如下：

1. VLOAM算法

VLOAM算法是在2015年ICRA上提出的，但至今在KITTI榜的Odometry數據集上成績仍然靠前

VLOAM原論文名為《Visual-lidar Odometry and Mapping: Low-drift, Robust, and Fast》，作者并沒有對代碼進行開源，復現的版本有兩個，分別是YukunXia/VLOAM-CMU-16833和Jinqiang/demo_lidar，其中，前者是在去年CMU的幾位大佬在A-LOAM代碼基礎上進行改進的，并通過報告對結果進行了詳細的分析，本博客就是在此基礎上VLOAM的原理和代碼進行簡單總結。

對A-LOAM還不了解的同學可以參考下博客學習LOAM筆記——特征點提取與匹配

1.1 總體框架

VLOAM算法的總體框架如下：

VLOAM算法整體分為視覺里程計VO和激光里程計LO兩部分，視覺里程計以60HZ的頻率運行，視覺里程計通過激光獲一部分視覺特征點的深度，其輸出位姿變換積分后為幀間變換估計（Sweep to Sweep Refinement）提供先驗，而激光里程計以1HZ進行幀間變換估計以及幀到地圖的位姿估計（Sweep to Map Registraion），并最終融合視覺里程計的60Hz輸出和激光里程計的1Hz輸出作為最終的60Hz輸出，可看出來，VLOAM主要有如下特點：

核心其實以LOAM為基礎的激光里程計，視覺里程計僅僅是提供了先驗，視覺里程計完全可以由IMU或者其他高頻里程計代替；

視覺里程計的優勢是可以提供高頻輸出，但是沒有尺度且對于光照、紋理等要求比較強，激光里程計的優勢是對環境要求較低并可以構建地圖，但是輸出頻率較低，因此從某種成都上說VLOAM算法正式很好地利用了他們的特點進行了互補才達到相對較好的效果；

下面具體分析下各個部分的算法

1.2 視覺里程計

VLOAM視覺里程計中追蹤部分沒啥特別的，使用光流法或者特征點匹配都可以，較為特別的一點是VLOAM使用激光來恢復視覺特征點的深度（在圖像系下尋找最近的三個激光點擬合平面或者求均值，在后面LIMO算法中詳細介紹），從而獲得了具備深度的特征點${\mathbf{X}}_i^k={\left[\begin{array}{lll}{x}_i^k,& y_i^k,& z_i^k\end{array}\right]}^T$以及不具備深度的特征點${\hat{\mathbf{X}}}_i^k={\left[\begin{array}{lll}{\hat{x}}_i^k,& {\hat{y}}_i^k,& {\hat{z}}_i^k\end{array}\right]}^T$兩種類型，前后幀匹配特征點均滿足變換方程$${\mathbf{X}}_i^{k+1}=\mathbf{R}{\mathbf{X}}_i^k+\mathbf{T}$$
對于具備深度的點：
對于前一幀有深度的點，下一幀未必有深度，因此我們可以總是假設下一幀該點無法通過激光獲得該點深度，有${\mathbf{X}}_i^{k+1}=d_i^{k+1}{\hat{\mathbf{X}}}_i^{k+1}$，其中$d_i^{k+1}$是下一幀該點假設深度，通過聯立該方程和變換方程， 并消除$d_i^{k+1}$就可以獲得如下方程組$$\left({\hat{z}}_i^{k+1}{\mathbf{R}}_1?{\hat{x}}_i^{k+1}{\mathbf{R}}_3\right){\mathbf{X}}_i^k+{\hat{z}}_i^{k+1}{T}_1?{\hat{x}}_i^{k+1}{T}_3=0$$$$\left({\hat{z}}_i^{k+1}{\mathbf{R}}_2?{\hat{y}}_i^{k+1}{\mathbf{R}}_3\right){\mathbf{X}}_i^k+{\hat{z}}_i^{k+1}{T}_2?{\hat{y}}_i^{k+1}{T}_3=0$$其中${\mathbf{R}}_1{\mathbf{R}}_2,{\mathbf{R}}_3$分別為旋轉矩陣的第一、第二和第三行，$T_1,T_2,T_3$分別為平移向量的第一、第二和第三行。
對于不具備深度的點
我們有$d_i^k{\hat{\mathbf{X}}}_i^k$和$d_i^{k+1}{\hat{\mathbf{X}}}_i^{k+1}$，其中$d_i^k$和$d_i^{k+1}$為當前幀和下一幀該點假設深度，我們同樣將該方程與變換方程聯立，通過消除$d_i^k$和$d_i^{k+1}$得到$$?\begin{array}{c}?{\hat{y}}_i^{k+1}{T}_3+{\hat{z}}_i^{k+1}{T}_2\\ ?{\hat{x}}_i^{k+1}{T}_3?{\hat{z}}_i^{k+1}{T}_1\\ ?{\hat{x}}_i^{k+1}{T}_2+{\hat{y}}_i^{k+1}{T}_1\end{array}?\mathbf{R}{\hat{\mathbf{X}}}_i^k=0$$在論文中推導的都是解析解的計算方式，而在基于Ceres復現的代碼中分別實現了3D-3D，3D-2D，2D-3D，2D-2D四種特征點基于優化的求解運算并在報告中做了對比，對于具備深度的點，也就是3D-2D的情況，殘差構建就是將前一幀具備深度的點變換到后一幀坐標系下，并在歸一化平面上構建X方向和Y方向的殘差，如下：

struct CostFunctor32 { // 32 means 3d - 2d observation pairCostFunctor32(double observed_x0, double observed_y0, double observed_z0, double observed_x1_bar,double observed_y1_bar): // TODO: check if const & is necessaryobserved_x0(observed_x0), observed_y0(observed_y0), observed_z0(observed_z0), // 3observed_x1_bar(observed_x1_bar), observed_y1_bar(observed_y1_bar){} // 2template <typename T>bool operator()(const T* const angles, const T* const t, T* residuals) const{T X0[3] = { T(observed_x0), T(observed_y0), T(observed_z0) };T observed_x1_bar_T = T(observed_x1_bar);T observed_y1_bar_T = T(observed_y1_bar);T R_dot_X0[3];ceres::AngleAxisRotatePoint(angles, X0, R_dot_X0);R_dot_X0[0] += t[0];R_dot_X0[1] += t[1];R_dot_X0[2] += t[2];residuals[0] = R_dot_X0[0] - R_dot_X0[2] * observed_x1_bar_T;residuals[1] = R_dot_X0[1] - R_dot_X0[2] * observed_y1_bar_T;return true;}

對于不具備深度的點，也就是2D-2D的情況，殘差構建就是將前一幀不具備深度點變換到后一幀后構建一維投影殘差（點積），這和我們平常求解基礎矩陣后者本質矩陣的方法是不太一樣的，為啥這樣做呢，我的理解是需要和具備深度點求解的變換保證在同一尺度下？我不是特別確定，具體如下：

struct CostFunctor22 { // 22 means 2d - 2d observation pairCostFunctor22(double observed_x0_bar, double observed_y0_bar, double observed_x1_bar, double observed_y1_bar): // TODO: check if const & is necessaryobserved_x0_bar(observed_x0_bar), observed_y0_bar(observed_y0_bar), // 2observed_x1_bar(observed_x1_bar), observed_y1_bar(observed_y1_bar){} // 2template <typename T>bool operator()(const T* const angles, const T* const t, T* residuals) const{T observed_X0_bar_T[3] = { T(observed_x0_bar), T(observed_y0_bar), T(1.0) };T observed_X1_bar_T[3] = { T(observed_x1_bar), T(observed_y1_bar), T(1.0) };T observed_X0_bar_T_to1[3];ceres::AngleAxisRotatePoint(angles, observed_X0_bar_T, observed_X0_bar_T_to1);T t_cross_observed_X0_bar_T_to1[3];ceres::CrossProduct(t, observed_X0_bar_T_to1, t_cross_observed_X0_bar_T_to1);residuals[0] = ceres::DotProduct(observed_X1_bar_T, t_cross_observed_X0_bar_T_to1);return true;}static ceres::CostFunction* Create(const double observed_x0_bar, const double observed_y0_bar,const double observed_x1_bar, const double observed_y1_bar){return (new ceres::AutoDiffCostFunction<CostFunctor22, 1, 3, 3>(new CostFunctor22(observed_x0_bar, observed_y0_bar, observed_x1_bar, observed_y1_bar)));}double observed_x0_bar, observed_y0_bar, observed_x1_bar, observed_y1_bar;// TODO: check if the repeated creations of cost functions will decreases the performance? };

1.3 激光里程計

VLOAM激光里程計和LOAM是幾乎一致的，在作者的代碼實現中也是直接復用了A-LOAM的代碼，唯一可能有一些區別的是在Sweep to Sweep Refinement的頻率是1Hz，并且是使用視覺里程計積分獲得先驗。

1.4 實驗結果

在復現代碼的報告中也對算法精度進行了詳細的評估， 如下表所示：

上表中，LO表示只是通過Sweep to Sweep Refinement計算激光里程計，MO表示Sweep to Map計算激光里程計，（D）表示不使用視覺里程計作為激光里程計的先驗，（D）表示使用視覺里程計作為激光里程計的先驗，也就是VLOAM算法，從表中平均值可以看到VLOAM算法的確是所有算法里誤差最小的，但是，01數據集上也有例外，在視覺里程計失效的情況下整個VLOAM算法也會失效，這也是VLOAM算法的弊端，以上就是對VLOAM算法的簡單總結，對該算法感興趣的同學推薦讀一下原Paper以及復現代碼的報告。

2. LIMO算法

LIMO算法2018年發表在IROS會議，原論文名為《LIMO: LiDAR-Monocular Visual Odometry》，該論文是有開源的johannes-graeter/limo，該算法我并沒有閱讀源碼，而是通過其他論文博客對該算法進行了一些初步了解，簡單總結如下

2.1 總體框架

LIMO算法的總體框架如下：

從上圖中可以看出，LIMO算法框架主要包括特征提取、特征預處理、幀間運動估計、尺度估計、BA和回環檢測，整體上就是一個完整的VSLAM的算法框架，區別較大的地方就是接入了激光進行尺度估計，原論文中也指出，作者是想要組合激光準確的深度估計和相機的強大特征追蹤能力，換句話說，LIMO就是一種激光深度增強的VSLAM算法。

下面我主要簡單介紹下LIMO是如何進行尺度估計以及后端優化的

2.2 尺度估計

所謂尺度估計就是指通過激光來恢復視覺特征點的深度
首先將激光點云投影到對應的相機坐標系中，然后對每個特征點，執行如下步驟：

首先尋找該特征點周圍的矩形框內的激光點；

然后對這些激光點按照深度進行劃分；

尋找最靠近該特征點的深度區間的點云，擬合平面；

我們認為該特征點位于該平面上，根據光心和特征點的連線與平面的交點記為該特征點的深度；

檢測估計深度的準確性：光心和特征點連線與平面的夾角必須小于某個閾值，并拒絕深度高于30m的特征點。

對于地面上的特征點進行特殊處理。首先從激光點云中提取地面，然后直接利用地面點云擬合平面，而不需要第2和第3步。該方法和VLOAM應該是大同小異。

2.3 幀間估計與后端優化

LIMO的幀間估計和VLOAM中的視覺里程計類似，也是將特征點分為具備深度的特征點和不具備深度的特征點，整體的
殘差計算公式如下：$$\underset{x,y,z,\alpha ,\beta ,\gamma }{\mathrm{argmin}}\sum _i{\rho }_{3d\to 2d}\left({\Vert {\phi }_{i,3d\to 2d}\Vert }_2^2\right)+{\rho }_{2d\to 2d}\left({\Vert {\phi }_{i,2d\to 2d}\Vert }_2^2\right)$$其中$${\phi }_{i,3d\to 2d}={\bar{p}}_i?\pi \left(p_i,P(x,y,z,\alpha ,\beta ,\gamma )\right)$$$${\phi }_{i,2d\to 2d}={\bar{p}}_{i}F\left(\frac{x}{z},\frac{y}{z},\alpha ,\beta ,\gamma \right){\tilde{p}}_i$$通過公式看出來，具備深度的特征點構建的PnP問題，即將前一幀三維空間點投影到后一幀二維平面上，并與對應特征點構建距離殘差，而不具備深度的特征點構建的極線約束問題，即通過基本矩陣約束，這和前面提到VLOAM中構建的方法不同也更為常見。
對于后端優化，實際上就是構建了一個基于滑窗的BA問題，具體形式為：$${\mathrm{argmin}}_{P_j\in \mathcal{P},l_i\in \mathcal{L},d_i\in \mathcal{D}}{w}_0{\Vert \nu \left(P_1,P_0\right)\Vert }_2^2+\sum _i\sum _j{w}_1{\rho }_{?}\left({\Vert {?}_{i,j}\left(l_i,P_i\right)\Vert }_2^2\right)+w_2{\rho }_{\xi }\left({\Vert {\xi }_{i,j}\left(l_i,P_j\right)\Vert }_2^2\right)$$第一項$$\nu \left(P_1,P_0\right)=\Vert \text{?translation?}\left(P_0^{?1}{P}_1\right){\Vert }_2^2?s$$表達是滑窗中最舊的兩幀之間的平移不能變化過大，$s$為該兩幀優化前的平移值
第二項$${?}_{i,j}\left(l_i,P_j\right)={\bar{l}}_{i,j}?\pi \left(l_i,P_j\right)$$指的是滑窗中的特征點在圖像上的重投影誤差
第三項$${\xi }_{i,j}\left(l_i,P_j\right)={\hat{d}}_{i,j}?\left[\begin{array}{lll}0& 0& 1\end{array}\right]\tau \left(l_i,P_j\right)$$即對于有深度的點進行深度約束，即優化后的點的深度和估計的深度差距不能過大，以上就是幀間約束和后端優化的基本介紹

2.4 實驗結果

在原論文中，對算法效果進行了評估對比:

通過對比可以看到，加入激光進行深度估計的相對原始的視覺里程計在效果上確實有明顯提升，但是算法整體精度相對VLOAM并沒那么高，畢竟是基于視覺的里程計算法，在KITTI的Odometry榜上排在40+

以上就完成VLOAM和LIMO兩種視覺激光融合的定位算法的介紹，就我目前了解，除了VLOAM（如果算）之外，目前還沒有特別成熟視覺和激光的緊耦合框架，如果有知道的小伙伴歡迎評論區交流

此外，對其他SLAM算法感興趣的同學可以看考我的博客SLAM算法總結——經典SLAM算法框架總結

總結

以上是生活随笔為你收集整理的视觉激光融合——VLOAM / LIMO算法解析的全部內容，希望文章能夠幫你解決所遇到的問題。

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