原文：Large-scale direct SLAM with stereo cameras. J Engel，J Stückler，D Cremers. 2015. IEEE/RSJ International Conference on Intelligent Robots & Systems

摘要：

基於雙目相機的LSD演算法是一種在普通CPU上能實時高幀率執行的雙目SLAM演算法。與基於稀疏角點的一些SLAM演算法不同，該演算法直接使用角點、邊緣和紋理等高對比度的一些畫素的攝影連貫性來關聯對齊圖片。雙目相機在同一時刻採集到的兩張圖片（空間維度）與雙目相機前後幀之間的圖片（時間維度），同時使用這時間維度和空間維度的圖片來估計出這些畫素的深度資料。而且雙目相機可以克服單目SLAM中常出現的尺度漂移或不定的情況。此外文中還介紹了一種魯棒方法用來對付環境光線漸變的影響，極大地提高了該雙目SLAM在真實環境中的效能。

III.  LSD-Slam with stereo

單目LSD-Slam演算法是一種基於關鍵幀的SLAM演算法，包括以下主要步驟：

*相機的運動跟蹤到地圖中的一個參考關鍵幀。如果當前幀離所有關鍵幀都較遠，則生成新的關鍵幀；

*通過跟蹤到的參考關鍵幀（時間維度上的雙目）與當前幀的雙目匹配點對計算出當前參考幀下的深度；

*通過各關鍵幀之間相互調整對齊和全域性圖優化把各關鍵幀的姿態優化。

而雙目LSD-Slam可以直接利用雙目算出關鍵幀的深度。這比只用時間維度或者空間維度的方法有一些優勢，如可以直接獲取場景中的尺度資訊且與相機運動無關。不過也有一些弱點，比如雙目的基線通常為固定長度，這樣就會使得三維重構精度只在一定深度範圍得到保證。而時間序列上的圖片構成的雙目由於基線不固定，在對於場景的規模上可以自動適應，不過，時間序列上的圖片組成的雙目三維重構不能得到尺度資訊，也不能有退化的運動形式。時間序列雙目和實際雙目組合在一起會得到非常之好處。

A、符號說明

黑體大寫字母表示矩陣，黑體小寫表示向量，表示矩陣的第n行，d表示深度z的倒數。

在雙目LSD中，地圖由一系列的關鍵幀組成，第i個關鍵幀用記，大括號中前兩項分別代表雙目相機某時刻的左影象和右影象，Di是深度倒數圖，Vi是方差圖。這些圖片都是雙目矯正後的圖片，以左相機為參考系。

畫素座標記為，場景中3D點記為；若相機引數矩陣為K，則有；由此可以得到3D點p為。

B、深度估計

使用若干關鍵幀估計出場景的幾何資訊，每個關鍵幀包括一系列特徵豐富（高影象梯度值的角點）的畫素及其含高斯噪聲的深度倒數圖。而其他紋理較少區域的畫素不再考慮，因為其能提供的視差資訊較少也不可靠。圖3為半稠密深度圖及其方差圖的示例。根據深度傳遞假設，可以使用前一個關鍵幀的資料來給當前幀深度圖初始化。再通過新測量資料來更新深度圖，並且對深度圖正則化和誤匹配點剔除。

雙目LSD-Slam與單目的不同，雙目既使用空間維度資訊也使用時間維度資訊來估計場景深度。

a）空間維度的雙目：使用雙目極線約束來獲取雙目的視差圖，如果雙目影象經過了雙目矯正，則可以非常迅速地在行對齊的特徵點中找到匹配點對，而得到視差圖。

在行搜尋特徵匹配中，使用5個畫素寬度的SSD灰度匹配演算法來匹配特徵點對。經過亞畫素精度優化後的匹配點對，用文獻【16】中的方法來計算方差圖。如果深度倒數圖的誤差假設為高斯模型，就可以把搜尋誤差限制在均值加減兩倍方差範圍內。實際中，除了新初始化的情況外，匹配收索範圍很小，就在一兩個畫素的鄰域內。這樣既會大大加速匹配點對的搜尋速度也會使得誤匹配的情況減少。根據誤差的兩個來源，豎直方向附近的梯度大的畫素和水平方向附近的梯度較小的畫素所能獲取的視差資料不太可靠，可以把它們直接忽略掉。

當一個新的關鍵幀初始化完成後，緊接著用空間維度雙目來更新深度倒數圖和方差圖，並修整深度資料，剔除被遮擋和填充空洞的深度資料。並把跟蹤到的非關鍵幀視差資料整合到其所屬的關鍵幀上：首先，關鍵幀畫素u的深度倒數根據估計到的姿態對映到當前幀上，並據此來雙目匹配特徵點，算出深度倒數和方差（1-3式）。

最後，測量到的深度倒數與方差根據（5-6式）對映到關鍵幀並融合到深度圖上。對於非關鍵幀的測量資料只能產生自已存在的角點畫素，見圖2。

b）時間維度雙目：跟蹤後，用當前幀和參考關鍵幀估計出視差圖，並把視差圖融合到此關鍵幀上，而且只使用深度倒數誤差較小的畫素來融合。誤差判斷標準是：梯度應該足夠大，梯度方向不要平行於極線方向，且遠離極點，文獻【16】有詳細說明。由於簡單地使用5個畫素的SSD來判斷匹配點誤差，所有使用跟蹤過程得到的仿射關係來矯正仿射光照變化，C小節將詳細說明。由於相對姿態直接從透視關係對齊估計得到，時間維度的雙目誤差常常大於空間維度雙目的誤差。

C、利用仿射光照矯正的直接透視對齊

直接從成像透視關係對齊得到相機運動，據此獲取相機基於參考關鍵幀的運動資訊和關鍵幀之間相互運動資訊的圖優化。於此，提出一種魯棒方法來補償仿射光照變化。

1）直接影象透視對齊：兩張圖片之間的運動關係，通過最小化成像誤差來估計（6式）。

其中，為的運動引數。

同時還要估計出方差，見文獻【11】。把當前幀跟蹤到一個關鍵幀的優化目標函式為：

其中是魯棒加權函式，可以將其選為Huber範數。

與文獻【12】不同的是，在這只是估計到的運動資訊，若再使用右圖片會降低估計精度。因為基線長度不一樣，右圖片基線更長，使得遮擋反光等引起的誤匹配點增多。

再考慮上各關鍵幀之間的殘差（8式）：

聯合目標函式為：

9式充分利用了所有的有用資訊，包括初始化傳遞來的深度資料和其他非關鍵幀融合過來的資料。這有別於文獻【12】中的複雜的利用四檢視的方法。

用迭代的加權列為馬夸特演算法優化（9式），使其達到最小值：首先給定一個迭代初值，在每次迭代時左乘迭代增量由求解E的二階近似的最小值得到，即（10式）。

（11式）是殘差的自變數左乘後的偏導；是E的海塞矩陣的高斯牛頓近似；W是一個對角矩陣，包含著權值。

由（12式）得到新的姿態估計。這是一種從大概到精確的迭代過程，為了得到較好的效率和收斂至最優值。

假設誤差相互獨立，從上一次迭代的得到的海塞矩陣的逆是左乘子的方差矩陣，即式13.

而實際中，各誤差是不獨立的，相關係數比較大，使得只是一個較小的界限。這還是很有用的，其包含了不同只自由度噪聲之間的相關資訊。

2）仿射光照矯正：影象透視關係對齊是基於圖片灰度一致性進行的。而這常常不能得到滿足，例如，根據灰度調整的自動曝光。一個比較著名的應對方法是使用不受仿射光照變化影響的代價函式，如使用NCC來雙目匹配而不用SSD。在這提出一種新的方法，該方法通過修改（6式）使其不受仿射光照變化的影響，改後為（14式）。

在使用LM演算法迭代求解時，不直接把所有引數一塊同時優化。而是，（1）固定a，b，優化；（2）固定，優化a，b，同時使用不同的加權方案。這是因為和a、b對於誤匹配點的效果不一樣。

*遮擋和過曝光的畫素會嚴重影響a、b所獲取的最小值，通常是被“拉”向同一個方向。很少及幾個匹配點就可以影響到最小值，所有使用最點單冒險的方法，直接去掉SSD誤差，例如。圖5顯示了兩個場景下的情況和剔除與不剔除誤匹配點的仿射對映關係。

*誤匹配點卻很少影響下的最小值，因為誤匹配點隨機“拉”向各方向，沒有確定趨勢。結果是，的某些分量只受稍多匹配點影響，而初始的時候誤差較大，此時剔除誤匹配點會使得優化結果為區域性最優值。因此使用文獻【11】中的方法，不是直接移除誤匹配點而是減小其權重。

迭代a、b最小化（15式），

其中，由匹配點集：

，

可得其閉合解為：

由此得到的a、b，接下來可以用在時間維度的雙目和深度傳遞的一致性檢測。

D、基於關鍵幀的SLAM

一個關鍵幀生成完成後（即當其變為跟蹤的參考關鍵幀並不再進行深度更新時），就將其加入到姿態圖裡面，再進行優化。