摘要
盡管運動恢復結構（SfM）作為一種成熟的技術已經在許多應用中得到了廣泛的應用，但現(xiàn)有的SfM算法在某些情況下仍然不夠魯棒。例如，比如圖像通常在近距離拍攝以獲得詳細的紋理才能更好的重建場景細節(jié)，這將導致圖像之間的重疊較少，從而降低估計運動的精度。在本文中，我們提出了一種激光雷達增強的SfM流程，這種聯(lián)合處理來自激光雷達和立體相機的數(shù)據，以估計傳感器的運動。結果表明，在大尺度環(huán)境下，加入激光雷達有助于有效地剔除虛假匹配圖像，并顯著提高模型的一致性。在不同的環(huán)境下進行了實驗，測試了該算法的性能，并與最新的SfM算法進行了比較。

CMU Smith Hall重建點云模型（灰色），覆蓋視覺特征點（紅色）

相關工作與主要貢獻
基于機器人的檢測需求越來越大，需要對橋梁、建筑物等大型土木工程設施的高分辨率圖像數(shù)據進行處理。這些應用通常使用高分辨率、寬視場（FOV）相機，相機在離結構表面近距離處拍攝，以獲得更豐富的視覺細節(jié)。這些特性對標準SfM算法提出了新的挑戰(zhàn)。首先，大多數(shù)可用的全局或增量SfM方案都是基于單個攝像機的，因此不能直接恢復比例。更重要的是，由于視場的限制，相鄰圖像之間的重疊區(qū)域被縮小，從而導致姿態(tài)圖只能局部連通，從而影響運動估計的精度。這個問題在大規(guī)模環(huán)境中變得更加重要。
為了解決上述挑戰(zhàn)本文提出了一種新的方案，它擴展了傳統(tǒng)的SfM算法，使之適用于立體相機和LiDAR傳感器。這項工作基于一個簡單的想法，即激光雷達的遠距離能力可以用來抑制圖像之間的相對運動。更具體地說，我們首先實現(xiàn)了一個立體視覺SfM方案，它計算攝像機的運動并估計視覺特征（結構）的三維位置。然后將激光雷達點云和視覺特征融合到一個單一的優(yōu)化函數(shù)中，迭代求解該優(yōu)化函數(shù)以最優(yōu)化相機的運動和結構。在我們的方案中，LiDAR數(shù)據從兩個方面增強了SfM算法：
1）LiDAR點云用于檢測和排除無效的圖像匹配，使基于立體相機的SfM方案對視覺模糊具有更強的魯棒性；
2）LiDAR點云與視覺特征在聯(lián)合優(yōu)化框架中相結合，以減少運動漂移。我們的方案可以實現(xiàn)比最先進的SfM算法更一致和更精確的運動估計。
本文的工作主要有以下幾個方面：
1）將全局SfM技術應用于立體攝像系統(tǒng)，實現(xiàn)了攝像機在真實尺度下的運動初始化。
2） 激光雷達數(shù)據被用來排除無效的圖像匹配，進一步加強了方案的可靠性。
3） 通過聯(lián)合立體相機和激光雷達的共同的數(shù)據，擴展了我們先前提出的聯(lián)合優(yōu)化方案，提高了所建模型的精度和一致性。
激光雷達增強的雙目SFM
該方案以一組立體圖像和相關的LiDAR點云作為輸入，以三角化特征點和合并的LiDAR點云的格式生成覆蓋環(huán)境的三維模型。下圖顯示了我們的LiDAR增強SfM方案的過程光雷達增強的雙目SFM方案

A、 對應特征點搜索
給定立體圖像對，計算對應關系包括特征提取、匹配和幾何驗證。首先，我們依賴OpenMVG庫從圖像中提取SIFT特征。然后使用所提供的級聯(lián)哈希方法對特征進行窮盡匹配。最后，通過對雙目極線約束進行幾何的驗證，驗證了兩幅圖像之間的匹配。具體地說，利用RANSAC估計基本矩陣F，然后用來檢查匹配特征的極線誤差。只保留幾何上一致的特征，以便進一步計算。

B、 相對運動估計
由于立體圖像對是預先校準的，所以我們將一對左右圖像作為一個獨立的單元，為了估計相對運動，標準的立體匹配方法依賴于兩對圖像中所有四幅圖像所觀察到的特征點，而我們觀察到許多點只被三幅甚至兩幅圖像共享。忽略這些點可能會丟失估計相機運動的重要信息，特別是在圖像重疊有限的情況下。因此，這里選擇顯式地處理兩個位姿點之間共享視圖的不同情況。具體來說，我們考慮至少3個視圖共享的特征點，以確保尺度的重建。雖然只有2個視圖的點可以幫助估計旋轉和平移方向，但是由于這些點通常來自于下圖所示的小重疊區(qū)域，所以這里忽略它們。另一方面，兩個位姿點之間也可能存在多種類型的共享特性。為了簡化問題，我們選擇對應關系最多的類型來求解相對運動。在三視圖情況下，首先用立體圖像對，對特征點點進行三角化，然后用RANSAC+P3P算法求解。在四視圖的情況下，我們遵循標準的處理方法，首先對兩個站點中的點進行三角化，然后應用RANSAC+PCA配準算法找到相對運動。在這兩種情況下，都使用非線性優(yōu)化程序來優(yōu)化計算的姿態(tài)和三角化，通過最小化內線的重投影誤差。最后，對所有姿態(tài)進行變換以表示左攝像機之間的相對運動。

兩視圖要素的區(qū)域示例。左：一位姿右圖像；中右：另一位姿的左右圖像。共同的小區(qū)域靠近邊界并用紅框標記。

兩個位姿點（紅色和藍色圓圈對）之間共享特征（灰點）的示例。彩色條表示已知的校準后的立體圖像對。（a）-（d）三視圖；（e）四視圖。

C、 相對運動驗證
一旦找到了相對運動，就可以建立一個姿態(tài)圖，其中節(jié)點表示圖像幀的姿態(tài)，邊表示相對運動。全局姿態(tài)可以通過平均位姿圖上的相對運動來求解。然而，由于環(huán)境中的視覺模糊性（見下圖），可能存在無效的邊緣，并且直接平均相對運動可能會產生不正確的全局姿勢。因此，設計了一個兩步邊緣驗證方案來去除異常值。
（1）在第一步中，檢查所有圖像幀對的激光雷達點云的重疊，并剔除不一致的點云。
（2）第二步中檢查回環(huán)的一致性。（具體方法可在論文中有詳細說明）

由于視覺模糊導致的無效相對運動的例子。（a） 由于相同的停車標志，兩對圖像匹配不正確。（b） 相應的點云來自兩個車站，標志用紅框標出。（c） 合并的占用網格顯示不正確的對齊方式（紅色橢圓）。在這種情況下，一致性比為0.56，而有效相對運動的一致性比通常超過0.7

D、 全局位姿初始化
這部分主要介紹優(yōu)化全局幀的代價函數(shù)：

E、三角化與RANSAC
本文采用文魯棒三角化方法，對每個三維特征點使用RANSAC來尋找最佳的三角化視圖。對于每個軌跡，它是不同相機視圖中一個特征點的觀察值的集合，隨機對兩個視圖進行采樣，并使用DLT方法對該點進行三角化。通過將該點投影到其他視圖上并選擇具有較小重投影誤差的視圖，可以找到更匹配的視圖。此過程重復多次，并保留最大的一組內部視圖（至少需要3個視圖）。最后，通過最小化重投影誤差，利用內聯(lián)視圖優(yōu)化特征點在全局結構中的位姿。

F、聯(lián)合位姿優(yōu)化
基于視覺的SfM算法的位姿優(yōu)化通常通過束調整（BA）來實現(xiàn)。然而，由于多個系統(tǒng)原因，如特征位置不準確、標定不準確、對應異常值等，位姿估計在長距離內可能會產生較大的漂移，尤其是在無法有效地發(fā)現(xiàn)閉合環(huán)路的情況下。為了解決這個問題，我們考慮利用激光雷達的遠距離能力來限制相機的運動，該方案將相機機和激光雷達觀測值聯(lián)合最優(yōu)化。這部分內容可查看原文理解公式。

實驗結果
A、實驗裝置
下圖具有多個機載傳感器，包括兩個Ximea彩色攝像頭（1200萬像素，全局快門）和一個安裝在連續(xù)旋轉電機上的Velodyne Puck激光雷達（VLP-16）。利用編碼器測量的電機角度，將VLP-16的掃描點轉換成固定的基架。

傳感器盒子和數(shù)據集

B、 相對運動估計

上圖：從4個視圖和3個視圖點顯示求解的對數(shù)。下圖：不同三元組檢查的邊數(shù)直方圖。

左圖：初始化的位姿圖有4個視圖特征。右：使用多視圖初始化位姿圖

C、 相對運動驗證
這里比較了所提出的基于網格的檢查（GC，閾值為0.6）和成功率檢查（SR）與OpenMVG使用的旋轉循環(huán)檢查和transform（旋轉和平移）循環(huán)檢查（TC）的異常值排除法的性能。

D、 聯(lián)合測量
這里展示聯(lián)合觀測建模在聯(lián)合優(yōu)化中的優(yōu)勢。如下圖所示

激光雷達點云（灰色）與重建視覺特征（紅色）疊加。左：沒有聯(lián)合觀測。右：聯(lián)合觀測。

E、重建
對收集到的數(shù)據集的重建結果下圖所示。在第一行，展示了小型混凝土結構的重建。第二行比較了使用COLMAP、OpenMVG和我們的方案Smith-Hall重建結果。在這三個測試中，使用左右圖像進行重建。然而，COLMAP和OpenMVG都無法處理由停車標志，和有限的重疊圖像造成的視覺模糊。因此，生成的模型要么不一致，要么不完整。使用我們的方案有助于有效地排除無效的運動，并允許建立一個更一致的模型。

重建的結果對比

總 結
本文提出了一種利用激光雷達信息提高立體SfM方案的魯棒性、準確性、一致性和完備性的LiDAR增強立體SfM方案。實驗結果表明，該方法能有效地找到有效的運動位姿，消除視覺模糊。此外，實驗結果還表明，結合相機和激光雷達的聯(lián)合觀測有助于完全約束外部變換。最后，與最先進的SfM方法相比，LiDAR增強SfM方案可以產生更一致的重建結果。