寫在最前面：

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

• 1
• 2
• 3
• 4
• 5
• 6
• 7

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

什麼叫旋轉向量：使用一個向量，他的方向和旋轉軸一致，長度等於旋轉角。（任意的一個旋轉，都可以用旋轉軸和旋轉角來表示）這就是軸角Axis-Angle

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

• 1

• 1

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

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

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

• 1

• 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

• 1
• 2
• 3
• 4

如何輸出一個四元數

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

• 1

• 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

• 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

• 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

• 1
• 2
• 3
• 4
• 5
• 6
• 7
• 8
• 9
• 10
• 11

賦值部分

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

• 1
• 2
• 3

• 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

• 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

• 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

• 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

• 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

• 1

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

#include <pangolin/pangolin.h>

• 1

• 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講

// 關於 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

• 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

• 1

並且在使用的時候，cmakelist需要這麼寫：

joinMap.cpp
用來讀寫檔案的

    #include<fstream>

• 1

• 1

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

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

• 1
• 2

• 1
• 2

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

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

• 1

• 1

讀取資料流

為了使用boost::format

#include <boost/format.hpp>  

• 1

• 1

boost::format格式化字串

這個兩個的效果是一模一樣的。

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

• 1

• 1

 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

• 1
• 2

這裡用來遍歷data的

 double data[7] = {0};//初始化
 for ( auto&am