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

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

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

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

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

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

兩個(gè)位姿點(diǎn)（紅色和藍(lán)色圓圈對(duì)）之間共享特征（灰點(diǎn)）的示例。彩色條表示已知的校準(zhǔn)后的立體圖像對(duì)。（a）-（d）三視圖；（e）四視圖。

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

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

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

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

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

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

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

B、 相對(duì)運(yùn)動(dòng)估計(jì)

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

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

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

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

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

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

重建的結(jié)果對(duì)比

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