一起做RGB-D SLAM (4)

第四讲 点云拼接

  广告:“一起做”系列的代码网址: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_PNP
11 {
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和帧2
25 // 输出:rvec 和 tvec
26 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     // 求解pnp
83     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! 我再也不用yaml了!简简单单多好!

# part 4 里定义的参数

detector=SIFT
descriptor=SIFT
good_match_threshold=4

# camera
camera.cx=325.5;
camera.cy=253.5;
camera.fx=518.0;
camera.fy=519.0;
camera.scale=1000.0;

  嗯,参数文件里,以“变量名=值”的形式定义变量。以井号开头的是注释啦!是不是很简单呢?

  小萝卜:师兄你为何对yaml有一股强烈的怨念?

  师兄:哎,不说了……总之实现简单的功能,就用简单的东西,特别是从教程上来说更应该如此啦。

  现在,如果我们想更改特征类型,就只需在parameters.txt文件里进行修改,不必编译源代码了。这对接下去的各种调试都会很有帮助。


拼接点云

  点云的拼接,实质上是对点云做变换的过程。这个变换往往是用变换矩阵(transform matrix)来描述的:$$T=\left[ \begin{array}{ll} R_{3 \times 3} & t_{3 \times 1} \\ O_{1 \times 3} & 1 \end{array} \right] \in R^{4 \times 4} $$ 该矩阵的左上部分是一个$3 \times 3$的旋转矩阵,它是一个正交阵。右上部分是$3 \times 1$的位移矢量。左下是$3 \times 1$的缩放矢量,在SLAM中通常取成0,因为环境里的东西不太可能突然变大变小(又没有缩小灯)。右下角是个1. 这样的一个阵可以对点或者其他东西进行齐次变换:$$ \left[ \begin{array}{l} y_1 \\ y_2 \\ y_3 \\ 1 \end{array} \right] = T \cdot \left[ \begin{array}{l} x_1 \\ x_2 \\ x_3 \\ 1 \end{array} \right]$$

  由于变换矩阵结合了旋转和缩放,是一种较为经济实用的表达方式。它在机器人和许多三维空间相关的科学中都有广泛的应用。PCL里提供了点云的变换函数,只要给定了变换矩阵,就能对移动整个点云:

pcl::transformPointCloud( input, output, T );

  小萝卜:所以我们现在就是要把OpenCV里的旋转向量、位移向量转换成这个矩阵喽?

  师兄:对!OpenCV认为旋转矩阵$R$,虽然有$3\times 3$那么大,自由变量却只有三个,不够节省空间。所以在OpenCV里使用了一个向量来表达旋转。向量的方向是旋转轴,大小则是转过的弧度.

  小萝卜:但是我们又把它变成了矩阵啊,这不就没有意义了吗!

  师兄:呃,这个,确实如此。不管如何,我们先用罗德里格斯变换(Rodrigues)将旋转向量转换为矩阵,然后“组装”成变换矩阵。代码如下:

  src/joinPointCloud.cpp

 1 /*************************************************************************
 2     > File Name: src/jointPointCloud.cpp
 3     > Author: Xiang gao
 4     > Mail: [email protected]
 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.h
27     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     // 求解pnp
53     RESULT_OF_PNP result = estimateMotion( frame1, frame2, camera );
54
55     cout<<result.rvec<<endl<<result.tvec<<endl;
56
57     // 处理result
58     // 将旋转向量转化为旋转矩阵
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啦!


课后作业

  由于参数文件可以很方便地调节,请你试试不同的特征点类型,看看哪种类型比较符合你的心意。为此,最好在源代码中加入显示匹配图的代码哦!

未完待续

时间: 2024-11-05 23:31:41

一起做RGB-D SLAM (4)的相关文章

QT使用QgraphicView/Qpainter展示RGB/YUV图像

本文记载说明使用Qt做RGB的渲染流程和问题.笔者认为:做媒体渲染这种数据量大而对象少的应用场景,使用Qpainter更佳,在多种RGB的转换中,YUV转RGB565最小而且字节对齐问题不容易发生,故推荐之. 加入与opengl  ddraw sdl 的 对比. RGB->QImage->QPixMap->Scene->GrapView 图像的数据是以字节为单位保存的,每一行的字节数必须是4的整数倍,不足的补0. (因为我们使用的是32操作系统,因此数据是按照32位对齐的,所以每行

Camera图像处理原理及实例分析-重要图像概念

Camera图像处理原理及实例分析 作者:刘旭晖  [email protected]  转载请注明出处 BLOG:http://blog.csdn.net/colorant/ 主页:http://rgbbones.googlepages.com/ 做为拍照手机的核心模块之一,camera sensor 效果的调整,涉及到众多的参数,如果对基本的光学原理及 sensor 软/硬件对图像处理的原理能有深入的理解和把握的话,对我们的工作将会起到事半功倍的效果.否则,缺乏了理论的指导,只能是凭感觉和经

Camera图像处理原理及实例分析

Camera图像处理原理及实例分析 作者:刘旭晖  [email protected]  转载请注明出处 BLOG:http://blog.csdn.net/colorant/ 主页:http://rgbbones.googlepages.com/ 做为拍照手机的核心模块之一,camera sensor 效果的调整,涉及到众多的参数,如果对基本的光学原理及 sensor 软/硬件对图像处理的原理能有深入的理解和把握的话,对我们的工作将会起到事半功倍的效果.否则,缺乏了理论的指导,只能是凭感觉和经

Unity -----一些可能存在的错误

关于Unity中的资源管理,你可能遇到这些问题 张鑫 8 个月前 原文链接:关于Unity中的资源管理,你可能遇到这些问题 - Blog 在优化Unity项目时,对资源的管理可谓是个系统纷繁的大工程.鉴于Unity独特又绝(cao)妙(dan)资源打包的AssetBundle管理机制,不同资源的属性适合于不同的存储和加载方式.此外,要处理好成百上千个资源之间的相互依赖关系也非易事.谁说良好的资源管理不是个艺术呢?:) ------------------ 关键字 AssetBundle资源制作

乐学成语项目的实现

首先给软件取名为HappyIdiom,在开始编码之前,需要先对程序进行需求分析,想一想HappyIdiom中应该具备哪些功能.将这些功能全部整理出来之后,我们才好动手一一实现.这里我认为HappyIdiom中至少应该具备以下功能: 1.成语分类学习:你可以根据自己喜欢的分类(动物类.人物类.季节类.自然类.数字类.语言类.其他类)来进行学习. 2.乐猜成语:学习之余玩玩猜成语游戏,寓教于乐. 3.成语收藏:当用户遇到自己想要保存的成语,点击保存按钮,即保存到收藏页面以方便用户再次查看,对已经掌握

一起做RGB-D SLAM (5)

第五讲 Visual Odometry (视觉里程计) 读者朋友们大家好,又到了我们开讲rgbd slam的时间了.由于前几天博主在忙开会拍婚纱照等一系列乱七八糟的事情,这一讲稍微做的慢了些,先向读者们道个歉! 上几讲中,我们详细讲了两张图像间的匹配与运动估计.然而一个实际的机器人总不可能只有两个图像数据吧?那该多么寂寞呀.所以,本讲开始,我们要处理一个视频流,包含八百左右的数据啦.这才像是在做SLAM嘛! 小萝卜:那我们去哪里下载这些数据呢? 师兄:可以到我的百度云里去:http://yun.

一起做RGB-D SLAM (2)

第二讲 从图像到点云 本讲中,我们将带领读者,编写一个将图像转换为点云的程序.该程序是后期处理地图的基础.最简单的点云地图即是把不同位置的点云进行拼接得到的. 当我们使用RGB-D相机时,会从相机里读到两种数据:彩色图像和深度图像.如果你有Kinect和ros,可以运行: 1 roslaunch openni_launch openni.launch 使Kinect工作.随后,如果PC连接上了Kinect,彩色图像与深度图像就会发布在 /camera/rgb/image_color 和 /cam

一起做RGB-D SLAM 第二季 (一)

小萝卜:师兄!过年啦!是不是很无聊啊!普通人的生活就是赚钱花钱,实在是很没意思啊! 师兄:是啊…… 小萝卜:他们都不懂搞科研和码代码的乐趣呀! 师兄:可不是嘛…… 小萝卜:所以今年过年,我们再做一个SLAM吧!之前写的那个太烂了啦,我都不好意思说是我做的了! 师兄:嗯那可真是对不住你啊…… 小萝卜:没事!你再写一个好一点的,我就原谅你了!写完再请我吃饭吧! 师兄:啊,好的…… 小萝卜:师兄你别这么没精神啊!加油咯! 前言 在经过了一番激烈的思想斗争之后呢,师兄厌倦了年假的无聊生活,开始写<一起做

一起做RGB-D SLAM (6)

第六讲 图优化工具g2o的入门 在上一讲中,我们介绍了如何使用两两匹配,搭建一个视觉里程计.那么,这个里程计有什么不足呢? 一旦出现了错误匹配,整个程序就会跑飞. 误差会累积.常见的现象是:相机转过去的过程能够做对,但转回来之后则出现明显的偏差. 效率方面不尽如人意.在线的点云显示比较费时. 累积误差是里程计中不可避免的,后续的相机姿态依赖着前面的姿态.想要保证地图的准确,必须要保证每次匹配都精确无误,而这是难以实现的.所以,我们希望用更好的方法来做slam.不仅仅考虑两帧的信息,而要把所有整的