PL-SVO是基於點、線特征的半直接法單目視覺里程計，我們先來介紹一下基於點特征的SVO，因為是在這個基礎上提出的。
[1]References： SVO: Fast Semi-Direct Monocular Visual Odometry
---Christian Forster, Matia Pizzoli, Davide Scaramuzza ∗
從名字來看，是半直接視覺里程計，所謂半直接是指通過對圖像中的特征點圖像塊進行直接匹配來獲取相機位姿，而不像直接匹配法那樣對整個圖像使用直接匹配。整幅圖像的直接匹配法常見於RGBD傳感器，因為RGBD傳感器能獲取整幅圖像的深度。 雖然semi-direct方法使用了特征，但它的思路主要還是通過direct method來獲取位姿，這和feature-method不一樣。同時，semi-direct方法和direct method不同的是它利用特征塊的配准來對direct method估計的位姿進行優化。
總結：半直接法（特征點法與直接法混用），跟蹤一些關鍵點（沒有描述子），然后像直接法那樣根據這些關鍵點周圍的信息（4×4的圖像塊）匹配來估計相機的位姿.
注意：1. 在1.sparse model-based image alignment 2.Relaxation Through Feature Alignment 中使用的都是光度誤差（像素差異），在3.Pose and Structure Refinement 中是優化后的特征位置和之前預測的特征位置存在的差異。
可以詳細看SVO的wiki，很有用：https://github.com/uzh-rpg/rpg_svo/wiki
rpg_svo/svo/doc：
Notation
px - Pixel coordinate (u,v)
f - Bearing vector of unit length (x,y,z)
T_f_w - Rigid body transformation from world frame w to camera frame f.
This transformation transforms a point in world coordinates p_w to a point in frame coordinates p_f as follows: p_f = T_f_w * p_w.
The camera position in world coordinates must be obtained by inversion: pos = T_f_w.inverse().translation()
感謝這些好的博客
白巧克力：
路游俠：
http://www.cnblogs.com/luyb/p/5773691.html
馮兵：
可愛的小螞蟻：
[2]Reference： PL-SVO: Semi-Direct Monocular Visual Odometry by Combining Points and Line Segments
---Ruben Gomez-Ojeda, Jesus Briales, and Javier Gonzalez-Jimenez
I. Introduction
在機器人應用中視覺里程計（VO）越來越重要，如無人機（UAV）或自主車，它作為導航系統的重要組成部分。為解決VO的問題已嘗試采用不同的傳感器，如單眼或立體攝像機[ 2 ] [ 3 ] [ 4 ]，RGB-D攝像機[ 5 ] [ 6 ]，或將其中的任何結合慣性測量單元（IMU）[ 7 ]。傳統的方法包括檢測和匹配幀間的特征點，然后，通過觀測值和投影點[ 8 ]之間的重投影誤差的最小二乘最小化來估計攝像機運動。在這種情況下，這種方法的性能在圖1所示的低紋理場景中變壞，在那里很難找到一個大的或分布良好的圖像特征集。與此相反，線段通常是豐富的在人工場景，其特點是豐富的邊緣規則結構和線性形狀。圖像中處理的線段，它不像點那么簡單，因為它們很難表示[ 9 ]，而且對檢測和匹配任務也要求很高的計算量，因此只有少數的解決方案被提出[ 10 ] [ 11 ]，勉強達到實時性規范。此外，基於邊緣的算法也被用於解決跟蹤問題[ 12 ] [ 13 ] [ 14 ]，並估計相機運動[ 15 ]。然而，這些方法需要一個相當代價高的直接對准，這使得它們不太適合實時，也限制了其在狹窄的基線估計的應用。據我們所知，本文提出了的結合點和直線段特征的單目視覺里程計（MVO）的第一個實時方法，因此它能夠有力地在結構和粗糙紋理的情況下工作源代碼開發的C++ pl-svo庫，這個提議的說明視頻可以在這里找到：http://mapir.isa.uma.es
A. Related Work
視覺里程計算法可以分為兩個主要群體。一個被稱為基於特征的，提取一組圖像特征（關鍵點）並沿着連續幀跟蹤它們。然后，通過最小化觀測到的特征點在不同幀中的投影之間的投影誤差（最小化重投影誤差）來估計姿態。文獻為我們提供了一些基於點方法的測距問題，如PTAM[ 16 ]，其介紹了一種快速SLAM系統能夠對成千上萬個地標進行實時並行跟蹤和映射。相比之下，線特征的運動估計問題較少探討，由於其內在的困難，特別是在單目測距。在[17 ]中，作者將迭代最近點（ICP）方法[ 18 ]推廣到具有線段的立體測距的情況下，在一對多的線匹配方法中他們替代代價高的描述符的計算。在我們以前的工作[ 19 ]，我們提出了一種基於點和直線段特征的立體視覺里程計系統{即PL-StVO}。每個特征的影響用它們的協方差矩陣的逆來加權來表示，它是通過重投影的不確定性傳播技術獲得。然而，這項工作仍然依賴於傳統的特征檢測和匹配，因此它具有很高的計算成本。
另一個被稱為直接方法，通過最小化在多個圖像位置上連續幀之間的光度誤差來估計相機運動。在[ 20 ]提出了一種直接的方法，稱為DTAM，他們估計相機的姿勢通過直接對准在每個關鍵幀的完整的強度圖[是表示單通道圖像像素的強度（值的大小），即灰度圖]，采用稠密深度圖估計。然而，這種方法需要GPU並行處理，因為它處理整個圖像。針對直接法的高計算要求，在[21]中提出了一種新的單目技術，在該方法中，作者用半密集的方法估計攝像機的運動，從而在CPU上達到實時性能。他們不斷估計和跟蹤一個具有足夠圖像梯度區域的半稠密逆深度圖，因此僅利用那些將有效信息引入系統的區域。然后，他們通過最小化感興趣區域上的光度誤差來估計相機運動，從而將直接算法的良好性質與允許快速處理的稀疏方法相結合。
B. Contributions貢獻
在低紋理和結構化的情景中點特征不豐富，因此，基於點的視覺里程算法的魯棒性和准確性大大降低了。另一方面，檢測和直線段匹配要求高的計算資源，這是對於通過這些特征進行視覺測距缺乏實時的主要原因。在這項工作中，我們將[ 1 ]的半直接方法擴展到了線段的情況，作為稀疏直接運動估計的隱式結果進行快速特征跟蹤。因此，我們利用稀疏結構的這種優點，消除高代價的檢測（當一個新的關鍵幀被引入到框架時，我們只進行稀疏檢測）和描述符的計算，同時保持線段的良好性能。因此，我們提供快速的單目測距系統可工作在低紋理場景得益於點和線段特征的組合信息。接下來，我們描述了所提出的系統，並在不同的環境中通過實驗驗證了其優良的特性。
II. System Overview
所提出的系統可以理解為[ 1 ]中的半直接框架的擴展，它不僅考慮點，而且還包括場景中的線段特征，並在傳遞中引入這兩個方面。這是一個不容易實現的擴展，因為線段從幾何角度來看比點特征更復雜。在實踐中，某些圖像操作對於點是很方便的，但在線段的情況下變得十分繁瑣。因此，為了節省計算資源，我們需要執行幾個近似，並采取一些有根據的啟發式方法。這些將在第III節和第IV節中詳細介紹。
為了完整起見，我們簡要回顧了[1]中半直接框架所示的每一階段，如圖2所示，同時展示了這種半直接方法和使用線段特性的融合如何變得互惠互利。半直接法分為兩個平行線程，一個用於估計相機運動（定位），另一個用於映射環境（建圖）。
運動線程第一步，使用稀疏直接法在連續幀之間執行初始運動估計（圖3）。該方法通過最小化圖像塊重投影誤差（光度誤差）來獲取位姿。（這使得半直接運動估計的幀間特征的快速跟蹤成為可能，消除了幀對幀檢測和匹配的需要。這不僅對於點特征是相當有利的，而且對於線段來說也很有利，因為線段在計算上耗時更多。）特征從當一個新的關鍵幀插入中提取，使LSD線段檢測[ 22 ]的總成本減小。此外，為了將姿態估計優化問題的維數降低，只需完成對極幾何的自動執行，不必考慮異常匹配。
運動線程的第二步，對由直接對齊變換估計給出的特征投影進行優化（圖4），從而違反極線約束以減少相機的漂移。特征優化是以最接近的視點作為參考塊進行的。並且這種方法對線段非常有益，因為它在跟蹤線段時限制了較大的觀測基線。因此，它減輕了眾所周知的線段的問題，例如端點的重復性、遮擋或由於視圖的改變而引起的線段形變[ 11 ]。
運動線程的第三步，相機的位姿和結構圖（三維點）是通過最小化重投影誤差改善的（圖5）。
此時，由於中間連續跟蹤，遠特征的匹配得到了充分的解決，並且我們可以應用特定的基於特征的優化方法，這些方法可以很好地處理線段特征。
綜上所述，我們看到，在一個半直接框架[ 1 ]中線段的引入，對於其他更傳統的方法，可以做到更好的實現，從初步的直接步驟緩解大多數歷來在視覺里程計阻止線段使用的缺點。另外，運動和映射線程通過線段的使用而變得豐富，而不會引起整個系統的很大消耗。
地圖線程估計2D特征的深度有個概率貝葉斯過濾器，每當新的關鍵幀插入，對其進行特征提取得到種子點，初始化深度濾波器，並用普通幀來更新深度濾波器，多次觀測到它的深度，並將其融合更新深度。深度濾波器初始化時具有很高的不確定性，但它們在幾次迭代中收斂到實際值，然后插入到map中，對運動估計非常有用。
接下來，我們詳細描述了算法的各個階段，並在實際環境中進行了實驗驗證。
III. Semi-Direct Monocular Visual Odometry
和
是在標定的相機坐標系中兩個連續的相機姿態，由相對位姿變換
關聯起來，其中
是6-矢量（6×1）坐標，屬於李代數
。我們面臨的問題是，在一個幀序列中估計相機的位姿，用
表示為在世界坐標系中在第k時刻的相機姿態，
表示為第k幀的灰度圖像（intensity image）[指的是：單通道圖像像素的強度（值的大小）,在灰度圖像中intensity 指的就是圖像的灰度]和Ω表示為圖像域。我們將圖像上點特征表示為
，對應的深度為
。在線段的情況下，我們將采用兩個端點，分別由p和q表示，和線方程為l表示。從圖像反投影的3D點在時間k表示為
，並可通過逆投影功能
獲得，即
。在圖像域的三維點的投影是通過相機投影模型
獲得，也就是
。在下面，我們將擴展SVO算法到線段的情況下。
A. Sparse Model-based Image Alignment（稀疏直接法，初始運動估計）
兩連續幀之間的相機運動，
，首先通過稀疏特征沿幀跟蹤直接圖像對准估計。與基於點的方法不同，我們不能直接對齊兩幀之間的線段所占據的整個區域，因為這在計算上代價高。為此，我們只最小化均勻分布在線段上部分圖像塊的圖像誤差，如圖3所示。讓我們定義L為端點的深度在以前的時刻k−1已知的圖像區域，而終點p和q是在目前時刻
在圖像域可見：
其中 
，n = 2, ..., 
，指沿線段均勻定義的中間點。然后，線段的灰度誤差（殘差）
定義為相同3D線段點像素之間的光度差，如下：
當n=0和n=
時，點
分別指端點p和q。
（將優化問題轉成了最小二乘問題）我們估計最優姿態增量
，最小化所有像素塊的光度誤差，對於點和線段特征：
類似於[ 1 ]，我們在[ 23 ]中提出了加速最小化的逆合成公式。在這種情況下，我們尋找線段殘差的線性雅可比矩陣，它可以表示為對於每個中間采樣點的雅可比矩陣的總和：
其表達式可由[ 1 ]獲得。然后，我們估計最佳姿態通過魯棒Gauss Newton最小化（3）式。注意，這個公式允許快速跟蹤線段，如圖1所示，這是一個有議論余地的問題，因為采用傳統的基於特征的方法[ 19 ]所需的計算負擔很高。
B. Individual Feature Alignment個體特征調整（優化）
通過第一步的幀間匹配能夠得到當前幀相機的位姿，但是這種frame to frame估計位姿的方式不可避免的會帶來累計誤差從而導致漂移。所以，應該通過已經建立好的地圖模型，來進一步約束當前幀的位姿。
類似於[ 1 ]，我們單獨細化每個特征的2D位置，通過最小化該特征在當前圖像中的圖像塊（真實值，藍色方框塊）和其觀測3D特征的投影（估計值，灰色方框塊）之間的光度誤差，可采用Lucas Kanade算法[ 23 ]解決。在線段的情況下，我們只需要改進2D端點的位置（參見圖4），它定義了投影誤差估計中使用的直線方程：
其中，
是當前幀中特征位置的二維估計（維持兩個端點都相等 ），
是特征在參考幀r中的位置。這是在線段的情況下一個大膽的假設，因為他們的端點比關鍵點相當少的描述。為了處理這個問題，我們還執行了一個魯棒優化（5），然后通過改進端點的三維位置來放寬這個假設。注意，在這一步中使用仿射變形A是必要的，因為我們投射該特征的最接近的關鍵幀通常是遠的，而且圖像塊的大小比先前的更大。
通過這一步我們能夠得到優化后的特征點預測位置，它比之前通過相機位姿預測的位置更准，所以反過來，我們利用這個優化后的特征位置，能夠進一步去優化相機位姿以及特征點的三維坐標。所以位姿估計的最后一步就是Pose and Structure Refinement。
C. Pose and Structure Refinement
，都在世界坐標系中，它大大降低了估計軌跡的漂移。當使用這兩種類型的特征時，方程是：
代表點特征情況下的投影誤差，
代表線特征情況下的投影誤差：
這是用Gauss Newton迭代求解，為此我們需要包括（6）在一個穩健的框架，並采用柯西損耗函數：
優化包括三個步驟：（i）首先估計所有樣本的相機運動。（ii）我們篩選出異常數據，這些特征的殘余誤差是兩倍以上的穩定標准差。（iii）我們快速細化相機姿態通過內點集的優化。最后，我們改進的三維點和線段特征的位置通過最小化投影誤差。
IV. Mapping
V. Experimental Validation
在本節中，我們演示了在運動估計中包含線段的好處，特別是在低紋理環境下工作。為此，我們從模擬和真實數據中估計單目攝像機在多個序列中的軌跡。所有的實驗已經進行了一個英特爾酷睿i5-6600CPU @ 3.30GHz沒有GPU並行。
A. Evaluation in ICL-NUIM Dataset [25]
RMSE：均方根誤差
Median:平均值
B. Evaluation in TUM Dataset [26]
C. Processing Time
SVO和pl-svo算法在每個算法階段的平均時間：
Pyramid creation金字塔創造
Sparse Image Alignment 稀疏圖像對齊
Feature Alignment 特征定位
Pose and Structure Refinement 位姿與結構細化
Total Motion Estimation 總運動估計
VI. Conclusions
References
[1] C. Forster, M. Pizzoli, and D. Scaramuzza, “SVO : Fast Semi-Direct Monocular Visual Odometry,” IEEE Interna-
tional Conference on Robotics and Automation, 2014.
[3]Reference： Robust Stereo Visual Odometry through a Probabilistic Combination of Points and Line Segments
---Ruben Gomez-Ojeda, Javier Gonzalez-Jimenez
Abstract---立體視覺測距的大多數方法基於沿着圖像序列的點特征的跟蹤來重建運動。然而，在低紋理的場景中，通常很難遇到大的點特征集合，或者可能發生的是它們在圖像上分布不好，從而使得這些算法的行為惡化。本文提出了一種基於點和線段的組合的立體視覺測徑的概率方法，該方法在各種場景中穩健地工作。通過非線性地最小化點和線段特征的投影誤差來恢復相機運動。為了有效地結合這兩種類型的特征，它們的相關誤差根據它們的協方差矩陣來加權，根據在傳感器測量中的高斯分布誤差的傳播來計算。當然，該方法相比於只使用一種類型的特征在計算上更昂貴，但仍然可以在計算機上實時運行並有很多優點，包括在移動機器人中普遍使用的任何概率框架中的直接集成。
I. Introduction
II. System Overview
簡而言之，我們跟蹤立體幀序列中的特征（點和片段）並計算它們的3D位置及其相關的不確定性。然后將3D地標投影到新的相機姿態，其中誤差函數被最小化，以得到相機的姿態增量和該估計的不確定性。下面我們介紹SVO系統的每一個步驟，並描述其實現的最重要的細節。
1）點特征：為了處理特征點，我們采用ORB【14】檢測器和描述符，因為它的效率和良好的性能。為了減少離群點的數量，我們只考慮相互最佳匹配的測量，即左圖像中的最佳匹配對應於右圖中的最佳匹配。為了確保對應關系是有意義的，我們還檢查在兩個最接近匹配之間的描述空間中的距離高於某個閾值，該距離被設置為最佳匹配距離的兩倍。我們還確保了在輸入圖像上的點分布的公平性，並用一個ButkTein方法將圖像分成16個 buckets，並盡量每個中至少有20個特征。
2）線段特征：線段檢測器（LSD）【15】檢測線段，具有較高的精度和可重復性。然而，耗時，這是其實時應用的主要弱點。為了減輕這一點，我們檢測在兩個立體圖像中的並行化的框架中的線段。 對於立體匹配和幀到幀跟蹤，我們首先計算每一行的LBD描述符【16】，並根據它們的局部外觀特征來匹配它們。類似於點的情況，我們檢查這兩個特征是相互最佳匹配，並且最好的兩個匹配在描述空間中被充分分離。我們還沒有在線段檢測中應用一種bucket策略，因為它提供了不太可靠的特征，會因此產生較差的結果。
3）運動估計：一旦特征從立體幀跟蹤到下一幀，線段端點和特征點已經進行反投影。然后，通過線和點投影誤差的概率高斯牛頓最小化迭代估計運動。不正確對應的負面影響通過采用Pseudo- Huber損失函數檢測和去除異常值，如【17】中提出的。完整的過程將在第三節中詳細說明。
4）不確定性傳播：為了提高增量姿態估計的精度，我們用誤差協方差矩陣的逆來加權它們。該協方差矩陣是通過傳播假設為零均值高斯分布的特征誤差（計算機視覺中的一個共同假設）來獲得的。
最終，這種傳播過程以估計姿態的不確定性結束，這使得我們的系統適合於容易地集成在任何概率機器人算法中。誤差分布將在第四節中進行描述和驗證。