[SLAM]（番外篇）：一起做RGB-D SLAM(6)
本文轉自高翔老師的部落格，建議在學完教程的第二講後，插入學習，做到工程快速入門。
原文連結：https://www.cnblogs.com/gaoxiang12/p/4739934.html
圖優化工具g2o的入門
在上一講中，我們介紹瞭如何使用兩兩匹配，搭建一個視覺里程計。那麼，這個里程計有什麼不足呢？
- 一旦出現了錯誤匹配，整個程式就會跑飛。
- 誤差會累積。常見的現象是：相機轉過去的過程能夠做對，但轉回來之後則出現明顯的偏差。
- 效率方面不盡如人意。線上的點雲顯示比較費時。
累積誤差是里程計中不可避免的，後續的相機姿態依賴著前面的姿態。想要保證地圖的準確，必須要保證每次匹配都精確無誤，而這是難以實現的。所以，我們希望用更好的方法來做slam。不僅僅考慮兩幀的資訊，而要把所有整的資訊都考慮進來，成為一個全slam問題（full slam）。下圖為累積誤差的一個例子。右側是原有掃過的地圖，左側是新掃的，可以看到出現了明顯的不重合。
所以，我們這一講要介紹姿態圖（pose graph），它是目前視覺slam裡最常用的方法之一。
姿態圖（原理部分）
姿態圖，顧名思義，就是由相機姿態構成的一個圖（graph）。這裡的圖，是從圖論的意義上來說的。一個圖由節點與邊構成：
G={V,E}.G={V,E}.
在最簡單的情況下，節點代表相機的各個姿態(四元數形式或矩陣形式）：
vi=[x,y,z,qx,qy,qz,qw]=Ti=[R3×3O1×3t3×11]ivi=[x,y,z,qx,qy,qz,qw]=Ti=[R3×3t3×1O1×31]i
而邊指的是兩個節點間的變換：
Ei,j=Ti,j=[R3×3O1×3t3×11]i,j.Ei,j=Ti,j=[R3×3t3×1O1×31]i,j.
於是乎，我們可以把前面計算的東西都放到了一個圖裡（請勿吐槽畫風）。
對於vo，這個圖應該像這樣（同樣請勿吐槽畫風）：
像vo這樣的圖呢，我們並沒有什麼可以做的。然而，當這個圖不是vo那樣的鏈狀結構時，由於邊Ti,jTi,j中存在誤差，使得所有的邊給出的資料並不一致。這時節，我們就可以優化一個不一致性誤差：
minE=∑i,j∥x∗i−Ti,jx∗j∥22.minE=∑i,j‖xi∗−Ti,jxj∗‖22.
這裡x∗ixi∗表示xixi的估計值。
小蘿蔔：師兄，什麼叫估計值啊？
師兄：嗯，每個xixi實質上都是優化變數啦。在優化過程中，它們有一個初始值。然後呢，根據目標函式對xx的梯度:
x∗(t+1)=x∗(t)−η∗∇xEx(t+1)∗=x(t)∗−η∗∇xE
調整xx的值使得EE縮小。最後，如果這個問題收斂的話，xx的變化就會越來越小，EE也收斂到一個極小值。在這個迭代的過程中，xx那不斷變化的值就是x∗x∗啦。
小蘿蔔：哦我明白了！是不是運籌學書裡講的非線性優化就是這個啊？
師兄：對！根據迭代策略的不同，又可分為Gauss-Netwon(GN)下山法，Levenberg-Marquardt(LM)方法等等。這個問題也稱為Bundle Adjustment(BA)，我們通常使用LM方法優化這個非線性平方誤差函式。
BA方法是近年來視覺slam裡用的很多的方法（所以很多研究者吐槽slam和sfm(structure from motion)越來越像了）。早些年間（2005以前），人們還認為用BA求解slam非常困難，因為計算量太大。不過06年之後，人們注意到slam構建的ba問題的稀疏性質，所以用稀疏的BA演算法（sparse BA）求解這個圖，才使BA在slam裡廣泛地應用起來。
為什麼說slam裡的BA問題稀疏呢？因為同樣的場景很少出現在許多位置中。這導致上面的pose graph中，圖GG離全圖很遠，只有少部分的節點存在直接邊的聯絡。這就是姿態圖的稀疏性。
求解BA的軟體包有很多，感興趣的讀者可以去看wiki: https://en.wikipedia.org/wiki/Bundle_adjustment。我們這裡介紹的g2o（Generalized Graph Optimizer），就是近年很流行的一個圖優化求解軟體包。下面我們通過例項程式碼，幫助大家入門g2o。
姿態圖（實現部分）
- 安裝g2o：
要使用g2o，首先你需要下載並安裝它：https://github.com/RainerKuemmerle/g2o。 在ubuntu 12.04下，安裝g2o步驟如下：
- 安裝依賴項：
1 sudo apt-get install libeigen3-dev libsuitesparse-dev libqt4-dev qt4-qmake libqglviewer-qt4-dev1404或1604的最後一項改為 libqglviewer-dev 即可。
- 解壓g2o並編譯安裝：
進入g2o的程式碼目錄，並：mkdir build cd build cmake .. make sudo make install多說兩句，你可以安裝cmake-curses-gui這個包，通過gui來選擇你想編譯的g2o模組並設定cmake編譯過程中的flags。例如，當你實在裝不好上面的libqglviewer時，你可以選擇不編譯g2o視覺化模組（把G2O_BUILD_APPS關掉)，這樣即使沒有libqglviewer，你也能編譯過g2o。
1 cd build 2 ccmake .. 3 make 4 sudo make install
安裝成功後，你可以在/usr/local/include/g2o中找到它的標頭檔案，而在/usr/local/lib中找到它的庫檔案。
- 使用g2o
安裝完成後，我們把g2o引入自己的cmake工程：
# 新增g2o的依賴
# 因為g2o不是常用庫，要新增它的findg2o.cmake檔案
LIST( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules )
SET( G2O_ROOT /usr/local/include/g2o )
FIND_PACKAGE( G2O )
# CSparse
FIND_PACKAGE( CSparse )
INCLUDE_DIRECTORIES( ${G2O_INCLUDE_DIR} ${CSPARSE_INCLUDE_DIR} )
同時，在程式碼根目錄下新建cmake_modules資料夾，把g2o程式碼目錄下的cmake_modules裡的東西都拷進來，保證cmake能夠順利找到g2o。
現在，複製一個上一講的visualOdometry.cpp，我們把它改成slamEnd.cpp：
src/slamEnd.cpp
1 /*************************************************************************
2 > File Name: rgbd-slam-tutorial-gx/part V/src/visualOdometry.cpp
3 > Author: xiang gao
4 > Mail: [email protected]
5 > Created Time: 2015年08月15日 星期六 15時35分42秒
6 * add g2o slam end to visual odometry
7 ************************************************************************/
8
9 #include <iostream>
10 #include <fstream>
11 #include <sstream>
12 using namespace std;
13
14 #include "slamBase.h"
15
16 //g2o的標頭檔案
17 #include <g2o/types/slam3d/types_slam3d.h> //頂點型別
18 #include <g2o/core/sparse_optimizer.h>
19 #include <g2o/core/block_solver.h>
20 #include <g2o/core/factory.h>
21 #include <g2o/core/optimization_algorithm_factory.h>
22 #include <g2o/core/optimization_algorithm_gauss_newton.h>
23 #include <g2o/solvers/csparse/linear_solver_csparse.h>
24 #include <g2o/core/robust_kernel.h>
25 #include <g2o/core/robust_kernel_factory.h>
26 #include <g2o/core/optimization_algorithm_levenberg.h>
27
28
29 // 給定index，讀取一幀資料
30 FRAME readFrame( int index, ParameterReader& pd );
31 // 估計一個運動的大小
32 double normofTransform( cv::Mat rvec, cv::Mat tvec );
33
34 int main( int argc, char** argv )
35 {
36 // 前面部分和vo是一樣的
37 ParameterReader pd;
38 int startIndex = atoi( pd.getData( "start_index" ).c_str() );
39 int endIndex = atoi( pd.getData( "end_index" ).c_str() );
40
41 // initialize
42 cout<<"Initializing ..."<<endl;
43 int currIndex = startIndex; // 當前索引為currIndex
44 FRAME lastFrame = readFrame( currIndex, pd ); // 上一幀資料
45 // 我們總是在比較currFrame和lastFrame
46 string detector = pd.getData( "detector" );
47 string descriptor = pd.getData( "descriptor" );
48 CAMERA_INTRINSIC_PARAMETERS camera = getDefaultCamera();
49 computeKeyPointsAndDesp( lastFrame, detector, descriptor );
50 PointCloud::Ptr cloud = image2PointCloud( lastFrame.rgb, lastFrame.depth, camera );
51
52 pcl::visualization::CloudViewer viewer("viewer");
53
54 // 是否顯示點雲
55 bool visualize = pd.getData("visualize_pointcloud")==string("yes");
56
57 int min_inliers = atoi( pd.getData("min_inliers").c_str() );
58 double max_norm = atof( pd.getData("max_norm").c_str() );
59
60 /*******************************
61 // 新增:有關g2o的初始化
62 *******************************/
63 // 選擇優化方法
64 typedef g2o::BlockSolver_6_3 SlamBlockSolver;
65 typedef g2o::LinearSolverCSparse< SlamBlockSolver::PoseMatrixType > SlamLinearSolver;
66
67 // 初始化求解器
68 SlamLinearSolver* linearSolver = new SlamLinearSolver();
69 linearSolver->setBlockOrdering( false );
70 SlamBlockSolver* blockSolver = new SlamBlockSolver( linearSolver );
71 g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( blockSolver );
72
73 g2o::SparseOptimizer globalOptimizer; // 最後用的就是這個東東
74 globalOptimizer.setAlgorithm( solver );
75 // 不要輸出除錯資訊
76 globalOptimizer.setVerbose( false );
77
78 // 向globalOptimizer增加第一個頂點
79 g2o::VertexSE3* v = new g2o::VertexSE3();
80 v->setId( currIndex );
81 v->setEstimate( Eigen::Isometry3d::Identity() ); //估計為單位矩陣
82 v->setFixed( true ); //第一個頂點固定，不用優化
83 globalOptimizer.addVertex( v );
84
85 int lastIndex = currIndex; // 上一幀的id
86
87 for ( currIndex=startIndex+1; currIndex<endIndex; currIndex++ )
88 {
89 cout<<"Reading files "<<currIndex<<endl;
90 FRAME currFrame = readFrame( currIndex,pd ); // 讀取currFrame
91 computeKeyPointsAndDesp( currFrame, detector, descriptor );
92 // 比較currFrame 和 lastFrame
93 RESULT_OF_PNP result = estimateMotion( lastFrame, currFrame, camera );
94 if ( result.inliers < min_inliers ) //inliers不夠，放棄該幀
95 continue;
96 // 計算運動範圍是否太大
97 double norm = normofTransform(result.rvec, result.tvec);
98 cout<<"norm = "<<norm<<endl;
99 if ( norm >= max_norm )
100 continue;
101 Eigen::Isometry3d T = cvMat2Eigen( result.rvec, result.tvec );
102 cout<<"T="<<T.matrix()<<endl;
103
104 // cloud = joinPointCloud( cloud, currFrame, T, camera );
105
106 // 向g2o中增加這個頂點與上一幀聯絡的邊
107 // 頂點部分
108 // 頂點只需設定id即可
109 g2o::VertexSE3 *v = new g2o::VertexSE3();
110 v->setId( currIndex );
111 v->setEstimate( Eigen::Isometry3d::Identity() );
112 globalOptimizer.addVertex(v);
113 // 邊部分
114 g2o::EdgeSE3* edge = new g2o::EdgeSE3();
115 // 連線此邊的兩個頂點id
116 edge->vertices() [0] = globalOptimizer.vertex( lastIndex );
117 edge->vertices() [1] = globalOptimizer.vertex( currIndex );
118 // 資訊矩陣
119 Eigen::Matrix<double, 6, 6> information = Eigen::Matrix< double, 6,6 >::Identity();
120 // 資訊矩陣是協方差矩陣的逆，表示我們對邊的精度的預先估計
121 // 因為pose為6D的，資訊矩陣是6*6的陣，假設位置和角度的估計精度均為0.1且互相獨立
122 // 那麼協方差則為對角為0.01的矩陣，資訊陣則為100的矩陣
123 information(0,0) = information(1,1) = information(2,2) = 100;
124 information(3,3) = information(4,4) = information(5,5) = 100;
125 // 也可以將角度設大一些，表示對角度的估計更加準確
126 edge->setInformation( information );
127 // 邊的估計即是pnp求解之結果
128 edge->setMeasurement( T );
129 // 將此邊加入圖中
130 globalOptimizer.addEdge(edge);
131
132 lastFrame = currFrame;
133 lastIndex = currIndex;
134
135 }
136
137 // pcl::io::savePCDFile( "data/result.pcd", *cloud );
138
139 // 優化所有邊
140 cout<<"optimizing pose graph, vertices: "<<globalOptimizer.vertices().size()<<endl;
141 globalOptimizer.save("./data/result_before.g2o");
142 globalOptimizer.initializeOptimization();
143 globalOptimizer.optimize( 100 ); //可以指定優化步數
144 globalOptimizer.save( "./data/result_after.g2o" );
145 cout<<"Optimization done."<<endl;
146
147 globalOptimizer.clear();
148
149 return 0;
150 }
151
152 FRAME readFrame( int index, ParameterReader& pd )
153 {
154 FRAME f;
155 string rgbDir = pd.getData("rgb_dir");
156 string depthDir = pd.getData("depth_dir");
157
158 string rgbExt = pd.getData("rgb_extension");
159 string depthExt = pd.getData("depth_extension");
160
161 stringstream ss;
162 ss<<rgbDir<<index<<rgbExt;
163 string filename;
164 ss>>filename;
165 f.rgb = cv::imread( filename );
166
167 ss.clear();
168 filename.clear();
169 ss<<depthDir<<index<<depthExt;
170 ss>>filename;
171
172 f.depth = cv::imread( filename, -1 );
173 f.frameID = index;
174 return f;
175 }
176
177 double normofTransform( cv::Mat rvec, cv::Mat tvec )
178 {
179 return fabs(min(cv::norm(rvec), 2*M_PI-cv::norm(rvec)))+ fabs(cv::norm(tvec));
180 }
其中，大部分程式碼和上一講是一樣的，此外新增了幾段g2o的初始化與簡單使用。
使用g2o圖優化的簡要步驟：第一步，構建一個求解器：globalOptimizer
1 // 選擇優化方法
2 typedef g2o::BlockSolver_6_3 SlamBlockSolver;
3 typedef g2o::LinearSolverCSparse< SlamBlockSolver::PoseMatrixType > SlamLinearSolver;
4
5 // 初始化求解器
6 SlamLinearSolver* linearSolver = new SlamLinearSolver();
7 linearSolver->setBlockOrdering( false );
8 SlamBlockSolver* blockSolver = new SlamBlockSolver( linearSolver );
9 g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( blockSolver );
10
11 g2o::SparseOptimizer globalOptimizer; // 最後用的就是這個東東
12 globalOptimizer.setAlgorithm( solver );
13 // 不要輸出除錯資訊
14 globalOptimizer.setVerbose( false );
然後，在求解器內新增點和邊：
1 // 新增點
2 g2o::VertexSE3* v = new g2o::VertexSE3();
3 // 設定點v ...
4 globalOptimizer.addVertex( v );
5
6 // 新增邊
7 g2o::EdgeSE3* edge = new g2o::EdgeSE3();
8 // 設定邊 edge ...
9 globalOptimizer.addEdge(edge);
最後，完成優化並存儲優化結果：
1 globalOptimizer.save("./data/result_before.g2o");
2 globalOptimizer.initializeOptimization();
3 globalOptimizer.optimize( 100 ); //可以指定優化步數
4 globalOptimizer.save( "./data/result_after.g2o" );大致就是這樣啦。
關於程式碼的一些解釋：
- 頂點和邊的型別
頂點和邊有不同的型別，這要看我們想求解什麼問題。由於我們是3D的slam，所以頂點取成了相機姿態：g2o::VertexSE3，而邊則是連線兩個VertexSE3的邊：g2o::EdgeSE3。如果你想用別的型別的頂點（如2Dslam，路標點），你可以看看/usr/local/include/g2o/types/下的檔案，基本上涵蓋了各種slam的應用，應該能滿足你的需求。
小蘿蔔：師兄，什麼是SE3呢？
師兄：簡單地說，就是4×44×4的變換矩陣啦，也就是我們這裡用的相機姿態了。更深層的解釋需要李代數裡的知識。相應的，2D slam就要用SE2作為姿態節點了。在我們引用<g2o/types/slam3d/types_slam3d.h>
時，就已經把相關的點和邊都包含進來了哦。 - 優化方法
g2o允許你使用不同的優化求解器（然而實際效果似乎差別不大）。你可以選用csparse, pcg, cholmod等等。我們這裡使用csparse為例啦。 - g2o檔案
g2o的優化結果是儲存在一個.g2o的文字檔案裡的，你可以用gedit等編輯軟體開啟它，結構是這樣的：
嗯，這個檔案前面是頂點的定義，包含 ID, x,y,z,qx,qy,qz,qw。後邊則是邊的定義：ID1, ID2, dx, T 以及資訊陣的上半形。實際上，你也可以自己寫個程式去生成這樣一個檔案，交給g2o去優化，寫文字檔案不會有啥困難的啦。
這個檔案也可以用g2o_viewer開啟，你還能直觀地看到裡面的節點與邊的位置。同時你可以選一個優化方法對該圖進行優化，這樣你可以直觀地看到優化的過程哦。然而對於我們現在的VO例子來說，似乎沒什麼可以優化的……
結束語
好了，因為篇幅已經有些長了，本講到這裡先告一段落。在這一講中，我們給讀者介紹了g2o的安裝與基本使用方法。為保證程式簡單易懂，我們暫時沒有用它構建實用的圖程式，這會在下一講中實現。同時，g2o也可以用來做迴環檢測，丟失恢復等工作，使得slam過程更加穩定可靠，真是一個方便的工具呢！
本講程式碼：https://github.com/gaoxiang12/rgbd-slam-tutorial-gx/tree/master/part%20VI
資料請見上一講。
未完待續
TIPS
- 現在（2016.10）github上的g2o已經可以在14.04下正常編譯安裝了，所以本文當中有些迂迴的安裝步驟就沒必要了。請讀者按照g2o的readme檔案進行編譯安裝即可。