第七講 添加回環檢測
2016.11 更新
- 把原文的SIFT替換成了ORB,這樣你可以在沒有nonfree模塊下使用本程序了。
- 迴環檢測的閾值作出了相應的調整。
- 請以現在的github上源碼爲準。
簡單迴環檢測的流程
上一講中,我們介紹了圖優化軟件g2o的使用。本講,我們將實現一個簡單的迴環檢測程序,利用g2o提升slam軌跡與地圖的質量。本講結束後,讀者朋友們將得到一個完整的slam程序,可以跑通我們在百度雲上給出的數據了。所以呢,本講也將成爲我們“一起做”系列的終點啦。
小蘿蔔:這麼快就要結束了嗎師兄?
師兄:嗯,因爲我想要說的都教給大家了嘛。不過,儘管如此,這個教程的程序還是比較初步的,可以進一步進行效率、魯棒性方面的優化,這就要靠大家後續的努力啦。同時我的暑假也將近結束,要開始新一輪的工作了呢。
好的,話不多說,先來講講,上一講的程序離完整的slam還有哪些距離。主要說來有兩點:
- 關鍵幀的提取。把每一幀都拼到地圖是去是不明智的。因爲幀與幀之間距離很近,導致地圖需要頻繁更新,浪費時間與空間。所以,我們希望,當機器人的運動超過一定間隔,就增加一個“關鍵幀”。最後只需把關鍵幀拼到地圖裏就行了。
- 迴環的檢測。迴環的本質是識別曾經到過的地方。最簡單的迴環檢測策略,就是把新來的關鍵幀與之前所有的關鍵幀進行比較,不過這樣會導致越往後,需要比較的幀越多。所以,稍微快速一點的方法是在過去的幀裏隨機挑選一些,與之進行比較。更進一步的,也可以用圖像處理/模式識別的方法計算圖像間的相似性,對相似的圖像進行檢測。
把這兩者合在一起,就得到了我們slam程序的基本流程。以下爲僞碼:
小蘿蔔:slam流程都是這樣的嗎?
師兄:大體上如此,也可以作一些更改。例如在線跑的話呢,可以定時進行一次優化與拼圖。或者,在成功檢測到迴環時,同時檢測這兩個幀附近的幀,那樣得到的邊就更多啦。再有呢,如果要做實用的程序,還要考慮機器人如何運動,如果跟丟了怎麼進行恢復等一些實際的問題呢。
實現代碼
代碼依舊是在上一講的代碼上進行更改得來的。由於是完整的程序,稍微有些長,請大家慢慢看:
src/slam.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
* add keyframe and simple loop closure
************************************************************************/
#include <iostream>
#include <fstream>
#include <sstream>
using namespace std;
#include "slamBase.h"
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/passthrough.h>
#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>
// 把g2o的定義放到前面
typedef g2o::BlockSolver_6_3 SlamBlockSolver;
typedef g2o::LinearSolverCSparse< SlamBlockSolver::PoseMatrixType > SlamLinearSolver;
// 給定index,讀取一幀數據
FRAME readFrame( int index, ParameterReader& pd );
// 估計一個運動的大小
double normofTransform( cv::Mat rvec, cv::Mat tvec );
// 檢測兩個幀,結果定義
enum CHECK_RESULT {NOT_MATCHED=0, TOO_FAR_AWAY, TOO_CLOSE, KEYFRAME};
// 函數聲明
CHECK_RESULT checkKeyframes( FRAME& f1, FRAME& f2, g2o::SparseOptimizer& opti, bool is_loops=false );
// 檢測近距離的迴環
void checkNearbyLoops( vector<FRAME>& frames, FRAME& currFrame, g2o::SparseOptimizer& opti );
// 隨機檢測迴環
void checkRandomLoops( vector<FRAME>& frames, FRAME& currFrame, g2o::SparseOptimizer& opti );
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() );
// 所有的關鍵幀都放在了這裏
vector< FRAME > keyframes;
// initialize
cout<<"Initializing ..."<<endl;
int currIndex = startIndex; // 當前索引爲currIndex
FRAME currFrame = readFrame( currIndex, pd ); // 當前幀數據
string detector = pd.getData( "detector" );
string descriptor = pd.getData( "descriptor" );
CAMERA_INTRINSIC_PARAMETERS camera = getDefaultCamera();
computeKeyPointsAndDesp( currFrame, detector, descriptor );
PointCloud::Ptr cloud = image2PointCloud( currFrame.rgb, currFrame.depth, camera );
/*******************************
// 新增:有關g2o的初始化
*******************************/
// 初始化求解器
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 );
keyframes.push_back( currFrame );
double keyframe_threshold = atof( pd.getData("keyframe_threshold").c_str() );
bool check_loop_closure = pd.getData("check_loop_closure")==string("yes");
for ( currIndex=startIndex+1; currIndex<endIndex; currIndex++ )
{
cout<<"Reading files "<<currIndex<<endl;
FRAME currFrame = readFrame( currIndex,pd ); // 讀取currFrame
computeKeyPointsAndDesp( currFrame, detector, descriptor ); //提取特徵
CHECK_RESULT result = checkKeyframes( keyframes.back(), currFrame, globalOptimizer ); //匹配該幀與keyframes裏最後一幀
switch (result) // 根據匹配結果不同採取不同策略
{
case NOT_MATCHED:
//沒匹配上,直接跳過
cout<<RED"Not enough inliers."<<endl;
break;
case TOO_FAR_AWAY:
// 太近了,也直接跳
cout<<RED"Too far away, may be an error."<<endl;
break;
case TOO_CLOSE:
// 太遠了,可能出錯了
cout<<RESET"Too close, not a keyframe"<<endl;
break;
case KEYFRAME:
cout<<GREEN"This is a new keyframe"<<endl;
// 不遠不近,剛好
/**
* This is important!!
* This is important!!
* This is important!!
* (very important so I've said three times!)
*/
// 檢測迴環
if (check_loop_closure)
{
checkNearbyLoops( keyframes, currFrame, globalOptimizer );
checkRandomLoops( keyframes, currFrame, globalOptimizer );
}
keyframes.push_back( currFrame );
break;
default:
break;
}
}
// 優化
cout<<RESET"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;
// 拼接點雲地圖
cout<<"saving the point cloud map..."<<endl;
PointCloud::Ptr output ( new PointCloud() ); //全局地圖
PointCloud::Ptr tmp ( new PointCloud() );
pcl::VoxelGrid<PointT> voxel; // 網格濾波器,調整地圖分辨率
pcl::PassThrough<PointT> pass; // z方向區間濾波器,由於rgbd相機的有效深度區間有限,把太遠的去掉
pass.setFilterFieldName("z");
pass.setFilterLimits( 0.0, 4.0 ); //4m以上就不要了
double gridsize = atof( pd.getData( "voxel_grid" ).c_str() ); //分辨圖可以在parameters.txt裏調
voxel.setLeafSize( gridsize, gridsize, gridsize );
for (size_t i=0; i<keyframes.size(); i++)
{
// 從g2o裏取出一幀
g2o::VertexSE3* vertex = dynamic_cast<g2o::VertexSE3*>(globalOptimizer.vertex( keyframes[i].frameID ));
Eigen::Isometry3d pose = vertex->estimate(); //該幀優化後的位姿
PointCloud::Ptr newCloud = image2PointCloud( keyframes[i].rgb, keyframes[i].depth, camera ); //轉成點雲
// 以下是濾波
voxel.setInputCloud( newCloud );
voxel.filter( *tmp );
pass.setInputCloud( tmp );
pass.filter( *newCloud );
// 把點雲變換後加入全局地圖中
pcl::transformPointCloud( *newCloud, *tmp, pose.matrix() );
*output += *tmp;
tmp->clear();
newCloud->clear();
}
voxel.setInputCloud( output );
voxel.filter( *tmp );
//存儲
pcl::io::savePCDFile( "./data/result.pcd", *tmp );
cout<<"Final map is saved."<<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));
}
CHECK_RESULT checkKeyframes( FRAME& f1, FRAME& f2, g2o::SparseOptimizer& opti, bool is_loops)
{
static ParameterReader pd;
static int min_inliers = atoi( pd.getData("min_inliers").c_str() );
static double max_norm = atof( pd.getData("max_norm").c_str() );
static double keyframe_threshold = atof( pd.getData("keyframe_threshold").c_str() );
static double max_norm_lp = atof( pd.getData("max_norm_lp").c_str() );
static CAMERA_INTRINSIC_PARAMETERS camera = getDefaultCamera();
static g2o::RobustKernel* robustKernel = g2o::RobustKernelFactory::instance()->construct( "Cauchy" );
// 比較f1 和 f2
RESULT_OF_PNP result = estimateMotion( f1, f2, camera );
if ( result.inliers < min_inliers ) //inliers不夠,放棄該幀
return NOT_MATCHED;
// 計算運動範圍是否太大
double norm = normofTransform(result.rvec, result.tvec);
if ( is_loops == false )
{
if ( norm >= max_norm )
return TOO_FAR_AWAY; // too far away, may be error
}
else
{
if ( norm >= max_norm_lp)
return TOO_FAR_AWAY;
}
if ( norm <= keyframe_threshold )
return TOO_CLOSE; // too adjacent frame
// 向g2o中增加這個頂點與上一幀聯繫的邊
// 頂點部分
// 頂點只需設定id即可
if (is_loops == false)
{
g2o::VertexSE3 *v = new g2o::VertexSE3();
v->setId( f2.frameID );
v->setEstimate( Eigen::Isometry3d::Identity() );
opti.addVertex(v);
}
// 邊部分
g2o::EdgeSE3* edge = new g2o::EdgeSE3();
// 連接此邊的兩個頂點id
edge->vertices() [0] = opti.vertex( f1.frameID );
edge->vertices() [1] = opti.vertex( f2.frameID );
edge->setRobustKernel( robustKernel );
// 信息矩陣
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求解之結果
Eigen::Isometry3d T = cvMat2Eigen( result.rvec, result.tvec );
edge->setMeasurement( T.inverse() );
// 將此邊加入圖中
opti.addEdge(edge);
return KEYFRAME;
}
void checkNearbyLoops( vector<FRAME>& frames, FRAME& currFrame, g2o::SparseOptimizer& opti )
{
static ParameterReader pd;
static int nearby_loops = atoi( pd.getData("nearby_loops").c_str() );
// 就是把currFrame和 frames裏末尾幾個測一遍
if ( frames.size() <= nearby_loops )
{
// no enough keyframes, check everyone
for (size_t i=0; i<frames.size(); i++)
{
checkKeyframes( frames[i], currFrame, opti, true );
}
}
else
{
// check the nearest ones
for (size_t i = frames.size()-nearby_loops; i<frames.size(); i++)
{
checkKeyframes( frames[i], currFrame, opti, true );
}
}
}
void checkRandomLoops( vector<FRAME>& frames, FRAME& currFrame, g2o::SparseOptimizer& opti )
{
static ParameterReader pd;
static int random_loops = atoi( pd.getData("random_loops").c_str() );
srand( (unsigned int) time(NULL) );
// 隨機取一些幀進行檢測
if ( frames.size() <= random_loops )
{
// no enough keyframes, check everyone
for (size_t i=0; i<frames.size(); i++)
{
checkKeyframes( frames[i], currFrame, opti, true );
}
}
else
{
// randomly check loops
for (int i=0; i<random_loops; i++)
{
int index = rand()%frames.size();
checkKeyframes( frames[index], currFrame, opti, true );
}
}
}
幾點註解:
- 迴環檢測是很怕"false positive"的,即“將實際上不同的地方當成了同一處”,這會導致地圖出現明顯的不一致。所以,在使用g2o時,要在邊裏添加"robust kernel",保證一兩個錯誤的邊不會影響整體結果。
- 我在slambase.h裏添加了一些彩色輸出代碼。運行此程序時,出現綠色信息則是添加新的關鍵幀,紅色爲出錯。
parameters.txt裏定義了檢測迴環的一些參數:
#part 7
keyframe_threshold=0.1
max_norm_lp=5.0
# Loop closure
check_loop_closure=yes
nearby_loops=5
random_loops=5
其中,nearby_loops就是mm,random_loops就是nn啦。這兩個數如果設大一些,匹配的幀就會多,不過太大了就會影響整體速度了呢。
迴環檢測的效果
對代碼進行編譯,然後bin/slam即可看到程序運行啦。
添加了迴環檢測之後呢,g2o文件就不會像上次那樣孤零零的啦,看起來是這樣子的:
怎麼樣?是不是感覺整條軌跡“如絲般順滑”了呢?它不再是上一講那樣一根筋通到底,而是有很多幀間的匹配數據,保證了一兩幀出錯能被其他匹配數據給“拉回來”。
百度雲上的數據最後拼出來是這樣的哦(780幀,關鍵幀62張,幀率5Hz左右):
咖啡臺左側有明顯的人通過的痕跡,導致地圖上出現了他的身影(帥哥你好拉風):
嗯,這個就可以算作是基本的地圖啦。至此,slam的兩大目標:“軌跡”和“地圖”我們都已得到了,可以算是基本上解決了這個問題了。
一些後話
這一個“一起做rgb-d slam”系列,前前後後花了我一個多月的時間。寫代碼,調代碼,然後寫成博文。雖然講的比較囉嗦,總體上希望能對各位slam愛好者、研究者有幫助啦!這樣我既然辛苦也很開心的!
寫作期間,得到了女朋友大臉蓮的不少幫助,也得到了讀者和同行之間的鼓勵,謝謝各位啦!等有工夫,我會把這一堆東西整理成一個pdf供slam初學者進行研究學習的。
slam仍是一個開放的問題,儘管有人曾說“在slam領域發文章越來越難”,然而現在機器人幾大期刊和會議(IJRR/TRO/RAM/JFD/ICRA/IROS...)仍有不少slam方面的文章。雖然我們“獲取軌跡與地圖”的目標已基本實現,但仍有許多工作等我們去做,包括:
- 更好的數學模型(新的濾波器/圖優化理論);
- 新的視覺特徵/不使用特徵的直接方法;
- 動態物體/人的處理;
- 地圖描述/點雲地圖優化/語義地圖
- 長時間/大規模/自動化slam
- 等等……
總之,大家千萬別以爲“slam問題已經有標準答案啦”。等我對slam有新的理解時,也會寫新的博客爲大家介紹的!