2016.11 更新
- 把原文的SIFT替換成了ORB,這樣你可以在沒有nonfree模塊下使用本程序了。
- OpenCV可以使用 apt-get install libopencv-dev ,一樣能成功。
- 因爲換成了ORB,所以調整了good match的閾值,並且匹配時需要使用 Brute Force match。
- 請以現在的github上源碼爲準。
在上一講中,我們介紹瞭如何使用兩兩匹配,搭建一個視覺里程計。那麼,這個里程計有什麼不足呢?
- 一旦出現了錯誤匹配,整個程序就會跑飛。
- 誤差會累積。常見的現象是:相機轉過去的過程能夠做對,但轉回來之後則出現明顯的偏差。
- 效率方面不盡如人意。在線的點雲顯示比較費時。
累積誤差是里程計中不可避免的,後續的相機姿態依賴着前面的姿態。想要保證地圖的準確,必須要保證每次匹配都精確無誤,而這是難以實現的。所以,我們希望用更好的方法來做slam。不僅僅考慮兩幀的信息,而要把所有整的信息都考慮進來,成爲一個全slam問題(full slam)。下圖爲累積誤差的一個例子。右側是原有掃過的地圖,左側是新掃的,可以看到出現了明顯的不重合。
所以,我們這一講要介紹姿態圖(pose graph),它是目前視覺slam裏最常用的方法之一。
姿態圖(原理部分)
小蘿蔔:哦我明白了!是不是運籌學書裏講的非線性優化就是這個啊?
師兄:對!根據迭代策略的不同,又可分爲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-dev
1404或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
/*************************************************************************
> File Name: rgbd-slam-tutorial-gx/part V/src/visualOdometry.cpp
> Author: xiang gao
> Mail: [email protected]
> Created Time: 2015年08月15日 星期六 15時35分42秒
* add g2o slam end to visual odometry
************************************************************************/
#include <iostream>
#include <fstream>
#include <sstream>
using namespace std;
#include "slamBase.h"
//g2o的頭文件
#include <g2o/types/slam3d/types_slam3d.h> //頂點類型
#include <g2o/core/sparse_optimizer.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/factory.h>
#include <g2o/core/optimization_algorithm_factory.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/core/robust_kernel.h>
#include <g2o/core/robust_kernel_factory.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
// 給定index,讀取一幀數據
FRAME readFrame( int index, ParameterReader& pd );
// 估計一個運動的大小
double normofTransform( cv::Mat rvec, cv::Mat tvec );
int main( int argc, char** argv )
{
// 前面部分和vo是一樣的
ParameterReader pd;
int startIndex = atoi( pd.getData( "start_index" ).c_str() );
int endIndex = atoi( pd.getData( "end_index" ).c_str() );
// initialize
cout<<"Initializing ..."<<endl;
int currIndex = startIndex; // 當前索引爲currIndex
FRAME lastFrame = readFrame( currIndex, pd ); // 上一幀數據
// 我們總是在比較currFrame和lastFrame
string detector = pd.getData( "detector" );
string descriptor = pd.getData( "descriptor" );
CAMERA_INTRINSIC_PARAMETERS camera = getDefaultCamera();
computeKeyPointsAndDesp( lastFrame, detector, descriptor );
PointCloud::Ptr cloud = image2PointCloud( lastFrame.rgb, lastFrame.depth, camera );
pcl::visualization::CloudViewer viewer("viewer");
// 是否顯示點雲
bool visualize = pd.getData("visualize_pointcloud")==string("yes");
int min_inliers = atoi( pd.getData("min_inliers").c_str() );
double max_norm = atof( pd.getData("max_norm").c_str() );
/*******************************
// 新增:有關g2o的初始化
*******************************/
// 選擇優化方法
typedef g2o::BlockSolver_6_3 SlamBlockSolver;
typedef g2o::LinearSolverCSparse< SlamBlockSolver::PoseMatrixType > SlamLinearSolver;
// 初始化求解器
SlamLinearSolver* linearSolver = new SlamLinearSolver();
linearSolver->setBlockOrdering( false );
SlamBlockSolver* blockSolver = new SlamBlockSolver( linearSolver );
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( blockSolver );
g2o::SparseOptimizer globalOptimizer; // 最後用的就是這個東東
globalOptimizer.setAlgorithm( solver );
// 不要輸出調試信息
globalOptimizer.setVerbose( false );
// 向globalOptimizer增加第一個頂點
g2o::VertexSE3* v = new g2o::VertexSE3();
v->setId( currIndex );
v->setEstimate( Eigen::Isometry3d::Identity() ); //估計爲單位矩陣
v->setFixed( true ); //第一個頂點固定,不用優化
globalOptimizer.addVertex( v );
int lastIndex = currIndex; // 上一幀的id
for ( currIndex=startIndex+1; currIndex<endIndex; currIndex++ )
{
cout<<"Reading files "<<currIndex<<endl;
FRAME currFrame = readFrame( currIndex,pd ); // 讀取currFrame
computeKeyPointsAndDesp( currFrame, detector, descriptor );
// 比較currFrame 和 lastFrame
RESULT_OF_PNP result = estimateMotion( lastFrame, currFrame, camera );
if ( result.inliers < min_inliers ) //inliers不夠,放棄該幀
continue;
// 計算運動範圍是否太大
double norm = normofTransform(result.rvec, result.tvec);
cout<<"norm = "<<norm<<endl;
if ( norm >= max_norm )
continue;
Eigen::Isometry3d T = cvMat2Eigen( result.rvec, result.tvec );
cout<<"T="<<T.matrix()<<endl;
// cloud = joinPointCloud( cloud, currFrame, T, camera );
// 向g2o中增加這個頂點與上一幀聯繫的邊
// 頂點部分
// 頂點只需設定id即可
g2o::VertexSE3 *v = new g2o::VertexSE3();
v->setId( currIndex );
v->setEstimate( Eigen::Isometry3d::Identity() );
globalOptimizer.addVertex(v);
// 邊部分
g2o::EdgeSE3* edge = new g2o::EdgeSE3();
// 連接此邊的兩個頂點id
edge->vertices() [0] = globalOptimizer.vertex( lastIndex );
edge->vertices() [1] = globalOptimizer.vertex( currIndex );
// 信息矩陣
Eigen::Matrix<double, 6, 6> information = Eigen::Matrix< double, 6,6 >::Identity();
// 信息矩陣是協方差矩陣的逆,表示我們對邊的精度的預先估計
// 因爲pose爲6D的,信息矩陣是6*6的陣,假設位置和角度的估計精度均爲0.1且互相獨立
// 那麼協方差則爲對角爲0.01的矩陣,信息陣則爲100的矩陣
information(0,0) = information(1,1) = information(2,2) = 100;
information(3,3) = information(4,4) = information(5,5) = 100;
// 也可以將角度設大一些,表示對角度的估計更加準確
edge->setInformation( information );
// 邊的估計即是pnp求解之結果
edge->setMeasurement( T );
// 將此邊加入圖中
globalOptimizer.addEdge(edge);
lastFrame = currFrame;
lastIndex = currIndex;
}
// pcl::io::savePCDFile( "data/result.pcd", *cloud );
// 優化所有邊
cout<<"optimizing pose graph, vertices: "<<globalOptimizer.vertices().size()<<endl;
globalOptimizer.save("./data/result_before.g2o");
globalOptimizer.initializeOptimization();
globalOptimizer.optimize( 100 ); //可以指定優化步數
globalOptimizer.save( "./data/result_after.g2o" );
cout<<"Optimization done."<<endl;
globalOptimizer.clear();
return 0;
}
FRAME readFrame( int index, ParameterReader& pd )
{
FRAME f;
string rgbDir = pd.getData("rgb_dir");
string depthDir = pd.getData("depth_dir");
string rgbExt = pd.getData("rgb_extension");
string depthExt = pd.getData("depth_extension");
stringstream ss;
ss<<rgbDir<<index<<rgbExt;
string filename;
ss>>filename;
f.rgb = cv::imread( filename );
ss.clear();
filename.clear();
ss<<depthDir<<index<<depthExt;
ss>>filename;
f.depth = cv::imread( filename, -1 );
f.frameID = index;
return f;
}
double normofTransform( cv::Mat rvec, cv::Mat tvec )
{
return fabs(min(cv::norm(rvec), 2*M_PI-cv::norm(rvec)))+ fabs(cv::norm(tvec));
}
其中,大部分代碼和上一講是一樣的,此外新增了幾段g2o的初始化與簡單使用。
使用g2o圖優化的簡要步驟:第一步,構建一個求解器:globalOptimizer
// 選擇優化方法
typedef g2o::BlockSolver_6_3 SlamBlockSolver;
typedef g2o::LinearSolverCSparse< SlamBlockSolver::PoseMatrixType > SlamLinearSolver;
// 初始化求解器
SlamLinearSolver* linearSolver = new SlamLinearSolver();
linearSolver->setBlockOrdering( false );
SlamBlockSolver* blockSolver = new SlamBlockSolver( linearSolver );
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( blockSolver );
g2o::SparseOptimizer globalOptimizer; // 最後用的就是這個東東
globalOptimizer.setAlgorithm( solver );
// 不要輸出調試信息
globalOptimizer.setVerbose( false );
然後,在求解器內添加點和邊:// 添加點
g2o::VertexSE3* v = new g2o::VertexSE3();
// 設置點v ...
globalOptimizer.addVertex( v );
// 添加邊
g2o::EdgeSE3* edge = new g2o::EdgeSE3();
// 設置邊 edge ...
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文件進行編譯安裝即可。