简单介绍

输入：指定数量的RGB+Depth深度图，相关参数配置文件.txt输出：实时显示的三维模型，和最终的.pcd点云图

主函数

/************************************************************************* > File Name: rgbd-slam-tutorial-gx/part V/src/visualOdometry.cpp > Author: xiang gao > Mail: gaoxiang12@mails.tsinghua.edu.cn > Created Time: 2015年08月01日 星期六 15时35分42秒 > Update: add comments by john 2020-10-16 ************************************************************************/ #include <iostream> #include <fstream> #include <sstream> using namespace std; #include "slamBase.h" // 给定index，读取一帧数据 FRAME readFrame( int index, ParameterReader& pd ); // 度量运动的大小 double normofTransform( cv::Mat rvec, cv::Mat tvec ); int main( int argc, char** argv ) { 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 //1. 读取当前帧的rgb+depth图像 FRAME lastFrame = readFrame( currIndex, pd ); // 上一帧数据 // 我们总是在比较currFrame和lastFrame string detector = pd.getData( "detector" ); string descriptor = pd.getData( "descriptor" ); //2. 获取相机内参： Tracking::mpCamera CAMERA_INTRINSIC_PARAMETERS camera = getDefaultCamera(); //计算出lastFrame的关键点和描述子，存在当前帧结构中 computeKeyPointsAndDesp( lastFrame, detector, descriptor ); //3. 根据rgb+depth+相机内参，获取最后一帧相机坐标系下的点云。 JOHNTODO 可以使用 PointCloud::Ptr cloud = image2PointCloud( lastFrame.rgb, lastFrame.depth, camera ); //4. 定义点云视图viewer 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() ); //5.遍历每一帧 for ( currIndex=startIndex+1; currIndex<endIndex; currIndex++ ) { cout<<"Reading files "<<currIndex<<endl; FRAME currFrame = readFrame( currIndex,pd ); // 读取currFrame computeKeyPointsAndDesp( currFrame, detector, descriptor ); // 比较currFrame 和 lastFrame // JOHNCOMMENT: 获取当前帧currFrame关于上一帧的外参,此处用到图片的 descriptor // 注意: 此处的相机外参 result 中的R,t 是当前帧相对于上一帧的旋转和平移 // 相机内参可以用于: "像素点坐标"到"相机坐标系下的空间点坐标"的转换 // 相机外参可以用于: "相机坐标系下的空间点坐标"到"世界坐标系下的对应空间点坐标"的转换 //6. 获取相机关于最后一帧的运动 RESULT_OF_PNP result = estimateMotion( lastFrame, currFrame, camera ); //7. 过滤掉质量差的帧 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; //8. 添加当前帧点云到历史点云中 // JOHNCOMMENT: 将当前帧的点云加入到上一帧(或者说之前的历史)点云中， // 更具体地说: 将历史点云使用当前帧的位姿进行变换, 和当前点云对齐,然后加到当前点云上, // 使点云图在pcl的view中能一直以当前帧(或者说当前的相机位姿)的视角动起来 // 点云变换函数: pcl::transformPointCloud( *original, *output, T.matrix()); cloud = joinPointCloud( cloud, currFrame, T, camera ); //9. 决定是否实时显示 if ( visualize == true ) viewer.showCloud( cloud ); lastFrame = currFrame; } //10. 保存点云文件 pcl::io::savePCDFile( "data/result.pcd", *cloud ); //保存最终的点云结果 JOHNTODO return 0; } //从配置文件pd中读取rgb+depth作为帧的属性，返回Frame 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 ); 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)); }

效果

完整代码： https://github.com/gaoxiang12/rgbd-slam-tutorial-gx/tree/master/part V

数据位置：http://yun.baidu.com/s/1i33uvw5

参考：一起做RGB-D SLAM (5)