一起做RGB-D SLAM(4)
来源:互联网 发布:苹果软件商店 编辑:程序博客网 时间:2024/05/19 20:42
第四讲 点云拼接
广告:“一起做”系列的代码网址:https://github.com/gaoxiang12/rgbd-slam-tutorial-gx 当博客更新时代码也会随着更新。 SLAM技术交流群:254787961
读者朋友们大家好!尽管还没到一周,我们的教程又继续更新了,因为暑假实在太闲了嘛!
上讲回顾
上一讲中,我们理解了如何利用图像中的特征点,估计相机的运动。最后,我们得到了一个旋转向量与平移向量。那么读者可能会问:这两个向量有什么用呢?在这一讲里,我们就要使用这两个向量,把两张图像的点云给拼接起来,形成更大的点云。
首先,我们把上一讲的内容封装进slamBase库中,代码如下:
include/slamBase.h
1 // 帧结构 2 struct FRAME 3 { 4 cv::Mat rgb, depth; //该帧对应的彩色图与深度图 5 cv::Mat desp; //特征描述子 6 vector<cv::KeyPoint> kp; //关键点 7 }; 8 9 // PnP 结果10 struct RESULT_OF_PNP11 {12 cv::Mat rvec, tvec;13 int inliers;14 };15 16 // computeKeyPointsAndDesp 同时提取关键点与特征描述子17 void computeKeyPointsAndDesp( FRAME& frame, string detector, string descriptor );18 19 // estimateMotion 计算两个帧之间的运动20 // 输入:帧1和帧2, 相机内参21 RESULT_OF_PNP estimateMotion( FRAME& frame1, FRAME& frame2, CAMERA_INTRINSIC_PARAMETERS& camera );
我们把关键帧和PnP的结果都封成了结构体,以便将来别的程序调用。这两个函数的实现如下:
src/slamBase.cpp
1 // computeKeyPointsAndDesp 同时提取关键点与特征描述子 2 void computeKeyPointsAndDesp( FRAME& frame, string detector, string descriptor ) 3 { 4 cv::Ptr<cv::FeatureDetector> _detector; 5 cv::Ptr<cv::DescriptorExtractor> _descriptor; 6 7 cv::initModule_nonfree(); 8 _detector = cv::FeatureDetector::create( detector.c_str() ); 9 _descriptor = cv::DescriptorExtractor::create( descriptor.c_str() );10 11 if (!_detector || !_descriptor)12 {13 cerr<<"Unknown detector or discriptor type !"<<detector<<","<<descriptor<<endl;14 return;15 }16 17 _detector->detect( frame.rgb, frame.kp );18 _descriptor->compute( frame.rgb, frame.kp, frame.desp );19 20 return;21 }22 23 // estimateMotion 计算两个帧之间的运动24 // 输入:帧1和帧225 // 输出:rvec 和 tvec26 RESULT_OF_PNP estimateMotion( FRAME& frame1, FRAME& frame2, CAMERA_INTRINSIC_PARAMETERS& camera )27 {28 static ParameterReader pd;29 vector< cv::DMatch > matches;30 cv::FlannBasedMatcher matcher;31 matcher.match( frame1.desp, frame2.desp, matches );32 33 cout<<"find total "<<matches.size()<<" matches."<<endl;34 vector< cv::DMatch > goodMatches;35 double minDis = 9999;36 double good_match_threshold = atof( pd.getData( "good_match_threshold" ).c_str() );37 for ( size_t i=0; i<matches.size(); i++ )38 {39 if ( matches[i].distance < minDis )40 minDis = matches[i].distance;41 }42 43 for ( size_t i=0; i<matches.size(); i++ )44 {45 if (matches[i].distance < good_match_threshold*minDis)46 goodMatches.push_back( matches[i] );47 }48 49 cout<<"good matches: "<<goodMatches.size()<<endl;50 // 第一个帧的三维点51 vector<cv::Point3f> pts_obj;52 // 第二个帧的图像点53 vector< cv::Point2f > pts_img;54 55 // 相机内参56 for (size_t i=0; i<goodMatches.size(); i++)57 {58 // query 是第一个, train 是第二个59 cv::Point2f p = frame1.kp[goodMatches[i].queryIdx].pt;60 // 获取d是要小心!x是向右的,y是向下的,所以y才是行,x是列!61 ushort d = frame1.depth.ptr<ushort>( int(p.y) )[ int(p.x) ];62 if (d == 0)63 continue;64 pts_img.push_back( cv::Point2f( frame2.kp[goodMatches[i].trainIdx].pt ) );65 66 // 将(u,v,d)转成(x,y,z)67 cv::Point3f pt ( p.x, p.y, d );68 cv::Point3f pd = point2dTo3d( pt, camera );69 pts_obj.push_back( pd );70 }71 72 double camera_matrix_data[3][3] = {73 {camera.fx, 0, camera.cx},74 {0, camera.fy, camera.cy},75 {0, 0, 1}76 };77 78 cout<<"solving pnp"<<endl;79 // 构建相机矩阵80 cv::Mat cameraMatrix( 3, 3, CV_64F, camera_matrix_data );81 cv::Mat rvec, tvec, inliers;82 // 求解pnp83 cv::solvePnPRansac( pts_obj, pts_img, cameraMatrix, cv::Mat(), rvec, tvec, false, 100, 1.0, 100, inliers );84 85 RESULT_OF_PNP result;86 result.rvec = rvec;87 result.tvec = tvec;88 result.inliers = inliers.rows;89 90 return result;91 }
此外,我们还实现了一个简单的参数读取类。这个类读取一个参数的文本文件,能够以关键字的形式提供文本文件中的变量:
include/slamBase.h
1 // 参数读取类 2 class ParameterReader 3 { 4 public: 5 ParameterReader( string filename="./parameters.txt" ) 6 { 7 ifstream fin( filename.c_str() ); 8 if (!fin) 9 {10 cerr<<"parameter file does not exist."<<endl;11 return;12 }13 while(!fin.eof())14 {15 string str;16 getline( fin, str );17 if (str[0] == '#')18 {19 // 以‘#’开头的是注释20 continue;21 }22 23 int pos = str.find("=");24 if (pos == -1)25 continue;26 string key = str.substr( 0, pos );27 string value = str.substr( pos+1, str.length() );28 data[key] = value;29 30 if ( !fin.good() )31 break;32 }33 }34 string getData( string key )35 {36 map<string, string>::iterator iter = data.find(key);37 if (iter == data.end())38 {39 cerr<<"Parameter name "<<key<<" not found!"<<endl;40 return string("NOT_FOUND");41 }42 return iter->second;43 }44 public:45 map<string, string> data;46 };
它读的参数文件是长这个样子的:
# 这是一个参数文件
嗯,参数文件里,以“变量名=值”的形式定义变量。以井号开头的是注释啦!是不是很简单呢?
小萝卜:师兄你为何对yaml有一股强烈的怨念?
师兄:哎,不说了……总之实现简单的功能,就用简单的东西,特别是从教程上来说更应该如此啦。
现在,如果我们想更改特征类型,就只需在parameters.txt文件里进行修改,不必编译源代码了。这对接下去的各种调试都会很有帮助。
拼接点云
点云的拼接,实质上是对点云做变换的过程。这个变换往往是用变换矩阵(transform matrix)来描述的:
由于变换矩阵结合了旋转和缩放,是一种较为经济实用的表达方式。它在机器人和许多三维空间相关的科学中都有广泛的应用。PCL里提供了点云的变换函数,只要给定了变换矩阵,就能对移动整个点云:
pcl::transformPointCloud( input, output, T );
小萝卜:所以我们现在就是要把OpenCV里的旋转向量、位移向量转换成这个矩阵喽?
师兄:对!opencv认为旋转矩阵R,虽然有3×3那么大,自由变量却只有三个,不够节省空间。所以在OpenCV里使用了一个向量来表达旋转。向量的方向是旋转轴,大小则是转过的弧度.
小萝卜:但是我们又把它变成了矩阵啊,这不就没有意义了吗!
师兄:呃,这个,确实如此。不管如何,我们先用罗德里格斯变换(Rodrigues)将旋转向量转换为矩阵,然后“组装”成变换矩阵。代码如下:
src/joinPointCloud.cpp
1 /************************************************************************* 2 > File Name: src/jointPointCloud.cpp 3 > Author: Xiang gao 4 > Mail: gaoxiang12@mails.tsinghua.edu.cn 5 > Created Time: 2015年07月22日 星期三 20时46分08秒 6 ************************************************************************/ 7 8 #include<iostream> 9 using namespace std;10 11 #include "slamBase.h"12 13 #include <opencv2/core/eigen.hpp>14 15 #include <pcl/common/transforms.h>16 #include <pcl/visualization/cloud_viewer.h>17 18 // Eigen !19 #include <Eigen/Core>20 #include <Eigen/Geometry>21 22 int main( int argc, char** argv )23 {24 //本节要拼合data中的两对图像25 ParameterReader pd;26 // 声明两个帧,FRAME结构请见include/slamBase.h27 FRAME frame1, frame2;28 29 //读取图像30 frame1.rgb = cv::imread( "./data/rgb1.png" );31 frame1.depth = cv::imread( "./data/depth1.png", -1);32 frame2.rgb = cv::imread( "./data/rgb2.png" );33 frame2.depth = cv::imread( "./data/depth2.png", -1 );34 35 // 提取特征并计算描述子36 cout<<"extracting features"<<endl;37 string detecter = pd.getData( "detector" );38 string descriptor = pd.getData( "descriptor" );39 40 computeKeyPointsAndDesp( frame1, detecter, descriptor );41 computeKeyPointsAndDesp( frame2, detecter, descriptor );42 43 // 相机内参44 CAMERA_INTRINSIC_PARAMETERS camera;45 camera.fx = atof( pd.getData( "camera.fx" ).c_str());46 camera.fy = atof( pd.getData( "camera.fy" ).c_str());47 camera.cx = atof( pd.getData( "camera.cx" ).c_str());48 camera.cy = atof( pd.getData( "camera.cy" ).c_str());49 camera.scale = atof( pd.getData( "camera.scale" ).c_str() );50 51 cout<<"solving pnp"<<endl;52 // 求解pnp53 RESULT_OF_PNP result = estimateMotion( frame1, frame2, camera );54 55 cout<<result.rvec<<endl<<result.tvec<<endl;56 57 // 处理result58 // 将旋转向量转化为旋转矩阵59 cv::Mat R;60 cv::Rodrigues( result.rvec, R );61 Eigen::Matrix3d r;62 cv::cv2eigen(R, r);63 64 // 将平移向量和旋转矩阵转换成变换矩阵65 Eigen::Isometry3d T = Eigen::Isometry3d::Identity();66 67 Eigen::AngleAxisd angle(r);68 cout<<"translation"<<endl;69 Eigen::Translation<double,3> trans(result.tvec.at<double>(0,0), result.tvec.at<double>(0,1), result.tvec.at<double>(0,2));70 T = angle;71 T(0,3) = result.tvec.at<double>(0,0); 72 T(1,3) = result.tvec.at<double>(0,1); 73 T(2,3) = result.tvec.at<double>(0,2);74 75 // 转换点云76 cout<<"converting image to clouds"<<endl;77 PointCloud::Ptr cloud1 = image2PointCloud( frame1.rgb, frame1.depth, camera );78 PointCloud::Ptr cloud2 = image2PointCloud( frame2.rgb, frame2.depth, camera );79 80 // 合并点云81 cout<<"combining clouds"<<endl;82 PointCloud::Ptr output (new PointCloud());83 pcl::transformPointCloud( *cloud1, *output, T.matrix() );84 *output += *cloud2;85 pcl::io::savePCDFile("data/result.pcd", *output);86 cout<<"Final result saved."<<endl;87 88 pcl::visualization::CloudViewer viewer( "viewer" );89 viewer.showCloud( output );90 while( !viewer.wasStopped() )91 {92 93 }94 return 0;95 }
重点在于59至73行,讲述了这个转换的过程。
变换完毕后,我们就得到了拼合的点云啦:
怎么样?是不是有点成就感了呢?
接下来的事……
至此,我们已经实现了一个只有两帧的SLAM程序。然而,也许你还不知道,这已经是一个视觉里程计(Visual Odometry)啦!只要不断地把进来的数据与上一帧对比,就可以得到完整的运动轨迹以及地图了呢!
小萝卜:这听着已经像是SLAM了呀!
师兄:嗯,要做完整的SLAM,还需要一些东西。以两两匹配为基础的里程计有明显的累积误差,我们需要通过回环检测来消除它。这也是我们后面几讲的主要内容啦!
小萝卜:那下一讲我们要做点什么呢?
师兄:我们先讲讲关键帧的处理,因为把每个图像都放进地图,会导致地图规模增长地太快,所以需要关键帧技术。然后呢,我们要做一个SLAM后端,就要用到g2o啦!
课后作业
由于参数文件可以很方便地调节,请你试试不同的特征点类型,看看哪种类型比较符合你的心意。为此,最好在源代码中加入显示匹配图的代码哦!
未完待续
- 一起做RGB-D SLAM (4)
- 一起做RGB-D SLAM (4)
- 一起做RGB-D SLAM (4)
- 一起做RGB-D SLAM(4)
- 一起做RGB-D SLAM (4) 点云拼接
- 【转】一起做RGB-D SLAM (1)
- 一起做RGB-D SLAM (1)
- 一起做RGB-D SLAM (2)
- 一起做RGB-D SLAM (3)
- 一起做RGB-D SLAM (5)
- 一起做RGB-D SLAM (6)
- 一起做RGB-D SLAM (3)
- 一起做RGB-D SLAM (1)
- 一起做RGB-D SLAM (1)
- 一起做RGB-D SLAM (2)
- 一起做RGB-D SLAM (3)
- 一起做RGB-D SLAM (5)
- 一起做RGB-D SLAM (6)
- Python+Selenium--XPath的使用
- 判断字符串是否包含中文
- C++ Primer 第五版习题10.32
- Hexo+Next在Github上搭建博客教程
- 修改 DbVisualizer 自动完成快捷键
- 一起做RGB-D SLAM(4)
- Firewall的常用命令
- 灵活运用 SQL SERVER FOR XML PATH
- svn: E155021: This client is too old to work with the working copy at..... No changes detected
- CentOS搭建git服务器实测
- case 关键字后面的的值有什么要求吗?
- 安卓动画之Frame Animation(逐帧动画)
- AD16 PCB规则详解
- Spring MVC REST异常处理最佳实践(下)