一起做RGB-D SLAM(7) （完結篇）
阿新 • • 發佈：2018-12-11
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 odometry7 * add keyframe and simple loop closure 8 ************************************************************************/ 9 10 #include <iostream> 11 #include <fstream> 12 #include <sstream> 13 using namespace std; 14 15 #include "slamBase.h" 16 17 #include <pcl/filters/voxel_grid.h> 18#include <pcl/filters/passthrough.h> 19 20 #include <g2o/types/slam3d/types_slam3d.h> 21 #include <g2o/core/sparse_optimizer.h> 22 #include <g2o/core/block_solver.h> 23 #include <g2o/core/factory.h> 24 #include <g2o/core/optimization_algorithm_factory.h> 25 #include <g2o/core/optimization_algorithm_gauss_newton.h> 26#include <g2o/solvers/csparse/linear_solver_csparse.h> 27 #include <g2o/core/robust_kernel.h> 28 #include <g2o/core/robust_kernel_factory.h> 29 #include <g2o/core/optimization_algorithm_levenberg.h> 30 31 // 把g2o的定義放到前面 32 typedef g2o::BlockSolver_6_3 SlamBlockSolver; 33 typedef g2o::LinearSolverCSparse< SlamBlockSolver::PoseMatrixType > SlamLinearSolver; 34 35 // 給定index，讀取一幀資料 36 FRAME readFrame( int index, ParameterReader& pd ); 37 // 估計一個運動的大小 38 double normofTransform( cv::Mat rvec, cv::Mat tvec ); 39 40 // 檢測兩個幀，結果定義 41 enum CHECK_RESULT {NOT_MATCHED=0, TOO_FAR_AWAY, TOO_CLOSE, KEYFRAME}; 42 // 函式宣告 43 CHECK_RESULT checkKeyframes( FRAME& f1, FRAME& f2, g2o::SparseOptimizer& opti, bool is_loops=false ); 44 // 檢測近距離的迴環 45 void checkNearbyLoops( vector<FRAME>& frames, FRAME& currFrame, g2o::SparseOptimizer& opti ); 46 // 隨機檢測迴環 47 void checkRandomLoops( vector<FRAME>& frames, FRAME& currFrame, g2o::SparseOptimizer& opti ); 48 49 int main( int argc, char** argv ) 50 { 51 // 前面部分和vo是一樣的 52 ParameterReader pd; 53 int startIndex = atoi( pd.getData( "start_index" ).c_str() ); 54 int endIndex = atoi( pd.getData( "end_index" ).c_str() ); 55 56 // 所有的關鍵幀都放在了這裡 57 vector< FRAME > keyframes; 58 // initialize 59 cout<<"Initializing ..."<<endl; 60 int currIndex = startIndex; // 當前索引為currIndex 61 FRAME currFrame = readFrame( currIndex, pd ); // 當前幀資料 62 63 string detector = pd.getData( "detector" ); 64 string descriptor = pd.getData( "descriptor" ); 65 CAMERA_INTRINSIC_PARAMETERS camera = getDefaultCamera(); 66 computeKeyPointsAndDesp( currFrame, detector, descriptor ); 67 PointCloud::Ptr cloud = image2PointCloud( currFrame.rgb, currFrame.depth, camera ); 68 69 /******************************* 70 // 新增:有關g2o的初始化 71 *******************************/ 72 // 初始化求解器 73 SlamLinearSolver* linearSolver = new SlamLinearSolver(); 74 linearSolver->setBlockOrdering( false ); 75 SlamBlockSolver* blockSolver = new SlamBlockSolver( linearSolver ); 76 g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( blockSolver ); 77 78 g2o::SparseOptimizer globalOptimizer; // 最後用的就是這個東東 79 globalOptimizer.setAlgorithm( solver ); 80 // 不要輸出除錯資訊 81 globalOptimizer.setVerbose( false ); 82 83 84 // 向globalOptimizer增加第一個頂點 85 g2o::VertexSE3* v = new g2o::VertexSE3(); 86 v->setId( currIndex ); 87 v->setEstimate( Eigen::Isometry3d::Identity() ); //估計為單位矩陣 88 v->setFixed( true ); //第一個頂點固定，不用優化 89 globalOptimizer.addVertex( v ); 90 91 keyframes.push_back( currFrame ); 92 93 double keyframe_threshold = atof( pd.getData("keyframe_threshold").c_str() ); 94 95 bool check_loop_closure = pd.getData("check_loop_closure")==string("yes"); 96 for ( currIndex=startIndex+1; currIndex<endIndex; currIndex++ ) 97 { 98 cout<<"Reading files "<<currIndex<<endl; 99 FRAME currFrame = readFrame( currIndex,pd ); // 讀取currFrame100 computeKeyPointsAndDesp( currFrame, detector, descriptor ); //提取特徵101 CHECK_RESULT result = checkKeyframes( keyframes.back(), currFrame, globalOptimizer ); //匹配該幀與keyframes裡最後一幀102 switch (result) // 根據匹配結果不同採取不同策略103 {104 case NOT_MATCHED:105 //沒匹配上，直接跳過106 cout<<RED"Not enough inliers."<<endl;107 break;108 case TOO_FAR_AWAY:109 // 太近了，也直接跳110 cout<<RED"Too far away, may be an error."<<endl;111 break;112 case TOO_CLOSE:113 // 太遠了，可能出錯了114 cout<<RESET"Too close, not a keyframe"<<endl;115 break;116 case KEYFRAME:117 cout<<GREEN"This is a new keyframe"<<endl;118 // 不遠不近，剛好119 /**120 * This is important!!121 * This is important!!122 * This is important!!123 * (very important so I've said three times!)124 */125 // 檢測迴環126 if (check_loop_closure)127 {128 checkNearbyLoops( keyframes, currFrame, globalOptimizer );129 checkRandomLoops( keyframes, currFrame, globalOptimizer );130 }131 keyframes.push_back( currFrame );132 break;133 default:134 break;135 }136 137 }138 139 // 優化140 cout<<RESET"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 // 拼接點雲地圖148 cout<<"saving the point cloud map..."<<endl;149 PointCloud::Ptr output ( new PointCloud() ); //全域性地圖150 PointCloud::Ptr tmp ( new PointCloud() );151 152 pcl::VoxelGrid<PointT> voxel; // 網格濾波器，調整地圖解析度153 pcl::PassThrough<PointT> pass; // z方向區間濾波器，由於rgbd相機的有效深度區間有限，把太遠的去掉154 pass.setFilterFieldName("z");155 pass.setFilterLimits( 0.0, 4.0 ); //4m以上就不要了156 157 double gridsize = atof( pd.getData( "voxel_grid" ).c_str() ); //分辨圖可以在parameters.txt裡調158 voxel.setLeafSize( gridsize, gridsize, gridsize );159 160 for (size_t i=0; i<keyframes.size(); i++)161 {162 // 從g2o裡取出一幀163 g2o::VertexSE3* vertex = dynamic_cast<g2o::VertexSE3*>(globalOptimizer.vertex( keyframes[i].frameID ));164 Eigen::Isometry3d pose = vertex->estimate(); //該幀優化後的位姿165 PointCloud::Ptr newCloud = image2PointCloud( keyframes[i].rgb, keyframes[i].depth, camera ); //轉成點雲166 // 以下是濾波167 voxel.setInputCloud( newCloud );168 voxel.filter( *tmp );169 pass.setInputCloud( tmp );170 pass.filter( *newCloud );171 // 把點雲變換後加入全域性地圖中172 pcl::transformPointCloud( *newCloud, *tmp, pose.matrix() );173 *output += *tmp;174 tmp->clear();175 newCloud->clear();176 }177 178 voxel.setInputCloud( output );179 voxel.filter( *tmp );180 //儲存181 pcl::io::savePCDFile( "./data/result.pcd", *tmp );182 183 cout<<"Final map is saved."<<endl;184 globalOptimizer.clear();185 186 return 0;187 }188 189 FRAME readFrame( int index, ParameterReader& pd )190 {191 FRAME f;192 string rgbDir = pd.getData("rgb_dir");193 string depthDir = pd.getData("depth_dir");194 195 string rgbExt = pd.getData("rgb_extension");196 string depthExt = pd.getData("depth_extension");197 198 stringstream ss;199 ss<<rgbDir<<index<<rgbExt;200 string filename;201 ss>>filename;202 f.rgb = cv::imread( filename );203 204 ss.clear();205 filename.clear();206 ss<<depthDir<<index<<depthExt;207 ss>>filename;208 209 f.depth = cv::imread( filename, -1 );210 f.frameID = index;211 return f;212 }213 214 double normofTransform( cv::Mat rvec, cv::Mat tvec )215 {216 return fabs(min(cv::norm(rvec), 2*M_PI-cv::norm(rvec)))+ fabs(cv::norm(tvec));217 }218 219 CHECK_RESULT checkKeyframes( FRAME& f1, FRAME& f2, g2o::SparseOptimizer& opti, bool is_loops)220 {221 static ParameterReader pd;222 static int min_inliers = atoi( pd.getData("min_inliers").c_str() );223 static double max_norm = atof( pd.getData("max_norm").c_str() );224 static double keyframe_threshold = atof( pd.getData("keyframe_threshold").c_str() );225 static double max_norm_lp = atof( pd.getData("max_norm_lp").c_str() );226 static CAMERA_INTRINSIC_PARAMETERS camera = getDefaultCamera();227 static g2o::RobustKernel* robustKernel = g2o::RobustKernelFactory::instance()->construct( "Cauchy" );228 // 比較f1 和 f2229 RESULT_OF_PNP result = estimateMotion( f1, f2, camera );230 if ( result.inliers < min_inliers ) //inliers不夠，放棄該幀231 return NOT_MATCHED;232 // 計算運動範圍是否太大233 double norm = normofTransform(result.rvec, result.tvec);234 if ( is_loops == false )235 {236 if ( norm >= max_norm )237 return TOO_FAR_AWAY; // too far away, may be error238 }239 else240 {241 if ( norm >= max_norm_lp)242 return TOO_FAR_AWAY;243 }244 245 if ( norm <= keyframe_threshold )246 return TOO_CLOSE; // too adjacent frame247 // 向g2o中增加這個頂點與上一幀聯絡的邊248 // 頂點部分249 // 頂點只需設定id即可250 if (is_loops == false)251 {252 g2o::VertexSE3 *v = new g2o::VertexSE3();253 v->setId( f2.frameID );254 v->setEstimate( Eigen::Isometry3d::Identity() );255 opti.addVertex(v);256 }257 // 邊部分258 g2o::EdgeSE3* edge = new g2o::EdgeSE3();259 // 連線此邊的兩個頂點id260 edge->vertices() [0] = opti.vertex( f1.frameID );261 edge->vertices() [1] = opti.vertex( f2.frameID );262 edge->setRobustKernel( robustKernel );263 // 資訊矩陣264 Eigen::Matrix<double, 6, 6> information = Eigen::Matrix< double, 6,6 >::Identity();265 // 資訊矩陣是協方差矩陣的逆，表示我們對邊的精度的預先估計266 // 因為pose為6D的，資訊矩陣是6*6的陣，假設位置和角度的估計精度均為0.1且互相獨立267 // 那麼協方差則為對角為0.01的矩陣，資訊陣則為100的矩陣268 information(0,0) = information(1,1) = information(2,2) = 100;269 information(3,3) = information(4,4) = information(5,5) = 100;270 // 也可以將角度設大一些，表示對角度的估計更加準確271 edge->setInformation( information );272 // 邊的估計即是pnp求解之結果273 Eigen::Isometry3d T = cvMat2Eigen( result.rvec, result.tvec );274 edge->setMeasurement( T.inverse() );275 // 將此邊加入圖中276 opti.addEdge(edge);277 return KEYFRAME;278 }279 280 void checkNearbyLoops( vector<FRAME>& frames, FRAME& currFrame, g2o::SparseOptimizer& opti )281 {282 static ParameterReader pd;283 static int nearby_loops = atoi( pd.getData("nearby_loops").c_str() );284 285 // 就是把currFrame和 frames裡末尾幾個測一遍286 if ( frames.size() <= nearby_loops )287 {288 // no enough keyframes, check everyone289 for (size_t i=0; i<frames.size(); i++)290 {291 checkKeyframes( frames[i], currFrame, opti, true );292 }293 }294 else295 {296 // check the nearest ones297 for (size_t i = frames.size()-nearby_loops; i<frames.size(); i++)298 {299 checkKeyframes( frames[i], currFrame, opti, true );300 }301 }302 }303 304 void checkRandomLoops( vector<FRAME>& frames, FRAME& currFrame, g2o::SparseOptimizer& opti )305 {306 static ParameterReader pd;307 static int random_loops = atoi( pd.getData("random_loops").c_str() );308 srand( (unsigned int) time(NULL) );309 // 隨機取一些幀進行檢測310 311 if ( frames.size() <= random_loops )312 {313 // no enough keyframes, check everyone314 for (size_t i=0; i<frames.size(); i++)315 {316 checkKeyframes( frames[i], currFrame, opti, true );317 }318 }319 else320 {321 // randomly check loops322 for (int i=0; i<random_loops; i++)323 {324 int index = rand()%frames.size();325 checkKeyframes( frames[index], currFrame, opti, true );326 }327 }328 }