slam筆記
1 Star2 Stars3 Stars4 Stars5 Stars 給文章打分!
Loading...

寫在最前面: 
這裡寫圖片描述

SLAM特指:特指搭載感測器的主體,在沒有環境先驗的資訊情況下,在運動過程中建立環境模型,通過估計自己的運動。 
SLAM的目的是解決兩個問題:1、定位 2、地圖構建 
也就是說,要一邊估計出感測器自身的位置,一邊要建立周圍環境的模型 
最終的目標:實時地,在沒有先驗知識的情況下進行定位和地圖重建。 
當相機作為感測器的時候,我們要做的就是根據一張張連續運動的影象,從中估計出相機的運動以及周圍環境中的情況 
需要大體瞭解的書籍: 
1、概率機器人 
2、計算機視覺中的多檢視幾何 
3、機器人學中的狀態估計 
需要用的Eigen、Opencv、PCL、g2o、Ceres 
安裝Kdevelop 
http://blog.163.com/[email protected]/blog/static/16793820820122224332943/

一些常見的感測器 
這裡寫圖片描述 
這裡寫圖片描述 
這裡寫圖片描述 
單目(Monocular)、雙目(Stereo)、深度相機(RGB-D) 
深度相機能夠讀取每個畫素離相機的距離 
單目相機 只使用一個攝像頭進行SLAM的做法叫做單目SLAM(Monocular SLAM),結構簡單,成本低。 
照片拍照的本質,就是在相機平面的一個投影,在這個過程當中丟失了這個場景的一個維度,就是深度(距離資訊) 
這裡寫圖片描述 
單目SLAM 估計的軌跡和地圖,與真實的軌跡和地圖之間相差一個因子,這就是所謂的尺度(scale)由於,單目SLAM無法憑藉影象來確定真實的尺度,又稱為尺度不確定 
單目SLAM 的缺點:1、只有平移後才能計算深度 2、無法確定真實的尺度。 
雙目相機: 
這裡寫圖片描述 
雙目相機和深度相機的目的是,通過某種手段測量物體離我們之間的距離。如果知道這個距離,場景的三維結構就可以通過這個單個影象恢復出來,消除了尺度不確定性。 
雙目相機是由於兩個單目相機組成,這兩個相機之間的距離叫做基線(baseline)這個基線的值是已知的,我們通過這個基線的來估計每個畫素的空間位置(就像是人通過左右眼的影象的差異,來判斷物體的遠近) 
但是計算機雙目相機需要大量的計算才能估計出每個畫素點的深度。雙目相機的測量到的深度範圍與基線相關,基線的距離越大,能夠檢測到的距離就越遠。雙目的相機的缺點是:配置和標定都比較複雜,其深度測量和精度受到雙目的基線和解析度的限制,而且視差的計算非常消耗計算機的資源。因此在現有的條件下,計算量大是雙目相機的主要問題之一。 
深度相機是2010年左右開始興起的一種相機,他的最大的特點就是採用紅外結構光或者(Time-of-Flight)ToF原理,像鐳射感測器那樣,主動向物體發射光並且接受返回的光,測量出物體離相機的距離。這部分並不像雙目那樣通過計算機計算來解決,而是通過物理測量的手段,可以節省大量的計算量。目前RGBD相機主要包括KinectV1/V2, Xtion live pro,Realsense,目前大多數的RGBD相機還存在測量範圍小,噪聲大,視野小,易受到日光干擾,無法測量透射材質等諸多問題。

經典視覺SLAM框架

這裡寫圖片描述 
視覺slam的路程分成以下幾步: 
1、感測器資訊的讀取。 
2、視覺里程計(Visual Odometry,VO)視覺里程計的任務是,估算相鄰影象之間相機的運動,以及區域性地圖。VO也稱為前端(Front End). 
3、後端優化(Optimization)後端是接受不同時刻視覺里程計測量的相機位姿,以及迴環檢測的資訊。(Back End) 
4、迴環檢測(loop Closing)。迴環檢測是判斷機器人是否或者曾經到達過先前的位置。如果檢測到迴環,它會吧資訊提供給後端進行處理 
5、建圖(Mapping),他根據估計出的軌跡,建立與任務要求的對應的地圖。

視覺里程計

視覺里程計關心的是,相鄰影象之間相機的移動,最簡單的情況就是計算兩張影象之間的運動關係。 
為了定量的估計相機的運動,必須瞭解相機與空間點的幾何關係。 
如果僅僅用視覺里程計來軌跡軌跡,將不可避免的出現累計漂移,注意,這個地方說的是累計漂移, 
這裡寫圖片描述, 
為了解決漂移問題。我們還需要兩種檢測技術:後端優化和迴環檢測的。迴環檢測負責吧“”機器人回到原來的位置“”這件事情給檢測出來。而後端優化則是根據這個資訊,優化整個軌跡形狀

後端優化

後端優化主要是指在處理SLAM噪聲問題,雖然我希望所有的資料都是準確的,但是在現實當中,再精確的感測器也是有一定噪聲的。後端只要的考慮的問題,就是如何從帶有噪聲的資料中,估計出整個系統的狀態,以及這個狀態估計的不確定性有多大(最大後驗概率(MAP)) 
視覺里程計(前端)給後端提供待優化的資料。 
後端負責整體的優化過程,後端面對的只有資料,並不關係資料到底是來自哪個感測器。 
在視覺SLAM當中,前端和計算機視覺領域更為相關,例如:影象的特徵提取與匹配。後端只要是濾波和非線性優化演算法。

迴環檢測

迴環檢測(閉環檢測(Loop Closure Detection))主要是解決位置估計隨時間漂移的問題。為了實現迴環檢測,我們需要讓機器人具有識別曾經到達過場景的能力。通過判斷影象之間的相似性,來完成迴環檢測。如果迴環檢測成功,可以顯著地減小累計誤差。所以視覺迴環檢測,實際上是一種計算影象資料相似性的演算法。

建圖

建圖(Mapping)是指構建地圖的過程。地圖是對環境的描述,但是這個描述並不是固定。(如果是鐳射雷達的地圖,就是一個二維的地圖,如果是其他視覺slam 三維的點雲圖) 
地圖可以分成度量地圖和拓撲地圖兩種。 
1、度量地圖(Metric Map) 
度量地圖強調的是精確表示出地圖中物體的位置關係,通常我們用稀疏(Sparse)與稠密(Dense)對他們進行分類。稀疏地圖進行了一定程度的抽象,並不需要表達所有物體。例如:我們需要一部分有代表意義的東西(簡稱為路標landmark), 
這裡寫圖片描述 
這裡的稀疏地圖,就是又路標組成的地圖。 
相對於稀疏地圖而言,稠密地圖將建模所看到的所有的東西 
這裡寫圖片描述 
對於定位而言:稀疏的路標地圖就足夠了。而要用於導航,我們往往需要稠密地圖。 
稠密地圖通常按照解析度,由許多個小塊組成。二維的度量地圖是許多小格子(Grid),三維則是許多小方塊(Voxel)。一般而言,一個小方塊,有佔據,空閒,未知三種狀態,來表達這個格子內是不是有物體。 
一些用於視覺導航的演算法 A*, D*,演算法這種演算法需要地圖能夠儲存每個格點的狀態,浪費了大量的儲存空間。而且大多數情況下,地圖的很多細節是無用的。另外,大規模度量地圖有的時候回出現一致性問題。很小的一點轉向誤差,可能會導致兩間屋子之間的牆出現重疊,使得地圖失效。 
2、拓撲地圖(Topological Map) 
相比於度量地圖的精準性,拓撲地圖則更強調了地圖元素之間的關係。拓撲地圖是一個圖。這個圖是由節點和邊組成,只考慮節點間的連通性。它放鬆了地圖對精確位置的需要,去掉了地圖的細節問題,是一種更為緊湊的表達方式。 
如何在拓撲圖中,進行分割形成結點和邊,如何使用拓撲地圖進行導航和路徑規劃。

slam 的數學表述

通常機器人會攜帶一個測量自身運動的感測器,比如說碼盤,或者慣性感測器。運動方程可以表示為: 
這裡寫圖片描述 
這裡寫圖片描述 
與運動方程相互對應的觀測方程。 
這裡寫圖片描述 
假設機器人在平面中運動,他的位姿由兩個位置和一個轉角來描述,也就是這裡寫圖片描述 
同時如果運動感測器能夠檢測到機器人每兩個時間間隔位置和轉角的變換量這裡寫圖片描述那麼此刻的運動方程就可以具體化: 
這裡寫圖片描述 
這是比較理想的狀態下。現實是,並不是所有的感測器都可以直接測量出位置和角度的變化。 
假如機器人攜帶的是一個二維的鐳射雷達。我們知道鐳射感測器觀測一個2D路標是時候,能夠測量到兩個量,一個是路標邊到機器人之間的距離r 另一個是夾角這裡寫圖片描述,那麼我們寫出的觀測方程就是: 
這裡寫圖片描述 
在考慮視覺SLAM時候,觀測方程就是“對路標點拍攝後,得到了影象中的畫素”的過程。 
針對不同的感測器,這兩個方程有不同的引數化。如果我們保持通用性,吧他們們抽象成通用的抽象形式,那麼SLAM過程可總結為兩個基本過程: 
這裡寫圖片描述 
將SLAM問題建模成一個狀態估計問題。狀態估計問題的求解,與這個兩個方程的具體的形式有關,以及噪聲服從那種分佈有關。我們按照運動和觀測方程是否為線性,噪聲是否服從高斯分佈,分成線性/非線性和高斯/非高斯系統。其中線性高斯系統(Line Gaussian,LG系統)是最簡單的,他的無偏的最優估計可以由卡爾曼濾波器給出。而在非線性,非高斯的系統中,我們會使用擴充套件卡爾曼濾波器(EKF)和非線性優化兩大類方法求解它。直到21世紀早期,以EKF為主,佔據了SLAM的主導地位。到今天視覺SLAM主要都是以圖優化來進行狀態估計 
機器人一般都是6自由度,三維空間的運動由三個軸以及繞這個三個軸的旋轉量組成。一共有6個自由度。 
CMakeLists.txt

#“#”表示註釋
#宣告要求cmake的最低的版本
cmake_minimum_required(VERSION 2.8)
#宣告一個工程
project(HelloSlam)
#新增一個可執行檔案
#語法
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7

第三講 三維空間的剛體運動

這裡寫圖片描述 
這裡寫圖片描述 
這裡寫圖片描述 
什麼叫旋轉向量:使用一個向量,他的方向和旋轉軸一致,長度等於旋轉角。(任意的一個旋轉,都可以用旋轉軸和旋轉角來表示)這就是軸角Axis-Angle

 Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/2, Eigen::Vector3d(0,0,1)).toRotationMatrix();
  • 1

任意的旋轉都可以使用角軸來描述,那麼前面的這個參數列示的旋轉的角度,後面的這個引數旋轉向量

然後我們現在已知了旋轉矩陣,通過這裡寫圖片描述公式求得特殊正交群。 
在程式碼中是這樣寫的

Sophus::SO3 SO3_R(R);   // Sophus::SO(3)可以直接從旋轉矩陣構造 由旋轉矩陣得到的
  • 1

當然還有以下幾種寫法

 Sophus::SO3 SO3_R(R);      // Sophus::SO(3)可以直接從旋轉矩陣構造 由旋轉矩陣得到的
Sophus::SO3 SO3_v( 0, 0, M_PI/2 );  // 亦可從旋轉向量構造
Eigen::Quaterniond q(R);            // 或者四元數 這裡是將R變成一個四元數
Sophus::SO3 SO3_q( q );
  • 1
  • 2
  • 3
  • 4

如何輸出一個四元數

    cout<<"q="<<q.coeffs()<<endl;
  • 1

這裡coeffs輸出的順序是(x,y,z,w)前面的x,y,z為虛部 w是實部 
這裡寫圖片描述

    Eigen::Vector3d t(1,0,0);           // 沿X軸平移1
Sophus::SE3 SE3_Rt(R, t);           // 從R,t構造SE(3)
Sophus::SE3 SE3_qt(q,t);            // 從q,t構造SE(3)
cout<<"SE3 from R,t= "<<endl<<SE3_Rt<<endl;
cout<<"SE3 from q,t= "<<endl<<SE3_qt<<endl;
cout<<"se3 hat = "<<endl<<Sophus::SE3::hat(se3)<<endl;
cout<<"se3 hat vee = "<<Sophus::SE3::vee( Sophus::SE3::hat(se3) ).transpose()<<endl;
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7

總結: 
這裡寫圖片描述


一個剛體在三維空間中的運動是如何描述的。 
平移是比較好描述的,旋轉這件事比較困難。如何來描述旋轉,我們使用:旋轉矩陣、四元數、尤拉角 
剛體:有位置資訊,還需要有自身的姿態(姿態:就是相機的朝向) 
結合起來:相機為與空間上(0,0,0)處,面朝正前方 
這是關於座標系,我們通過座標系來描述一個向量。 
這裡寫圖片描述 
外積就是將外積表示式拆開成為第2個式子,然後再a拆開,b表示列向量就變成a^b 
a^表示把a這個向量變成一個矩陣,這個矩陣是a的反對稱矩陣, 
反對稱矩陣就是 矩陣a的轉置等於-a 
這裡a與b的外積是一個行列式, 
這裡寫圖片描述 
外積的物理意義是a和b組成的平行四邊形的的面積

現在存在一個問題,同一個向量在不同的座標系裡面座標該怎麼表示?

在機器人運動過程中,我們設定的是慣性座標系(世界座標系),可以認為它是固定不動的 
ZW,XW,YW是世界座標系,ZC,XC,YC是相機的座標系,P在相機座標系中的座標是PC,在世界座標系中的座標是PW,首先我們得到在相機座標系中的 pc 然後通過變換矩陣T 來變換到PW

正交矩陣就是逆等於轉置

這裡寫圖片描述 
什麼叫歐式變換?就是說同一個向量,在各個座標系下的長度和夾角都不會發生改變。

歐式變換由一個旋轉和一個平移兩個部分組成(前提是剛體) 
可以用旋轉矩陣,來描述相機的旋轉,旋轉矩陣是一個行列式為1的正交矩陣。我們可以吧所有的旋轉矩陣的集合定義如下: 
這裡寫圖片描述 
SO(n)是特殊正交群,這個集合由n維空間旋轉矩陣組成,其中SO(3)就是三維空間的旋轉,根據正交矩陣的性質,正交矩陣的逆和轉置是一樣的。 
因此歐式空間的座標變換可以寫成:(一次旋轉+一次平移) 
這裡寫圖片描述 
但是這個表示式是非線性的,因此我們需要重寫 
1、引入齊次座標(也是用4個數來表達三維向量的做法) 
這裡寫圖片描述 
在三維向量的末尾新增1,成為4維向量,稱為齊次座標 
這裡要區分特殊歐式群和特殊正交群 
這裡寫圖片描述 
實踐部分,新增eigen的庫

#include_directories("/usr/include/eigen3")
  • 1

不需要加入target_link

宣告部分

Eigen只需要關心前面3個引數,分別是:資料型別,行,列
宣告一個2行3列的float型別的矩陣
Eigen::Matrix<float,2,3>matrix_23;
宣告一個3行1列的向量 這裡的d表示double
Eigen::Vector3d v_3d;
宣告一個double型別的3*3的矩陣,並且初始化為零
Eigen::Matrix3d matrix_33=Eigen::Matrix3d::zero();
當不確定矩陣的大小的時候,可以申請動態矩陣
Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamix> matrix_dynamc;
同樣,可以表示為:
Eigen::MatrixXd matrix_x;
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11

賦值部分

為矩陣賦值
matrix_23<<1,2,3,4,5,6;
  • 1
  • 2
  • 3

訪問矩陣內部儲存的數值

for(int i=0;i<2;i  ){
for(int j=0;j<3,j  ){
cout<<matrix_23(i,j)<<"\t";
}
cout<<endl;
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6

矩陣的四則運算

v_3d<<3,2,1;
vd_3d<<4,5,6;
矩陣的型別是一樣
Eigen::Matrix<double,2,1>result=matrix_23.cast<double>()*v_3d;//輸入想要改變矩陣的型別
在一個矩陣的後面 .cast<double>() 將原來的float轉化的double型別
Eigen::Matrix<float,2,1>result2=matrix_23*vd_3d;//預設條件下矩陣的型別
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6

矩陣的性質

產生隨機數矩陣
matrix_33=Eigen::Matrix3d::Random();
求這個矩陣的轉置
cout<<matrix_33.transpose()<<endl;
求這個矩陣各個元素的和
cout<<matrix_33.sum()<<endl;
求這個矩陣的跡(主對角線元素之和)
cout<<matrix_33.trace()<<endl;
求這個矩陣的數乘
cout<<matrix_33*10<<endl;
求這個矩陣的逆
cout<<matrix_33.inverse()<<endl;
求這個矩陣的行列式
cout<<matrix_33.determinant()<<endl;
求解特徵值和特徵向量
Eigen::SelgAdjointEigenSlover<Eigen::Matirx> eigen_solver(matrix_33);
cout<<eigen_solver.eigenvalues()<<endl;
cout<<eigen_solver.eigenvectors()<<endl;
求解方程:例如求解 NN*X=V_Nd 這個方程
首先定義這幾個變數
Eigen::Matrix< double, MATRIX_SIZE, MATRIX_SIZE > matrix_NN;
matrix_NN = Eigen::MatrixXd::Random( MATRIX_SIZE, MATRIX_SIZE );
Eigen::Matrix< double, MATRIX_SIZE,  1> v_Nd;
v_Nd = Eigen::MatrixXd::Random( MATRIX_SIZE,1 );
clock_t time_stt=clock();//用來計時
第一種方法:直接求逆
Eigen::Matrix<double,MATRIX_SIZE,1> x = matrix_NN.inverse()*v_Nd;
然後輸出計算的時間
cout <<"time use in normal invers is " << 1000* (clock() - time_stt)/(double)CLOCKS_PER_SEC << "ms"<< endl;
第二種方法:使用QR分解來求解
time_stt = clock();
x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
cout <<"time use in Qr compsition is " <<1000*  (clock() - time_stt)/(double)CLOCKS_PER_SEC <<"ms" << endl;
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33

當矩陣的規模龐大的時候,使用QR分解更加高效 
QR分解也叫做正交三角分解,把矩陣分解成一個正交矩陣與一個上三角矩陣的積。通常用來求解線性最小二乘法的問題 
QR求解的方法:

  • Givens變換求矩陣的QR分解
  • Householder變換又稱為反射變換或者映象變換
  • Gram-Schmidt正交化

由於旋轉矩陣是用9個量來描述旋轉的,並且有正交性約束和行列式值約束 
我們希望能夠一種更加緊湊的方式來系統的描述旋轉和平移 
因此,我們換了一種方式進行比較,就是需旋轉軸和旋轉角因此我們使用一個向量,這個向量的方向和旋轉軸一致,長度等於旋轉角,這種向量,稱為旋轉向量(或者軸角,Axis-Angle),也可以叫做角軸, 這種方法的優勢在於,只需要一個三維的向量,就可以描述旋轉。 
作為一個承上啟下的部分,旋轉向量和旋轉矩陣是如何轉化呢?假設有一個旋轉軸為n,角度為A,那麼他對應的旋轉向量是An,由軸角到旋轉矩陣的過程由於羅德里格斯公式表明: 
這裡寫圖片描述 
因此 旋轉矩陣到軸角的表示方法: 
這裡寫圖片描述 
這裡寫圖片描述

尤拉角

優勢:直觀 
尤拉角使用的三個分離的轉角,把一次旋轉分解成三次繞不同的軸的旋轉 
其中較為常用的ZYX軸旋轉 
尤拉角會出現奇異值(萬向鎖問題)

  1. 物體繞Z軸旋轉之後得到偏航角yaw
  2. 繞Y軸之後得到俯仰角pitch
  3. 繞X軸旋轉之後得到滾動角roll 
    因此我們可以使用[r,p,y]來表示任意的旋轉 rpy角的旋轉順序是ZYX

RPY的參考連結: 
http://blog.csdn.net/heroacool/article/details/70568858 
缺陷:這種方法會碰到萬向鎖的問題,在俯仰角pitch為+- 90度的時候,第一次旋轉和第三次旋轉使用的同一個軸,使得系統丟失一個自由度。也成為萬向鎖 
由於這個問題,尤拉角不適合用於插值和迭代,往往只是用於人機互動中,同樣也不會在濾波和優化中使用尤拉角來表達旋轉(因為具有奇異性)

四元數

旋轉矩陣用9個量,來描述3自由度的旋轉,具有冗餘性,而尤拉角和旋轉向量具有奇異性 
在工程當中使用的四元數來儲存機器人軌跡和旋轉 
例如,當我們想要用二維的量來表示地球表面的時候,必然會出現奇異性,例如,經緯度在+-90度的地方,毫無意義,因此想要表示三維的剛體的時候,我們就打算用四元數。四元數的優勢在於1、緊湊,2、沒有奇異性 
一個四元數擁有一個實部和3個虛部 本質上是一種擴充套件複數,描述3維的量,用四元數 
這裡寫圖片描述 
這裡寫圖片描述 
這裡寫圖片描述 
如何用四元數來表示三維剛體的旋轉 
假設,某個旋轉是繞單位向量 
這裡寫圖片描述 
結論在四元數中,任意旋轉都可以有兩個互為相反數的四元數來表示。 
四元數到旋轉矩陣的變換方式 
這裡寫圖片描述 
反之,由旋轉矩陣到四元數的變換如下:假設矩陣R={mij},其對應的四元數是: 
這裡寫圖片描述

相似、仿射、射影變換

這三種變換都會改變原來物體的外形 
相似變換比歐式變換對了一個自由度,它允許物體進行均勻的縮放,其矩陣表示為: 
這裡寫圖片描述 
其中旋轉矩陣部分多了一個縮放因子s,可以將x,y,z在三個座標軸上面進行縮放 
仿射變換 
這裡寫圖片描述 
A只是要求是一個可逆矩陣,不要求是正交矩陣,仿射變換後的立方體就不再是方的買單時各個面仍然是平行四邊形。 
射影變換 
這裡寫圖片描述 
這是一個最一般的形式,一個正方形經過射影變換,最後變成一個規則的四邊形 
總結: 
這裡寫圖片描述 
從真實世界到相機照片變換的過程就是一個射影變換。如果相機的焦距為無窮遠的時候,那麼這個變換是仿射變換。 
實踐部分:Eigen的幾何模組

首先新增標頭檔案#include<Eigen/Geometry>
Eigen/Geometry提供了各種旋轉和平移的表示
宣告一個3維的旋轉矩陣
Eigen::Matrix3d rotation_matrix=Eigen::Matrix3d::Identity();
宣告旋轉向量,並且沿Z軸旋轉45度
Eigen::AngleAxisd rotation_vecotr (M PI/4,Eigen::Vector3d(0,0,1));
cout .precision(3);//輸出返回3位有效數字
cout<<rotation_vector.matrix()<<endl;
將旋轉向量賦值給另外一個向量
rotation_matrix=rotation_vector.toRotationMatrix();
通過跟一個向量相乘從而進行座標變換
Eigen::Vector3d v(1,0,0);
Eigen::Vector3d v_rotated=rotation_vector *v;
cout<<v_rotated.transport()<<endl;//將(0,0,1)旋轉之後的向量轉置輸出
將一個向量(1,0,0)使用旋轉矩陣之後的輸出
v_rotated=rotation_matrix*v;//其實說白了,就是讓這個旋轉矩陣和這個向量相乘,然後重視[3*3]*[3*1]最後輸出的結果就是3*1
cout<<v_rotated.transport()<<endl;//將(0,0,1)旋轉之後的向量轉置輸出
可以將旋轉矩陣直接轉化成尤拉角,其實尤拉角存在的意義,只是為了方便人們理解,在程式當中,我們處理問題一般是用的四元數
Eigen::Vector3d  euler_angles=rotation_matrix.eulerAngles(2,1,0);//這是輸出的預設的資料是roll pitch yaw
如果逆向輸出的順序是yaw pitch roll,那麼
cout<<"yaw pitch roll"<<euler_angles.transport()<<endl;
歐式變換陣 使用Eigen::Isometry
說的是三維空間下的變換Isometry3d
Eigen::Isometry T=Eigen::Isometry3d::Identity();
T.rotate(rotation_vector);//按照rotation_vector的方式進行旋轉
T.pretranslate(Eigen::Vector3d(1,3,4));//吧這個向量進行平移
cout<<T.matrix()<<endl;
幾種座標變換的方式:1、用變換矩陣進行座標變換
Eigen::Vector3d v_transformed=T*v;
關於四元數
Eigen::Quaterniond q=Eigen::Quaterniond(rotation_vector);
cout<<q.coeffs()<<endl;//這裡coeffs輸出的順序是(x,y,z,w)前面的x,y,z為虛部 w是實部
同樣也可以把旋轉矩陣賦給他
q=Eigen::Quateriond(rotation_matrix)
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34

visualize_Geometry

#include<iomanip>
  • 1

這個標頭檔案對cin和cout這種操作符進行控制,例如可以控制輸出位數,左對齊,右對齊等。

#include <pangolin/pangolin.h>
  • 1

pangolin是SLAM視覺化的openGL的庫


第四講 李群李代數

由於相機的位姿是未知的,我們需要解決什麼樣的相機位姿最符合當前關鍵資料這樣的問題 
通過李群李代數之間的轉換關係,我們希望把位姿估計變成無約束的優化問題。 
特殊正交群SO(3),是由三維旋轉矩陣構成的;特殊歐式群SE(3)是變換矩陣構成的 
特殊正交群SO(3)就是,3*3的矩陣,然後正交,行列式為1,其定義是: 
這裡寫圖片描述 
特殊歐式群SE(3),是在這個3*3矩陣基礎上,轉化成一個齊次的矩陣,為了避免非齊次帶來的不便,其定義如下 
這裡寫圖片描述 
加法的封閉性a屬於集合A,b也屬於集合A,如果a+b也是屬於集合A的話,這就叫做加法的封閉性 
但是旋轉矩陣和變換矩陣是沒有加法封閉性的,但是他們的乘法是封閉的,也就是說 
這裡寫圖片描述 
兩個旋轉矩陣相乘表示做了兩次旋轉。對於這種只有一個運算的集合,我們把他叫做群。 
群 
群是一種集合加上一種集合運算的代數結構,我們把集合記作A,運算子號記作 · ,那麼一個群可以記作 G=(A, ·),群這種運算要求滿足以下幾個條件: 
這裡寫圖片描述 
s.t.表示約束條件 
總結:旋轉矩陣和矩陣的乘法運算構成旋轉矩陣群;變換矩陣和矩陣乘法構成變換矩陣群 
李群 Lie Group 
李群,是指的具有連續(光滑)性質的群,SO(n),SE(n)是李群。因為每個李群都有對應的李代數因此,我們先引出李代數so(n) 
李代數的引出 
我們每次想要求一個旋轉矩陣的導數,就可以左乘一個矩陣這裡寫圖片描述即可, 
我們根據導數的定義,在R(t)的0附近進行一階泰勒展開式 
這裡寫圖片描述 
他是推到了,如果一個導數等於他本身,那麼這個導數就是e^x這個函式,即: 
這裡寫圖片描述 
這裡寫圖片描述 
每個李群都對應一種李代數 
由於每個李群都有李代數,李代數描述了李群的區域性性質。 
李代數由一個集合V,一個數域F,和一個二以下元運算[,]組成,如果他們滿足以下幾條性質,稱(V,F,[,])為一個李代數,記作 g 
這裡寫圖片描述 
[ ]這個運算子叫做李括號,要求元素和自己做李括號這種運算之後為0,也即是[x,x]=0,李括號直觀上來講是表達了兩個元素之間的差異 
李代數so(3) 
這裡寫圖片描述在這個式子總的這裡寫圖片描述就是SO(3)對應的李代數,記作so(3) 
so(3)是一個三維的向量組成的集合,每個向量都對應著一個反對稱的矩陣,來表達旋轉矩陣的導數 
這裡寫圖片描述 
SO(3)和so(3)之間的對映關係是: 
這裡寫圖片描述 
同理:SE(3)也是有對應的李代數的se(3) 
這裡寫圖片描述 
他是一個6維的向量,前三維表示平移,記作這裡寫圖片描述,後面三維表示旋轉,記作:這裡寫圖片描述, 
這裡寫圖片描述 
SO(3)上的指數對映的物理意義就是旋轉向量 
為什麼我們要有李代數,因為我們需要解決李群上面只有乘法沒有加法這個問題。 
利用BCH線性近似,來推導so(3)和se(3)上的導數和擾動波形,通常情況下,擾動模型比較簡潔,更加常用。

實踐部分:使用sophus庫 
編譯部分:到高博的3rdparty,然後解壓sophus,然後mkdir build cd build cmake .. make 就可以了。 
cmake .. 編譯的時候出現這幅圖是正常的。當還以為有問題呢?看來是多慮的 
這裡寫圖片描述


視覺SLAM第5講

這裡寫圖片描述 
opencv各個版本的github上的連結: 
https://github.com/opencv/opencv/releases 
安裝opencv3.1.0遇到的問題: 
這裡寫圖片描述 
找到的解決方案: 
http://blog.csdn.net/huangkangying/article/details/53406370 
然後

// 關於 cv::Mat 的拷貝
// 直接賦值並不會拷貝資料
cv::Mat image_another = image;
// 修改 image_another 會導致 image 發生變化
image_another ( cv::Rect ( 0,0,100,100 ) ).setTo ( 0 ); // 將左上角100*100的塊置零
cv::imshow ( "image", image );
cv::waitKey ( 0 );
// 使用clone函式來拷貝資料
cv::Mat image_clone = image.clone();
image_clone ( cv::Rect ( 0,0,100,100 ) ).setTo ( 255 );
cv::imshow ( "image", image );
cv::imshow ( "image_clone", image_clone );
cv::waitKey ( 0 );
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15

第一種方式,拷貝資料的話是淺拷貝,就是說,你如果該資料,那麼原始的資料也會改變。 
第二種方式,在拷貝資料的時候用image.clone()那麼這樣不會修改原始的資料。 
Ubuntu16.04須知: 
安裝pcl

sudo apt-get install libpcl-dev pcl-tools
  • 1

並且在使用的時候,cmakelist需要這麼寫: 
這裡寫圖片描述 
joinMap.cpp 
用來讀寫檔案的

    #include<fstream>
  • 1

將影象都放到一個容器當中,<這裡面試資料型別>

    vector<cv::Mat> colorImgs, depthImgs;    // 彩色圖和深度圖
vector<Eigen::Isometry3d> poses;         // 相機位姿
  • 1
  • 2

利用fin來讀取txt當中的引數

    ifstream fin("./pose.txt");//匯入相機的位置和姿態的資料
  • 1

讀取資料流 
這裡寫圖片描述

為了使用boost::format

#include <boost/format.hpp>  
  • 1

boost::format格式化字串

這裡寫圖片描述 
這個兩個的效果是一模一樣的。 
這裡寫圖片描述

boost::format fmt( "./%s/%d.%s" ); //影象檔案格式
  • 1

容器的基本使用:http://blog.csdn.net/ws_20100/article/details/50829327 
這裡.push_back是在這個容器的尾部插入、push_back(圖片)

 colorImgs.push_back( cv::imread( (fmt%"color"%(i 1)%"png").str() ));
depthImgs.push_back( cv::imread( (fmt%"depth"%(i 1)%"pgm").str(), -1 )); // 使用-1讀取原始影象
  • 1
  • 2

這裡用來遍歷data的

 double data[7] = {0};//初始化
for ( auto& d:data )
fin>>d;
  • 1
  • 2
  • 3

參考連結:c++11 for http://blog.csdn.net/hackmind/article/details/24271949 
將影象的外參

 for ( int i=0; i<5; i   )
{
boost::format fmt( "./%s/%d.%s" ); //影象檔案格式
colorImgs.push_back( cv::imread( (fmt%"color"%(i 1)%"png").str() ));
depthImgs.push_back( cv::imread( (fmt%"depth"%(i 1)%"pgm").str(), -1 )); // 使用-1讀取原始影象
//將相機的外引數,都給了T
double data[7] = {0};
for ( auto& d:data )
fin>>d;//將上面的每個圖片的外參都給了d
Eigen::Quaterniond q( data[6], data[3], data[4], data[5] );
Eigen::Isometry3d T(q);//定義一個歐式變換矩陣,
T.pretranslate( Eigen::Vector3d( data[0], data[1], data[2] ));
poses.push_back( T );//將這裡的資料,新增的相機的位子當中,進行重新的排序
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15

在點雲拼接部分:

opencv當中顏色通道是bgr的順序

        cv::Mat color = colorImgs[i]; 
cv::Mat depth = depthImgs[i];
Eigen::Isometry3d T = poses[i];
for ( int v=0; v<color.rows; v   )
for ( int u=0; u<color.cols; u   )
{
unsigned int d = depth.ptr<unsigned short> ( v )[u]; // 深度值
if ( d==0 ) continue; // 為0表示沒有測量到
Eigen::Vector3d point; //宣告一個3維的列向量
point[2] = double(d)/depthScale; 
point[0] = (u-cx)*point[2]/fx;
point[1] = (v-cy)*point[2]/fy; 
Eigen::Vector3d pointWorld = T*point;
//XYZRGB,在opencv當中,                
PointT p ;
p.x = pointWorld[0];
p.y = -pointWorld[1];
p.z = pointWorld[2];
p.b = color.data[ v*color.step u*color.channels() ];
p.g = color.data[ v*color.step u*color.channels() 1 ];
p.r = color.data[ v*color.step u*color.channels() 2 ];
pointCloud->points.push_back( p );
}
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25

這裡寫圖片描述

opencv當中的 .step參數列示的實際畫素的寬度 .channel()表示的是,通道的提取


關於影象上下翻轉問題,是因為opencv定義的座標系和pcl_viewer顯示座標系不同,opencv是x右y下,而pcl顯示是x右y上。解決方法:找到群主程式函式中,把計算點空間座標的公式的p.y值新增負號,這樣y方向就可以正常顯示了,so easy。(或許還有別的方法) 
這裡寫圖片描述 
最後儲存檔案

cout<<"點雲共有"<<pointCloud->size()<<"個點."<<endl;
pcl::io::savePCDFileBinary("map.pcd", *pointCloud );
  • 1
  • 2

補充:歸一化平面 
這裡寫圖片描述


視覺SLAM和鐳射SLAM的區別在於,觀測模型不同。(他們的運動模型是相同的) 
觀測模型當中最常用的就是針孔相機模型 
相機能夠將三維世界中的座標點(單位米)對映到二維影象平面(單位畫素)的過程用一個幾何模型進行討論。

針孔相機模型

針孔相機模型,就是利用了相似三角形的知識 
這裡寫圖片描述 
徑向畸變由透鏡形狀引起的畸變,就是徑向畸變 
主要分成兩大類:桶性畸變和枕形畸變 
桶形畸變是由於影象放大率隨著離光軸的距離的增加而減小

這裡寫圖片描述

枕形畸變是影象的放大率隨著光軸距離的增加而增加

這裡寫圖片描述

切向畸變:相機在組裝的過程中由於 透鏡和成像平面不是嚴格平行的 導致的切向畸變

這裡寫圖片描述

畫素座標系的定義: 
畫素座標系與成像平面之間的差別: 
相差了一個縮放和原點的平移 
這裡寫圖片描述 
這裡寫圖片描述 
這裡寫圖片描述 
通常我們把中間的量組成的矩陣稱為相機的內引數矩陣K。通常認為相機在內參在出廠之後是固定不變的,有的卻沒有,這個時候就是需要標定一下,但是我覺得標定這件事 
這裡寫圖片描述 
這裡寫圖片描述 
這裡寫圖片描述 
這裡寫圖片描述

畸變

RGB-D相機模型

目前的RGB-D相機按照原理可以分成兩類 
1、通過紅外光結構(structured light)來測量畫素的距離,例如:Kinect1 Project Tango1、Intel Realsense 
這裡寫圖片描述 
2、通過飛行時間法(Time-of-light,ToF)原理來測量畫素的的距離。例如有KInect2和一些Tof感測器 
這裡寫圖片描述 
這裡寫圖片描述

標定K就是相機的內參


視覺SLAM第6講狀態估計的問題

總結 
這裡寫圖片描述 
這裡寫圖片描述 
這裡寫圖片描述 
這裡寫圖片描述 
安裝g2o使用的是高博的slambook中的g2o 
安裝了依賴項之後編譯好順利啊

sudo apt-get install libeigen3-dev libsuitesparse-dev libqt4-dev qt4-qmake 
然後cd 進g2o
mkdir build
cd build
cmake ..
make -j8
sudo make install
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7

安裝cerse

sudo apt-get install liblapack-dev libsuitesparse-dev libcxsparse3.1.2 libgflags-dev libgoogle-glog-dev libgtest-dev 
然後cd 進cerse
mkdir build
cd build
cmake ..
make -j8
sudo make install
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7

實踐部分,我們已知帶有噪聲的x,y, 我們想要求得引數a,b,c 
這裡寫圖片描述

// 代價函式的計算模型
struct CURVE_FITTING_COST
{
CURVE_FITTING_COST ( double x, double y ) : _x ( x ), _y ( y ) {}
// 殘差的計算 初始化
template <typename T>
bool operator() (
const T* const abc,     // 模型引數,有3維
T* residual ) const     // 殘差
{
residual[0] = T ( _y ) - ceres::exp ( abc[0]*T ( _x ) *T ( _x )   abc[1]*T ( _x )   abc[2] ); // y-exp(ax^2 bx c) 這就是誤差
return true;
}
const double _x, _y;    // x,y資料
};
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
ceres::Problem problem;
for ( int i=0; i<N; i   )
{
problem.AddResidualBlock (     // 向問題中新增誤差項
// 使用自動求導,模板引數:誤差型別,輸出維度,輸入維度,維數要與前面struct中一致
new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3> ( 
new CURVE_FITTING_COST ( x_data[i], y_data[i] )
),
nullptr,            // 核函式,這裡不使用,為空
abc                 // 待估計引數
);
}
// 配置求解器
ceres::Solver::Options options;     // 這裡有很多配置項可以填
options.linear_solver_type = ceres::DENSE_QR;  // 增量方程如何求解 指定QR分解
options.minimizer_progress_to_stdout = true;   // 輸出到cout 
ceres::Solver::Summary summary;                // 優化資訊
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
ceres::Solve ( options, &problem, &summary );  // 開始優化
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );
cout<<"solve time cost = "<<time_used.count()<<" seconds. "<<endl;
// 輸出結果
cout<<summary.BriefReport() <<endl;
cout<<"estimated a,b,c = ";
for ( auto a:abc ) cout<<a<<" ";
cout<<endl;
return 0;
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32

輸出結果

Ceres Solver Report: Iterations: 22, Initial cost: 1.824887e 04, Final cost: 5.096854e 01, Termination: CONVERGENCE
estimated a,b,c = 0.891943 2.17039 0.944142 
  • 1
  • 2

迭代了22次,最開始的誤差是Initial cost: 1.824887e+04 
最後的誤差是:inal cost: 5.096854e+01,最後收斂的原因是,Termination: CONVERGENCE,函式收斂了。 
最後得出a,b,c的數值是estimated a,b,c = 0.891943 2.17039 0.944142

優化處理噪聲資料 
sfm : 就是從很多副影象中進行三維重建 struct of motion 
想要跟高博一樣,畫出比價好看的圖,寫論文。 
這裡寫圖片描述

data=load('C:\Users\Administrator\Desktop\d0.txt');
x=data(:,1);
y=data(:,2);
scatter(x,y,10,'o')
  • 1
  • 2
  • 3
  • 4

scatter用於畫散點圖、 
X和Y是資料向量,以X中資料為橫座標,以Y中資料位縱座標描繪散點圖,點的形狀預設使用圈。、 
這裡寫圖片描述 
scatter(x,y,10,’filled’) 
這裡寫圖片描述 
scatter3(x,y,z)描述的三維影象 
其實簡單點:

plot(x,y,'o')
  • 1

將點狀圖和曲線擬合圖結合起來

data=load('C:\Users\Administrator\Desktop\d0.txt');
x=data(:,1);
y=data(:,2);
a=0.891943;
b=2.17039;
c=0.944142;
y1=exp(a*x.^2 b*x c);%注意這裡是點乘,相當於把矩陣中每個數字都平方。不能用* ,因為*表示的矩陣的乘法
figure(1)
plot(x,y,'o',x,y1)
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9

這裡寫圖片描述 
總結:其實有用的資料就是要求的a,b,c三個引數,然後我帶迭代的次數,和如何迭代說明一下,就可以了

關於g2o求解同樣的問題: 
將優化問題用圖的方式表示出來。一個圖有若干個頂點(Vertex),以及連線這些節點的邊(Edge),我們用頂點表示需要優化的變數,用邊表示誤差項。求最短路徑,和無向圖,就是找關於最短路徑的邊,是的求解問題最簡單 
g2o實現

// 構建圖優化,先設定g2o
typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block;  // 每個誤差項優化變數維度為3,誤差值維度為1
Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); // 線性方程求解器
Block* solver_ptr = new Block( linearSolver );      // 矩陣塊求解器
  • 1
  • 2
  • 3
  • 4
  // 梯度下降方法,從GN, LM, DogLeg 中選
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );
// g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr );
// g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( solver_ptr );
g2o::SparseOptimizer optimizer;     // 圖模型
optimizer.setAlgorithm( solver );   // 設定求解器
optimizer.setVerbose( true );       // 開啟除錯輸出
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
 // 往圖中增加頂點
CurveFittingVertex* v = new CurveFittingVertex();
v->setEstimate( Eigen::Vector3d(0,0,0) );
v->setId(0);
optimizer.addVertex( v );
  • 1
  • 2
  • 3
  • 4
  • 5
// 往圖中增加邊
for ( int i=0; i<N; i   )
{
CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] );
edge->setId(i);
edge->setVertex( 0, v );                // 設定連線的頂點
edge->setMeasurement( y_data[i] );      // 觀測數值
edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) ); // 資訊矩陣:協方差矩陣之逆
optimizer.addEdge( edge );
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
// 執行優化
cout<<"start optimization"<<endl;
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
optimizer.initializeOptimization();
optimizer.optimize(100);
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );
cout<<"solve time cost = "<<time_used.count()<<" seconds. "<<endl;
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
// 輸出優化值
Eigen::Vector3d abc_estimate = v->estimate();
cout<<"estimated model: "<<abc_estimate.transpose()<<endl;
  • 1
  • 2
  • 3
class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
virtual void setToOriginImpl() // 重置
{
_estimate << 0,0,0;
}
virtual void oplusImpl( const double* update ) // 更新
{
_estimate  = Eigen::Vector3d(update);
}
// 存檔和讀盤:留空
virtual bool read( istream& in ) {}
virtual bool write( ostream& out ) const {}
};
// 誤差模型 模板引數:觀測值維度,型別,連線頂點型別
class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
CurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {}
// 計算曲線模型誤差
void computeError()
{
const CurveFittingVertex* v = static_cast<const CurveFittingVertex*> (_vertices[0]);
const Eigen::Vector3d abc = v->estimate();
_error(0,0) = _measurement - std::exp( abc(0,0)*_x*_x   abc(1,0)*_x   abc(2,0) ) ;
}
virtual bool read( istream& in ) {}
virtual bool write( ostream& out ) const {}
public:
double _x;  // x 值, y 值為 _measurement
};
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37

視覺slam第7講視覺里程計1

ICP是指,利用點雲的匹配關係,來求解相機的三維運動。 
PNP是利用已知的三維結構與影象的對應關係,來求解相機的三維運動。 
對極幾何,利用對極幾何的約束,恢復出影象之間的相機的三維運動。 
視覺里程計,分成特徵提取和不提取特徵,特徵提取執行穩定,對光照,動態物體不敏感。 
我們就是用影象的特徵來代表這個影象。特徵點在相機運動後保持穩定。影象的角點就是影象的特徵。 
角點的侷限:例如:從遠處上看上去是角點的地方,當相機走近後,可能就是角點了。或者,當旋轉相機的時候,角點的外觀會發生變換。 
進而,我們提出了SIFT,SURF,ORB特徵。 
特徵點:關鍵點(key-point)和描述子組成。 
當我們談論SIFT特徵的時候,是指的提取SIFT關鍵點,並且計算SIFT描述子。關鍵點是指的特徵點在影象中的位置,有些特徵點具有朝向和大小的等資訊。描述子通常是一個向量,這個向量是人為設計的,描述了該關鍵點周圍畫素的資訊。 
描述子是按照:外觀相似的特徵應該具有相似的描述子的原則設計的。 
因此,只要兩個特徵點的描述子在向量空間上的距離相近,就可以認為他們是同樣的特徵點。 
**FAST只有關鍵點,是沒有描述子的。**ORB特徵則是目前最具有代表性的實時影象特徵,他改進了FAST檢測子不具有方向性的問題。並且採用速度極快的二進位制描述子BRIEF,使得整個影象的特徵提取環節大大加速。 
ORB特徵也是採用的關鍵點和描述子兩部分組成。*他的關鍵點是“Oriented FAST”是一種改進的FAST角點。他的描述子是BRIEF。* 
FAST是一種角點,主要檢測區域性畫素灰度變化明顯的地方。他的思想是:如果一個畫素與他的鄰域內的畫素差別較大,過亮或者過暗,那麼他就是角點。FAST只需要比較畫素的大小,因此十分快捷。 
FAST角點提取:提取出FAST角點,計算了特徵點的主方向。 
這裡寫圖片描述 
FAST是一種角點,他的思路是: 
1、在影象中選取畫素p,假設他的亮度是IP 
2、設定一個閾值T,例如T=IP*20% 
3、以畫素p為中心,選取半徑為3的圓上的16個畫素點, 這裡的3應該是三個畫素框 
4、如果在選取的圓上面,有連續N個點的亮度值大於IP+T(1.2*IP))或者小於IP-T(0.8*IP),那麼畫素p通常可以被認為是特徵點(N通常取12,FAST-12,通常n取9和11,的時候,稱為FAST-9和FAST-11) 
FAST-12演算法中為了能夠更加高效的提取特徵點,新增一個預測試的技術,能夠快速地排除大多數不是角點的元素,對於每個畫素,直接檢測在鄰域圓上的第1,5,9,13個畫素的亮度,只有當這四個畫素當中有三個同時大於IP+T或者小於IP-T的時候,當前畫素才有可能是是角點。FAST特徵點的計算,僅僅是比較畫素間亮度的差異,速度非常快,但是也存在一些問題:FAST特徵點的數量很多,並且不是確定,而大多數情況下,我們希望能夠固定特徵點的數量。 
在ORB當中,我們可以指定要提取的特徵點數量。對原始的FAST角點分別計算Harris的響應值,然後選取前N個點具有最大相應值的角點,作為最終角點的集合。 
FAST角點不具有方向資訊,由於他取自半徑為3的圓,存在尺度問題:從遠處看是角點,但是近看卻不是角點(就像是影象的金字塔)。因此ORB當中新增了尺度和旋轉的描述。尺度不變性構建的影象的金字塔,並且從每一層上面來檢測角點。旋轉性是由灰度質心法實現。 
質心是指以影象塊灰度值作為權重的中心。(目標是為找找到方向) 
1、在一個小的影象塊B中,定義影象塊的矩為: 
這裡寫圖片描述 
2、通過矩找到影象塊的質心 
這裡寫圖片描述 
3、連線影象塊的幾何中心o與質心C,得到一個oc的向量,把這個向量的方向定義特徵點的方向 
這裡寫圖片描述 
**總結:**Oriented FAST的方向就是幾何中心指向質心的方向 
在尺度不變的問題上,採用影象金字塔,進行非極大值抑制。 
BRIEF描述子 
我們在提取到 Oriented FAST關鍵點後,我們需要對每個關鍵點計算其描述子。 
BRIEF是二進位制描述子,它的描述向量是由許多個0和1組成,這裡的0和1編碼了關鍵點附近的兩個畫素p和q的大小關係,如果P>Q,則取1 ;如果P

#include<iostream>
#include<opencv2/core/core.hpp>
//特徵提取的模組
#include<opencv2/features2d/features2d.hpp>
#include<opencv2/highgui/highgui.hpp>
int main()
{
//載入影象
Mat img_1=imread("");
//初始化特徵點和描述子,特徵點使用一個KeyPoint型別的容器來儲存,描述子使用mat型別
std::vector<KeyPoint> keypoint_1,keypoint_2;
Mat description_1,description_2;
//宣告Ptr模版類的FeatureDetector型別 
//這裡的creat()說白就是給他賦了很多初始值
Ptr<FeatureDetector> detector=ORB::create();
Ptr<DescriptiorExtractor> matcher=ORB::create();
//使用這個detect去檢測兩個影象,然後返回值是一個keyPoint型別的陣列 提取角點的位置
detector->detect(img_1,keypoints_1);
detector->detect(img_2,keypoints_2);
//根據角點的位置,計算BRIEF描述子
descripiton->compute(img_1,keypoint_1,description_1);
description->compute(img_2,keypoint_2,description_2);
//然後使用huigui模組,將特徵點畫出來
Mat outimg1;
drawKeypoints(img_1,keypoints_1,outimg1,Scalar::all(-1),DrawMatchesFlags::DEFAULT);
imshow(outimg1);
//對兩幅圖片的BRIEF描述子進行匹配,使用hamming距離,返回的matches.
vector<DMatch> matches;
BFMatcher matcher(NORM_HAMMING);
matcher->(descriptors_1,descriptors_2,matches);
//對匹配點進行篩選,找出匹配點之間最小距離和最行名大距離,也即是說,最相似和最不相似的兩組點之間的距離
double min_dist=10000,max_dist=0;
//接下來是一個迭代的過程,找出匹配點之間最大的漢明距離和最小的漢明距離,
for(int i=0;i<descriptors_1.rows;i  )
{
double dist=matches[i].distance;//獲取當前點的距離,注意這裡min的初始10000,max的初始值是0
if(dist<min_dist) min_dist=dist;
if(dist>max_dist) max_dist=dist;
}
//高博的輸出結果是:最近的兩個量的匹配的距離是4,最遠的匹配距離是95
//當描述子距離大於兩倍的最小距離時,即認為有誤匹配。這是工程經驗
//但是最小距離會非常小,設定一個經驗值30作為下限
//這裡初始化漢明距離
Ptr<DescriptiorMatcher> matcher=DescriptiorMatcher::create("BruteForce-Hamming");
std::vector<DMatch> good_matches;
for(int i=0,i<descriptors_1.rows;i  )
{
if(matches[i].distance<=max(2*min_dist,30))
{
good_matches.push_back(matches[i]);
}
}
//最後將這些點都畫出來
Mat img_goodmatch;
drawMatches(img_1,keypoint_1,img_2,keypoint_2,good_matches,img_goodmatch);
imshow("good_matches".img_goodmatch);
waitKey(0);
return 0;
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60

我想想要檢視在描述子不同額清下,經過某種手段對描述子進行篩選, 
想要知道描述字的大小:

std::cout<<vector.size()<<std::endl;
  • 1

想要輸出圖片的資訊

cout<<img.size()<<endl;
  • 1

附錄: 
這裡寫圖片描述 
ORB::creat() 資料參考連結: 
http://blog.csdn.net/xizero00/article/details/43216727 
智慧指標只有三個: 
auto_ptr unique_ptr shared_ptr 
從上面的結果當中,我們可以發現,如果沒有篩選匹配結果,那麼提取的特徵有大量的無匹配。因此,我們篩選的依據:漢明距離小於最小距離的兩倍,並且大於30.(這是工程經驗)

對描述子的構造傳入的string型別 
這裡寫圖片描述 
這裡寫圖片描述 
L1是1範數,就是絕對值的差 
L2是2範數,也就是歐式距離 
可以在這裡修改成200個特徵點 ORB預設的500個特徵點 
這裡寫圖片描述 
這裡寫圖片描述

如果得到的是兩個單目影象,得到就是2D-2D之間的對應關係 –使用對極幾何 
如果得到的是幀和地圖,得到就是3D-2D之間的對應關係 –使用對極幾何 
如果得到的是RGBD影象,得到就是3D-3D之間的對應關係 –使用對極幾何 
總結: 
這裡寫圖片描述 
注意:這裡的s1和s2分別表示特徵點1和特徵點2的深度。

這裡寫圖片描述

這裡寫圖片描述

這裡寫圖片描述

這裡寫圖片描述

這裡寫圖片描述

#include <opencv2/calib3d/cali3d.hpp>
//計算本質矩陣,基礎矩陣
void find_feature_matches(
const Mat& img_1, const Mat& img_2,
std::vector<KeyPoint>& keypoints_1,
std::vector<KeyPoint>& keypoints_2,
std::vector<DMatch>& matches);
//呼叫
find_feature_matches(img_1,img_2,keypoints_1,keypoints_2,matches);
void find_feature_matches(const Mat& img_1,const Mat& img_2,
std::vector<KeyPoint>& keypoints_1;
std::vector<KeyPoint>& keypoints_2;
std::vector<DMatch>& matches)
{
Mat descriptors_1,descriptors_2;
Ptr<FeatureDetector>detector=ORB::create();
Ptr<DescriptorExtractor>descriptor=ORB::create();
Ptr<DescriptotMatcher> matcher=Descriptor::create("BruteForce-Hamming");
detector->detect(img_1,keypoints_1);
detector->detect(img_2,keypoints_2);
descriptor->compute(img_1,keypoints_1,descriptors_1);
descriptor->compute(img_2,keypoints_2,descriptors_2);
vector<DMatch> match;
matcher->match(descriptors_1,descriptors_2,match);
for(int i=0;i<descriptors_1.rows;i  )
{
if(match[i].distance<max(2*min_dist,30))    
{
matches.push_back(match[i]);
} 
}
}
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
//使用歸一化座標系
Point2d pixel2cam(const Point2d& p,const Mat& K);
{
return Point2d
(
(p.x-K.at<double>(0,2))/k.at<double>(0,0),
(p.y-K.at<double>(1,2))/k.at<double>(1,1))
)
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
void pose_estimation_2d2d(
std::vector<KeyPoint>keypoints_1,
std::vector<KeyPoint>keypoints_2,
std::vector<DMatch> matches,
Mat& R,Mat& t)
{
//相機內參
Mat K=(Mat_<double>(3,3)<<520.9,0,325.1,0,521.0,249.7,0,0,1);
//將匹配點轉換成vector<Point2f>的形式
vector<Point2f> points1;
vector<Point2f> points2;
for(int i=0;i<(int)matches.size();i  )
{
points1.push_back(keypoints_1[matches[i].queryIdx].pt);
points2.push_back(keypoints_2[matches[i].trainIdx].pt);
}
//計算基礎矩陣
Mat fundamental_matrix;
fundamental_matrix=findFundamentalMat(points1,points2,CV_FM_8POINT);
cout<<"fundamental_matrix="<<fundamental_matrix<<endl;
//計算本質矩陣需要相機光心和相機的焦距
Point2d principal_point(325.1,249.7);//相機光心
double focal_length=521;//相機焦距
Mat essential_matrix;
essential_matrix=finEssentialMat(points1,points2,focal_length,principal_point);
cout<<"essential_matrix="<<essential_matrix<<endl;
//單應矩陣,單應矩陣針對當是一個平面,需要針對初始化
Mat homography_matrix;
homography_matrix=findHomography(points1,points2,RANSAC,3);
cout<<"homography_matrix="<<homography_matrix<<endl;
//從本質矩陣恢復旋轉和平移
recoverPose(essential_matrix,points1,points2,R,t,focal_length,principal_point);
cout<<R<<endl;
cout<<t<<endl;
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
    points1.push_back(keypoints_1[matches[i].queryIdx].pt);
points2.push_back(keypoints_2[matches[i].trainIdx].pt);
  • 1
  • 2

在影象匹配的時候有兩種影象的集合,查詢集(Query set)和訓練集(Train set) 
這裡寫圖片描述 
總這份圖片當中可以看出,是一個int型別。

    funfamental_matrix=findFundamentalMat(points1,points2,CV_FM_8POINT)
  • 1

查詢了一下原始碼: 
這裡寫圖片描述 
這裡寫圖片描述

int main(int argc,char **argv)
{
Mat img_1=imread(argv[1],CV_LOAD_IMAGE_COLOR);
Mat img_2=imread(argv[2],CV_LOAD_IMAGE_COLOR);
vector<KeyPoint> keypoints_1,keypoints_2;
vector<DMatch> matches;
find_feature_matches(img_1,img_2,keypoints_1,keypoints_2,matches);
cout<<"keypoint number="<<matches.size()<<endl;
//估計相機運動
Mat R,t;
pose_estimation_2d2d(keypoints_1,keypoints_2,matches,R,t);
//驗證:E=t^R*scale
Mat t_x=(Mat_<double>(3,3)<<
0,  -t.at<double>(2,0),t.at<double>(1,0),
t.at<double>(2,0),0,-t.at<double>(0,0),
-t.at<double>(1,0),t.at<double>(0,0),0);
cout<<"t^R"<<t_x*R<<endl;
//驗證對極約束
Mat K=(Mat_<double>(3,3)<<520.9,0,325.1,0,521.0,249.7,0,0,1);
for(DMatch m:matches)
{
Point2d pt1=pixel2cam(keypoints_1[m.queryIdx].pt,K);
Mat y1=(Mat<double>(3,1)<<pt1.x,pt1.y,1);
Point2d pt2=pixel2cam(keypoints_2[m.trainIdx].pt,K);
Mat y2=(Mat<double>(3,1)<<pt2.x,pt2.y,1);
Mat d=y2.t()*t_x*R*y1;
cout<<"epipolar constraint ="<<d<<endl; 
}
return 0;
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33

本質矩陣和基礎矩陣之間的差別,基礎矩陣多了一個相機的內參K。 
由於尺度的不確定性。在程式中輸出的t的第一維是是0.822,但是這個0.822的單位到底是0.822米,還是0.822釐米。我們是沒有辦法確定的。因為t乘以任意比例常數之後,對極約束仍然成立。 
單目相機的初始化,不能只有純旋轉,必須要有一定程度的平移 
多於8對點的情況下當給定的點數8對(例如上面這個例子當中找到了79對),我們可以計算一個最小二乘解。在存在誤匹配的情況下,我們會更傾向於使用隨機取樣一致性(Random Sample Concensus,RANSAC)來求,而不是用最小二乘。RANSAC是一種通用的做法,適用於有很多誤匹配的情況下。


三角測量(Triangulation)或者三角化 
我們上一步當中,使用對極幾何約束估計了相機的運動。下一步我們需要用相機的運動估計特徵點的空間位置。在單目SLAM當中,由於我們無法通過單張影象獲得相機的深度,因此我們通過三角測量的方法來估計地圖點的深度。 
三角測量是指:通過兩處觀察同一個點的夾角。來確定該點的距離。我們使用三角化來估計畫素點距離。 
這裡寫圖片描述 
因此我們可以通過最小二乘法來求解。

void triangulation(
std::vector<keyPoint>& keypoints_1,
std::vector<keyPoint>& keypoints_2,
const std::vector<DMatch>& matches,
const Mat& R,const Mat& t,
vector<point3d> points)
{
Mat T1=(Mat_<float>(3,4)<<
1,0,0,0,
0,1,0,0,
0,0,1,0);
Mat T2=(Mat_<float>(3,4)<<
R.at<double>(0,0),R.at<double>(0,1),R.at<double>(0,2),t.at<double>(0,0)
R.at<double>(1,0),R.at<double>(1,1),R.at<double>(1,2).t.at<double>(1,0)
R.at<double>(2,0),R.at<double>(2,1),R.at<double>(2,2),t.at<double>(2,0)
);
//輸入相機的內參
Mat K=(Mat <double>(3,3)<<520.9,0,325.1,0,521.0,249.7,0,0,1);
vector<Point2f>pts_1,pts_2;
for(DMatch m:matches)
{
pts_1.push_back(pixel2cam(keypoint_1[m.queryIdx].pt,K));
pts_2.push_back(pixel2cam(keypoint_2[m.trainIdx].pt,K));
}   
Mat pts_4d;
cv::triangulatePoints(T1,T2,pts_1,pts_2,pts_4d);
//轉換成非齊次座標
for(int i=0;i<pts_4d.cols;i  )
{
Mat x=pts_4d.col(i);
x/=x.at<float>(3,0);
Point3d P(
x.at<float>(0,0),
x.at<float>(1,0),
x.at<float>(2,0)
);
points.push_back(p);
}
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
//三角化
vector<Point3d> points;
trangulation(keypoints_1,keypoints_2,matches,R,t);
//驗證三角於特徵點的重投影的關係
Mat K=(Mat_<double>(3,3)<<520.9,0,325.1,0,521.0,249.7,0,0,1);
for(int i=0;i<matches.size();i  )
{
//第一張圖片
Point2d pt1_cam=pixel2cam(keypoints_1[matches[i].queryIdx].pt,K);
Point2d pt1_cam_3d(
points[i].x/points[i].z,
points[i].y/points[i].z 
);
cout<<"point in the first camera"<<pt1_cam<<endl;
cout<<"point project from 3D"<<pt1_cam_3d<<endl;
//第二張圖片
Point2f pt2_cam=pixel2cam(keypoints_2[mathces[i].trainIdx].pt,K);
Mat pt2_trans=R*(Mat_<double>(3,1)<<points[i].x<<points[i].y,<<points[i].z) t;
ptr2_trans/=pts_trans.at<double>(2,0);
cout<<"point in second camera"<<pt2_cam<<endl;
cout<<"point reprojected from second frame"<<pt2_trans.t()<<endl;
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24

三角測量是由平移得到的,有平移才會有對極幾何中的三角形。 
在純旋轉的情況下無法使用三角測量。因為在純旋轉的情況下,對極約束永遠滿足。 
當平移很小的時候,畫素上的不確定性將導致較大的深度的不確定性。想要增加三角化的精度,其一就是提高特徵點的提取精度,也就是提高影象的解析度,另一種方法,是的平移量增大。總而言之,增大平移會使得匹配失效,而平移太小,則三角化的精度不夠。

PnP(Perspective-n-Point)是求解3D到2D點對運動的方法。這種方法,描述了當我知道n個3D空間的點,以及它的投影位置的時候,如何來估計相機所在的位姿。 
2D-2D的對極極化方法需要8個或者8個以上的點,且存在初始化,純旋轉和尺度的問題 
在雙目或者RGBD的視覺里程計中,特徵點的位置,可以由三角化(雙目),或者RGBD的深度圖確定,那麼我們可以直接進行pnp。而在單目相機中,必須進行初始化,才能進行pnp。 
3D-2D的方法,不需要對極約束,同時可以在很少的匹配點中獲得較好的運動估計,因此是一種十分重要的姿態估計的方法。 
pnp問題又很多求解方法,例如 針對三對點的P3P ; 直接線性變換 DLT ; EPnP(Efficient);UPnP。此外還可以有非線性優化的方式構造最小二乘問題並迭代求解,也就是Bundle Adjustment.

DLT直接線性變換 
這裡寫圖片描述 
p3p 
僅僅使用三對匹配點,對資料要求較少 
在SLAM問題中,通常都是先通過P3P等方法來估計相機的位姿,然後構建最小二乘問題進行Bundle Adjustment. 
SVD奇異值分解 
ICP求解。在唯一解的情況下,只要能找到極小值解,那麼這個極小值就是全域性最優值,這也就意味著ICP可以任意選定初始值 在RGBDslam當中,由於一個畫素的深度資料可能是測不到。所有我們可以混合使用pnp和ICP。對於深度已知的特徵點,對3D-3D建模誤差。對於深度未知的特徵點,建模3D-2D的重投影誤差。 
在實際方面,可以用eigen庫直接求出。

擴充套件:參考馮兵的部落格 實現一個簡單的視覺里程計

視覺里程計的就是通過分析處理影象來確定機器人當前的位姿 
輸入:視訊流+相機的內參 
輸出:每一幀相機的位姿 
基本過程: 
1、獲取影象 
2、對影象進行處理 
3、通過FAST演算法對影象進行特徵檢測,通過KLT跟蹤影象的特徵,如果跟蹤的特徵有所丟失,特徵數小於某個閾值,那麼重新特徵檢測。 
4、通過RANSAC的5點演算法來估計出兩幅影象的本質矩陣。 
5、通過本質矩陣進行估計R和t 
6、對尺度資訊進行估計,最終確定旋轉矩陣和平移向量

實現視覺里程計 
這裡寫圖片描述 
這裡寫圖片描述 
實現的過程 
這裡寫圖片描述 
這裡 定義相機圖片寬度和高,焦距和相機餓中心點。 
這裡寫圖片描述 
這裡寫圖片描述 
列舉名:FrameStage 
如果列舉沒有初始化, 即省掉”=整型常數”時, 則從第一個識別符號開始,依次賦給識別符號0, 1, 2, …。但當列舉中的某個成員賦值後, 其後的成員按依次 加1的規則確定其值。

這裡寫圖片描述

向這個檔案中輸出資料

std::ofstream out("position.txt");
  • 1
int font_face=cv::FONT_HERSHEY_PLAIN;
  • 1

FONT_HERSHEY_PLAIN的值就是1

cv::namedWindow("abc",cv::WINDOW_AUTOSIZE);
  • 1

自動調節視窗的大小。這裡只是定義了視窗的名字,但是並沒有把他顯示出來。

cv::Mat::zeros(600,600,CV_8UC3)
  • 1

CV_8UC3使用的Unsigned 8bits RGB3 
Mat::zero返回指定大小的零陣列 
這裡就是返回600*600的零陣列,陣列的型別是無符號型別的8bit RGB3型別

std::setw(6)填充6個空格 
std::setfill(‘0’)填充0字元

// 匯入影象
std::stringstream ss;
ss <<  "C:/dataset/00/image_1/"
<< std::setw(6) << std::setfill('0') << img_id << ".png";
  • 1
  • 2
  • 3
  • 4
cv::Mat img(cv::imread(ss.str().c_str(), 0));
  • 1

將匯入的圖片轉換成mat型別 
這個ss.str().c_str(),

assert(!img.empty());
  • 1

如果沒有影象的話,就跳出迴圈

我第一次有這種感觸,有的CPP檔案,就像是一個大的儲存容器,用來儲存各種各樣,在標頭檔案當中沒有辦法實現的東西。但是你在看原始碼的過程當中,還是要從mian函式,從執行緒 的角度去看這些檔案。

void addImage(const cv::Mat & img, int frame_id); 
指的記住的東西就是 const cv::Mat &img.

/// 提供一個影象
void VisualOdometry::addImage(const cv::Mat& img, int frame_id)
{
//對新增的影象進行判斷
if (img.empty() || img.type() != CV_8UC1 || img.cols != cam_->width() || img.rows != cam_->height())
throw std::runtime_error("Frame: provided image has not the same size as the camera model or image is not grayscale");
// 新增新幀
new_frame_ = img;
bool res = true;//結果狀態
if (frame_stage_ == STAGE_DEFAULT_FRAME)
res = processFrame(frame_id);
else if (frame_stage_ == STAGE_SECOND_FRAME)
res = processSecondFrame();
else if (frame_stage_ == STAGE_FIRST_FRAME)
res = processFirstFrame();
// 處理結束之後將當前幀設為上一處理幀
last_frame_ = new_frame_;
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18

這裡要丟擲一個異常

throw std::runtime_error("sdfdff")
  • 1

然後判斷一下

if (frame_stage_ == STAGE_DEFAULT_FRAME)
res = processFrame(frame_id);
else if (frame_stage_ == STAGE_SECOND_FRAME)
res = processSecondFrame();
else if (frame_stage_ == STAGE_FIRST_FRAME)
res = processFirstFrame();
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
bool VisualOdometry::processFrame(int frame_id)
{
double scale = 1.00;//初始尺度為1
featureTracking(last_frame_, new_frame_, px_ref_, px_cur_, disparities_); //通過光流跟蹤確定第二幀中的相關特徵
cv::Mat E, R, t, mask;
E = cv::findEssentialMat(px_cur_, px_ref_, focal_, pp_, cv::RANSAC, 0.999, 1.0, mask);
cv::recoverPose(E, px_cur_, px_ref_, R, t, focal_, pp_, mask);
scale = getAbsoluteScale(frame_id);//得到當前幀的實際尺度
if (scale > 0.1) //如果尺度小於0.1可能計算出的Rt存在一定的問題,則不做處理,保留上一幀的值
{
cur_t_ = cur_t_   scale*(cur_R_*t);
cur_R_ = R*cur_R_;
}
// 如果跟蹤特徵點數小於給定閾值,進行重新特徵檢測
if (px_ref_.size() < kMinNumFeature)
{
featureDetection(new_frame_, px_ref_);
featureTracking(last_frame_, new_frame_, px_ref_, px_cur_, disparities_);
}
px_ref_ = px_cur_;
retur
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
void VisualOdometry::featureTracking(cv::Mat image_ref, cv::Mat image_cur,
std::vector<cv::Point2f>& px_ref, std::vector<cv::Point2f>& px_cur, std::vector<double>& disparities)
{
const double klt_win_size = 21.0;
const int klt_max_iter = 30;
const double klt_eps = 0.001;
std::vector<uchar> status;
std::vector<float> error;
std::vector<float> min_eig_vec;
cv::TermCriteria termcrit(cv::TermCriteria::COUNT   cv::TermCriteria::EPS, klt_max_iter, klt_eps);
cv::calcOpticalFlowPyrLK(image_ref, image_cur,
px_ref, px_cur,
status, error,
cv::Size2i(klt_win_size, klt_win_size),
4, termcrit, 0);
std::vector<cv::Point2f>::iterator px_ref_it = px_ref.begin();
std::vector<cv::Point2f>::iterator px_cur_it = px_cur.begin();
disparities.clear(); disparities.reserve(px_cur.size());
for (size_t i = 0; px_ref_it != px_ref.end();   i)
{
if (!status[i])
{
px_ref_it = px_ref.erase(px_ref_it);
px_cur_it = px_cur.erase(px_cur_it);
continue;
}
disparities.push_back(norm(cv::Point2d(px_ref_it->x - px_cur_it->x, px_ref_it->y - px_cur_it->y)));
px_ref_it;
px_cur_it;
}
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32

KLT屬入光流法。就是將上一個幀的影象和這一幀的影象對比,發現影象序列在時間域上的變換。 
這裡寫圖片描述

這裡寫圖片描述 
從這裡可以看出,最大的迭代次數 和 結果的精準度,其實就是誤差容限。這裡其實就是例項化了一個物件,然後 
賦予了初值。 
這部分的例程可以參考這篇部落格: 
http://blog.csdn.net/liyuqian199695/article/details/49121525

cv::TermCriteria termcrit(cv::TermCriteria::COUNT cv::TermCriteria::EPS, klt_max_iter, klt_eps);
  • 1

我們只用關係這個klt_max_iter和klt_eps這兩個引數,其他的引數不需要關注

    cv::calcOpticalFlowPyrLK(image_ref, image_cur,
px_ref, px_cur,
status, error,
cv::Size2i(klt_win_size, klt_win_size),
4, termcrit, 0);
  • 1
  • 2
  • 3
  • 4
  • 5

image_ref表示參考幀的(也就是上一幀的影象) 
image_cur表示表示當前幀的影象 資料型別就是Mat型別 
px_ref表示表示參考幀的特徵點的光流 資料型別就是:std::vector<cv::Point2f>& px_ref 
px_cur表示當真幀的特徵點的光流 
status表示 如果有特徵流被發現,那麼向量的元素都置為1,否則為0,那麼這樣的 他的資料型別是;std::vector<uchar> 
error表示輸出錯誤的向量,資料型別std::vector<float> 
klt_win_size每個金字塔水平搜尋視窗的尺寸 double型別 但是真正傳遞引數的時候,直接把他變成一個int型別,其實就cv::size2i(21.0,21.0),這麼做的話其實就是,是一個矩形 
下面這個4表示的是影象金字塔最大的層數是4層。 
然後termcrit就是剛剛在上面例項化和初始化的引數,他表示的時判定準側。 
最後一個0表示的是標誌位, 
還有一個隱藏的資料,就是話精度的閾值是e^-4 
通過這個函式,返回的話,可以直接找打KLT光流法的特徵點

接下來,準備設定開始迭代

std::vector<cv::Point2f>::iteration px_ref_it=px_ref.begin();
std::vector<cv::Point2f>::iteration px_ref_it=px_ref.begin();
  • 1
  • 2

將一個向量 
disparities就是將所有的原來容器當中東西進行清除

disparities.clear();
  • 1

下一條就是強制改變這個容器的大小

disparities.reserve(px_cur.size);
  • 1

我覺他是要通過比較參考幀和當前幀的差異,然後通過這樣的差異,來判斷畫素的位移。

然後然後現在開始迴圈

for (size_t i = 0; px_ref_it != px_ref.end();   i)
{
if (!status[i])
{
px_ref_it = px_ref.erase(px_ref_it);
px_cur_it = px_cur.erase(px_cur_it);
continue;
}
disparities.push_back(norm(cv::Point2d(px_ref_it->x - px_cur_it->x, px_ref_it->y - px_cur_it->y)));
px_ref_it;
px_cur_it;
}
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13

如果當前的狀態不是1的話,,那麼就從這兩個容器中刪除這些不相互匹配的元素 
然後continue,繼續執行 
然後對每個畫素都這麼執行, 
然後就是對每個disparities進行 
然後就是這個裡面的函式,進行平方,舉個例子norm(3,4)=25,也就是說3的平方+4的平方=25 
這樣就實現了一個完整的跟隨,從當真幀和參考幀當中

順便看一個特徵點的提取

void VisualOdometry::featureDetection(cv::Mat image, std::vector<cv::Point2f>& px_vec)  
  • 1

可以看出輸入的影象,和特徵點 
, 
可以看出,這個視覺里程計利用使用的FAST進行提取的焦點

    std::vector<cv::KeyPoint> keypoints;
int fast_threshold = 20;
bool non_max_suppression = true;
cv::FAST(image, keypoints, fast_threshold, non_max_suppression);
  • 1
  • 2
  • 3
  • 4

然後將找到的特徵點,給了這個時候輸入的影象,也就是px_vec

cv::KeyPoint::convert(keypoints, px_vec);
  • 1

然後我

bool VisualOdometry::processFrame(int frame_id)
{
double scale = 1.00;//初始尺度為1
featureTracking(last_frame_, new_frame_, px_ref_, px_cur_, disparities_); //通過光流跟蹤確定第二幀中的相關特徵
cv::Mat E, R, t, mask;
E = cv::findEssentialMat(px_cur_, px_ref_, focal_, pp_, cv::RANSAC, 0.999, 1.0, mask);
cv::recoverPose(E, px_cur_, px_ref_, R, t, focal_, pp_, mask);
scale = getAbsoluteScale(frame_id);//得到當前幀的實際尺度
if (scale > 0.1) //如果尺度小於0.1可能計算出的Rt存在一定的問題,則不做處理,保留上一幀的值
{
cur_t_ = cur_t_   scale*(cur_R_*t);
cur_R_ = R*cur_R_;
}
// 如果跟蹤特徵點數小於給定閾值,進行重新特徵檢測
if (px_ref_.size() < kMinNumFeature)
{
featureDetection(new_frame_, px_ref_);
featureTracking(last_frame_, new_frame_, px_ref_, px_cur_, disparities_);
}
px_ref_ = px_cur_;
return true;
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
featureTracking(last_frame_, new_frame_, px_ref_, px_cur_, disparities_); //通過光流跟蹤確定第二幀中的相關特徵
  • 1

找本質矩陣,以及從本質矩陣中,恢復出R和t。

E = cv::findEssentialMat(px_cur_, px_ref_, focal_, pp_, cv::RANSAC, 0.999, 1.0, mask);
cv::recoverPose(E, px_cur_, px_ref_, R, t, focal_, pp_, mask);
  • 1
  • 2

輸入當前幀的Id

double VisualOdometry::getAbsoluteScale(int frame_id)
  • 1
double VisualOdometry::getAbsoluteScale(int frame_id)
{
std::string line;
int i = 0;
std::ifstream ground_truth("C:/dataset/00/00.txt");
double x = 0, y = 0, z = 0;
double x_prev, y_prev, z_prev;
// 獲取當前幀真實位置與前一幀的真實位置的距離作為尺度值
if (ground_truth.is_open())
{
while ((std::getline(ground_truth, line)) && (i <= frame_id))
{
z_prev = z;
x_prev = x;
y_prev = y;
std::istringstream in(line);
for (int j = 0; j < 12; j  )  {
in >> z;
if (j == 7) y = z;
if (j == 3)  x = z;
}
i  ;
}
ground_truth.close();
}
else {
std::cerr<< "Unable to open file";
return 0;
}
return sqrt((x - x_prev)*(x - x_prev)   (y - y_prev)*(y - y_prev)   (z - z_prev)*(z - z_prev));
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33

這個函式的目的獲取當前幀實際的尺度,我覺得是為了消除測量誤差

輸入這個檔案

std::ifstream ground_truth("C:/dataset/00/00.txt");
  • 1

將當前幀幀和前一幀實際位置的距離的尺度值

std::getline(ground_truth, line)) && (i <= frame_id)
  • 1

用於讀入字元 將你讀入的字元儲存到line當中, 
如果geitline沒有讀入字元,將返回false,可用於判斷檔案結束 
這裡的i++肯定是對下一幀的影象進行迴圈 
這裡我也不清楚為什麼要如果是是j=7,我看出來了,這是有兩個迴圈。 
返回的數值是一個誤差尺度,就是就是一個的模值。 
然後我們接著主線路走,在處理當前幀的時候,那麼我已通過getsolutescale得到真實的尺度,如果這個尺度比0.1小的話,呢麼可能這個數值是有問題的。那麼我們就不要這個數值。然後這個值比0.1大的話,我們就更新一下現在的平移向量和旋轉矩陣。 
這裡寫圖片描述 
這裡寫圖片描述 
然後我們將當前幀賦值給參考幀,完成一次迭代更新。

最後我們回到主執行緒當中。也就是addImage當中,會發現,如果當前幀的狀態等對 
STAGE_DEFAULT_FRAME,那麼我們就將這個處理的過程賦值給res。否則的話,我們就認為是其他幀。其實總共這裡裡面有3種幀。第一幀,第二幀,第一幀到第二幀都過渡幀,我們通過這個過渡幀,將第一幀和第二幀聯絡起來。

bool VisualOdometry::processSecondFrame()
{
featureTracking(last_frame_, new_frame_, px_ref_, px_cur_, disparities_); //通過光流跟蹤確定第二幀中的相關特徵
// 計算初始位置
cv::Mat E, R, t, mask;
E = cv::findEssentialMat(px_cur_, px_ref_, focal_, pp_, cv::RANSAC, 0.999, 1.0, mask);
cv::recoverPose(E, px_cur_, px_ref_, R, t, focal_, pp_, mask);
cur_R_ = R.clone();
cur_t_ = t.clone();
frame_stage_ = STAGE_DEFAULT_FRAME;// 設定狀態處理預設幀
px_ref_ = px_cur_;
return true;
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13

然後我們分析一下第二幀。第二針當中。我們就是對frame_stage進行一種狀態的賦值。他的資料型別是:FrameStage frame_stage_; //!< 當前為第幾幀

cur_R_ = R.clone();
cur_t_ = t.clone();
frame_stage_ = STAGE_DEFAULT_FRAME;// 設定狀態處理預設幀
px_ref_ = px_cur_;
  • 1
  • 2
  • 3
  • 4

將旋轉矩陣做了替換

同時,我們看一下是如何處理第一幀的

bool VisualOdometry::processFirstFrame()
{
// 對當前幀進行特徵檢測
featureDetection(new_frame_, px_ref_);//第一幀計算出來的特徵,為參考特徵點
// 修改狀態,表明第一幀已經處理完成
frame_stage_ = STAGE_SECOND_FRAME;
return true;
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8

在這裡我們給影象的ID的最大值只是2000.現在我最好奇就是這裡的檔案,匯入的路徑。 
我們通過addimge這個函式,將每一幀的影象和他對應ID匹配起來。 
然後,通過設定不同的標誌位,從而進行區分。 
我們通過getCurrent來返回當真當前幀的平移量。 
我們獲得餓平移量。然後分別賦值給x,y,z 
這裡寫圖片描述 
同時我們進行的輸出,這樣就知道平移量是怎麼情況。

隨後我們想要著這條路畫出來。那麼首先將這個x轉化成int型別,然後這裡的300和100表示的在畫素中起始點的座標,我們不希望將從一個平面當中的3來畫出來。所有我們就移動了座標軸。

circle這個,那麼他的原理就是,第一個引數,表示要話的影象,第二個即使中心點,這個地方就是點餓座標。然後就是點的半徑,然後其實你可以選擇用實心來畫這個圓。然後選擇線的顏色,選的線寬。然後就是一些預設餓引數,就可以了。 
cv::Mat traj = cv::Mat::zeros(600, 600, CV_8UC3);// 用於繪製軌跡 
從這個地方來看的,這個是一個空白的畫布。就像是我們在視訊當中看到黑色的畫布。 
我們在這裡設定線的半徑是1,線寬是2,顏色是紅色。 
然後我們接著畫出一個矩形。準確的的說,就是一個實心的矩形,不就是一條直線麼。然後我們輸出一些文字,然後們就是通過sprintf輸出一些東西。然後就是putText將一些文字弄出來,同時就是設定一些線寬什麼亂七八糟的。哈哈。我們在將兩個前面我們定義的namedwindow的東西輸出出來。 
然後我們將當時例項化的東西,釋放到記憶體,同時關閉噹噹時開啟的一些東西。並且將終端視窗用getchar()進行暫停。基本上這個這個視覺里程計的程式碼我就分析完畢了。

總體來看,就是的通過FAST進行焦點檢測。然後通過光流法進行對特徵點的提取和跟蹤,然後通過處理幀,當前幀的參考幀,這三幀的相互配合,處理,在這個過程中我自己的收穫,就是發現,原來看原始碼不是一個Cpp,一個cpp的看,而是通過一條清晰的主線,一條主線 ,一條主線的進行分析,然後當到程序和執行緒的一些東西的時候,順著執行緒和程序的一條一條的走下去。然後很多人都是在標頭檔案中宣告很多東西,然後在cpp檔案當中實現很多東西。


視覺slam第8講 視覺里程計

特徵點法的缺陷: 
1、關鍵點的提取和描述子的計算都很耗時間 
2、我們總是想用特徵點來代替整個影象,導致很多影象的資訊沒有被使用。 
3、相機運動的那麼沒有很多特徵點的地方,例如白牆。 
直接法 從大的方面 根據影象的灰度資訊來計算相機的運動。 
使用特徵法估計相機運動的,我們是吧特徵點看做固定在三維空間的不動點。根據他們在相機中的投影位置,通過最小化重投影誤差(Reprojection error)來優化相機的運動。在直接法當中,我們是通過最小化光度誤差(Photometric error) 
光流(optical flow) 
光流描述了畫素在影象中的運動。計算部分畫素稱為稀疏光流,計算所有畫素稱為稠密光流。稀疏光流是以Lucas-Kanada光流為代表,並在SLAM當中用於跟蹤特徵點的位置。 
灰度不變假設:同一個空間點的畫素的灰度值,在各個影象中是固定不變的。在LK光流中,我們假設某一個視窗內的畫素具有相同的運動。我感覺看下來,光流法還是提取特徵。 
直接法 完全依靠優化來求解相機的位姿。畫素梯度引導著優化的方向。如果想要得到正確的優化結果,就必須保證大部分畫素梯度能夠把優化引導到正確的方向。 
直接法的優點: 
1、可以省去計算特徵點和描述子的時間 
2、只要求有畫素的梯度就可以。 
3、可以構建半稠密乃至稠密的地圖 
缺點: 
1、非凸性,直接法完全依靠梯度搜尋,降低目標函式來計算相機的位姿。 
2、單個畫素沒有區分度

轉載 http://blog.csdn.net/david_han008/article/details/53560736

相關文章

程式語言 最新文章