初識SLAM

主要目標：

理解一個SLAM框架由哪幾個模塊組成，各模塊的任務是什麼。
搭建編程環境，爲開發和實驗做準備。
理解如何在Linux下編譯並運行一個程序，如果有Bug，該如何調試。
掌握cmake的基本使用方法。

2.1引子

輪式移動機器人+相機。

問題：定位和建圖。

傳感器可以分爲兩類：

攜帶於機器人本體的，如輪式編碼器、相機、激光傳感器。
安裝於環境中的，導軌、二維碼標誌等。

輪式編碼器可以測得輪子轉動的角度；IMU測量運動的角速度和加速度；相機和激光傳感器則讀取外部環境的某種觀測數據。這些傳感器讓幾人通過間接手段推斷自己的位置而沒有對環境提出任何要求(對比於導軌，二維碼，Gps—有些環境沒法用Gps)。

視覺slam主要用相機解決定位和建圖問題。

相機有很多種，按照工作方式分爲單目相機(Monocular)、雙目相機(Stereo)和深度相機(RGB-D)三大類。

我們先詳細討論下他們的結構和工作原理吧。

相機模型

這裏提前可能先跳到第五講了，主要是想先來個對三大類相機的理解。相機工作原理會解釋“機器人如何觀測外部世界”。

小目標：

理解針孔相機的模型(針孔是棱鏡模型的精簡版)、內參與徑向畸變參數。
理解一個點是如何投影到相機成像平面的。

單目相機(Monocular):

我們在現實生活中能看到大量的照片。在計算機中，一張照片由很多個像素組成，每個像素記錄了色彩或亮度信息。三維世界中的一個物體反射或者發出光線，穿過相機光心後，投影在相機的成像平面上。相機的的感光器件接受到光線後，產生測量值，就得到了像素，形成我們見到的照片。這個過程如何用數學語言來描述呢？如何用數學語言描述投影關係。

相機將三維世界中的座標點(單位爲米)映射到二維圖像平面(單位爲像素)的過程能夠用一個幾何模型進行描述。最簡單的是針孔模型，它很常用很有效，它描述了一束光線通過針孔後，在針孔背面投影成像的關係。

我們將由簡單的針孔成型模型來對這種映射關係建模。同時由於相機鏡頭上的透鏡存在，使得光線投影到成像平面的過程中會產生畸變。因此，要分爲針孔和畸變兩個模型來討論整個投影過程。

兩個模型能夠把外部的三維點投影到相機內部成像平面，構成相機的內參數(Intrinsics)。

針孔相機模型

看下這個圖，設O-x-y-z爲相機座標系，習慣上我們讓z軸指向相機的前方，x向右，y向下。O爲相機光心，也是針孔模型的針孔。現實世界的空間點P，經過小孔O投影之後，落在物理成像平面O'-x'-y'上，成像點爲P'。設P的座標爲[X,Y, Z]T,P‘爲[X',Y,' Z']T,並設物理成像平面到小孔的距離爲f(焦距)。那麼，根據三角形相似，看下上圖右邊的圖。

Z/f = -X/X' = - Y/Y'

其中負號表示成的像是倒立的。不過，實際相機得到的並不是倒像(否則相機的使用會非常不方便)。爲了讓模型更符合實際，我們可以等價地把成像平面對稱地放到相機前方，和三維空間點一起放在相機座標系地同一側，如下圖所示：

這樣做可以把公式中地負號去掉，使式子更加簡潔：

Z/f = X/X' = Y/Y'

我們把X'，Y'放到等式左側，整理得:

X' = f*X/Z;
Y' = f*Y/Z;

可能會有疑問，爲什麼我們可以隨意地把成像平面挪到前方呢？這只是我們處理真實世界與相機投影地數學手段，並且，絕大多數圖像輸出地圖像並不是倒像——相機自身的軟件會幫我們翻轉這張圖片，所以我們得到的是正像，也就是對稱地成像平面地像。

所以儘管從物理原理來說，小孔成像應該是倒像，但是我們對圖像做了預處理。

上面地我們推導出了點P和它的像之間的空間關係，這裏的所有單位都可以理解成米。不過，在相機中，我們最終獲得的是一個個像素，這還需要在成像平面上對像進行採樣和量化。

爲了描述傳感器將感受到地光線轉換成圖像像素地過程，我們設再物理成像平面上固定着一個像素平面o-u-v。我們在像素平面得到了P'的像素座標：[u,v]T。

像素座標系通常的定義方式是:原點o'位於圖像的左上角，u軸向右與x軸平行，v軸向下與y軸平行。像素座標系與成像平面之間，相差了一個縮放和一個原點的平移。我們設像素座標在u軸上縮放了α倍，在v軸方向縮放了β倍。同時，原點平移了[cx,cy]T。那麼，P‘的座標與像素座標[u,v]T的關係爲：

注意一下這裏的單位問題。α和β的單位？fx和fy的單位？cx和xy的單位？

u = αX'+cx;
v = βY'+cy;

帶入上面推導出來的X'和Y’

u = α*f*X/Z + cx；
v = β*f*Y/Z + cy；

我們把α*f合併成fx，β*f合併成fy：

u = fx*X/Z +cx；
v = fy*Y/Z +cy；

其中f單位爲米，α，β的單位爲像素/米，所以fx，fy和cx，cy的單位爲像素。我們把上面的式子寫成矩陣形式會更加簡潔，不過左側需要用到其次座標，右側則是非齊次座標：

上面這個式子要熟記於心。上面的式子中我們把中間的量組成的矩陣成爲相機的內參數矩陣K。

相機內參的標定求K。這個有專門的工具箱。

剛纔只是一個簡單的投影過程。只涉及到了內參，因爲P是在相機座標系下的三維點。

由於相機在運動，所以P的相機座標應該是它的世界座標系(記爲Pw)根據相機當前的位姿變換到相機座標系下的結果。相機的位姿由它的旋轉矩陣R和平移向量t來描述。那麼有：

齊次座標到非齊次座標的轉換？

上面那個式子描述了P的世界座標到像素座標的投影關係。其中相機的位姿R和t又稱爲相機的外參數。相比於不變的內參，外參會隨着相機運動發生變化，同時也是SLAM中待估計的目標，代表着機器人的軌跡。

投影過程還可以從另一個角度來看。我們可以把一個世界座標點先轉換到相機座標系，再除掉它的最後一維的數值(即該點距離相機成像平面的深度)，相當於把最後一維進行了歸一化處理，得到點P在相機歸一化平面上的投影：

歸一化可以看成相機前方z=1處的平面上的一個點，這個z = 1平面也成爲歸一化平面。歸一化座標再左乘內參就得到了像素座標，所以我們可以把像素座標[u,v]T看成對歸一化平面上的點進行量化測量的結果。

這個模型中也可以看出，如果對相機座標同時乘以任意非零常數，歸一化座標都是一樣的，說明點的深度再投影過程中被丟失了，所以單目視覺中沒法得到像素點的深度。

理解下上面那句話的含義。

畸變

一般相機爲了取得好的成像效果，都會在相機的前方加透鏡。但是透鏡的加入會對成像過程中光線的傳播產生影響：一是透鏡自身的形狀對光線傳播的影響，二世在機械組裝過程中，透鏡和成像平面不能完全平行，這也會使得光線穿過透鏡投影到成像平面時的位置發生變化。

由透鏡形狀引起的畸變，稱爲徑向畸變。在理想的針孔模型中，一條直線投影到像素平面上還是一條直線。可是在實際拍攝中往往使得真實環境中的一條直線變成了曲線。越靠近圖像的邊緣，這種現象越明顯。由於實際加工的透鏡往往是對稱的，這使得不規則的畸變通常徑向對稱。主要分爲：桶形畸變和枕形畸變。

桶形畸變是由於圖像放大率隨着與光軸之間的距離增加而減小，而枕形畸變恰好相反。這兩種畸變中，穿過圖像中心和光軸有交點的直線還能保持形狀不變。

除了透鏡的形狀會引起徑向畸變，在相機的組裝過程中由於不能使透鏡和成像平面嚴格平行也會引入切向畸變。

爲了更好地理解徑向畸變和切向畸變，我們用更嚴格地數學形式對兩者進行描述。考慮歸一化平面上地任一點p，它的座標爲[x,y]T，也可以寫成極座標的形式[r,角度]T，其中r表示p與座標系原點之間地距離，角度表示與水平軸地夾角。徑向畸變可以看成座標點沿着長度方向發生了變化，也就是其距離原點的長度發生了變化。切向畸變可以看成座標點沿着切線方向發生了變化，也就是水平夾角發生了變化。

1. 將三維點投影到歸一化平面。設它的歸一化座標爲[x,y]T。

2.對歸一化平面上地點計算徑向畸變和切向畸變。

3.將畸變後地點通過內參矩陣投影到像素平面，得到該點在圖像上地正確位置。

上面的糾正畸變過程，我們使用了五個畸變項。

總結下小孔模型和畸變模型：

雙目相機(Stereo):

這個結合自己寫的相關博客1，相關博客2。

針孔相機模型描述了單個相機的成像模型。前面也說了，單目丟失了深度信息(怎麼用語言來表達這回事，歸一化平面？任意不爲零的數？)，所以。。。(官方原因：從相機光心到歸一化平面連線上的所有點，都可以投影至該像素。)指由當P的深度確定時(通過雙目或者RGB-D)，我們才能確切的知道它的位置。

承上啓下的操作？

雙目相機原理：通過同步採集左右相機的圖像，計算圖像間視差，來估計每一個像素的深度。下面簡述原理。

雙目相機一般由左眼相機和右眼相機兩個水平放置的相機組成(上下也可以)。在左右相機中，我們可以把兩個相機都視爲針孔相機。它們是水平放置的，意味着兩個相機的光圈都在x軸上。兩者之間的距離稱爲雙目基線(Baseline,計做b)，這是雙目相機的重要參數。

其中d定義爲左右圖的橫座標之差，稱爲視差(Disparity),根據視差，我們可以估計一個像素與相機之間的距離。視差與距離之間成反比：視差越大，距離越近。同時，由於視差最小爲一個像素，於是雙目的深度存在一個理論上的最大值，由f*b確定。

視差d的計算比較難。計算量與精度的問題。。。

這裏講的Stereo還是比較簡單的。

RGB-D相機模型

也貼一些自己寫的相關博客鏈接。

博客1，博客2。

相比雙目相機通過視差計算深度的方式，RGB-D相機的做法更爲”主動“，它能主動測量每個像素的深度。目前的RGB-D相機按原理可以分爲兩大類：

紅外結構光(Structured Light)來測量像素距離。Kinect1、Project Tango 1、Intel RealSense等。
通過飛行時間法(Time-of-flight,ToF)原理來測量距離。Kinect2 和一些現有的ToF。

無論是哪種類型，RGB-D相機都需要向探測目標發射一束光線(通常是紅外光).在結構光原理中，相機根據返回的結構光圖案，計算物體與自身之間的距離。而在ToF原理中，相機向目標發射脈衝光，然後根據返回之間的光束飛行時間，確定物體與自身之間的距離。ToF原理和激光傳感器十分相似，只不過激光是通過逐點掃描來獲取距離，而ToF相機則可以獲得整個圖像的像素深度，這也是RGB-D相機的特點。

通常在測量完之後，RGB-D相機通常按照生產時的各相機擺放位置，自己完成深度與彩色圖像像素之間的配對，輸出一一對應的彩色圖和深度圖。

RGB-D的缺點？這種發射-接收的測量方式範圍比較受限。用紅外光進行深度值測量的RGB-D相機，容易受到日光或者其他傳感器發射的紅外線干擾，因此不能在室外使用。同時，在沒有調製的情況下，同時使用多個RGB-D相機時也會互相干擾。對於透射材質的物體，因爲接收不到反射光，所以無法測量這些點的位置。另外就是成本和功耗方面不佔優勢。

https://blog.csdn.net/myarrow/article/details/52678020

https://blog.csdn.net/jackkang01/article/details/84134966

相關補充，對雙目和RGB-D的原理解釋

ToF的原理：