一起做RGB-D SLAM (4)

第四讲点云拼接

　　广告：“一起做”系列的代码网址：https://github.com/gaoxiang12/rgbd-slam-tutorial-gx 当博客更新时代码也会随着更新。 SLAM技术交流群：374238181

　　读者朋友们大家好！尽管还没到一周，我们的教程又继续更新了，因为暑假实在太闲了嘛！

上讲回顾

　　上一讲中，我们理解了如何利用图像中的特征点，估计相机的运动。最后，我们得到了一个旋转向量与平移向量。那么读者可能会问：这两个向量有什么用呢？在这一讲里，我们就要使用这两个向量，把两张图像的点云给拼接起来，形成更大的点云。

　　首先，我们把上一讲的内容封装进slamBase库中，代码如下：

　　include/slamBase.h

 // 帧结构

 struct FRAME

 {

     cv::Mat rgb, depth; //该帧对应的彩色图与深度图

     cv::Mat desp;       //特征描述子

     vector<cv::KeyPoint> kp; //关键点

 };

 // PnP 结果

 struct RESULT_OF_PNP

 {

     cv::Mat rvec, tvec;

     int inliers;

 };

 // computeKeyPointsAndDesp 同时提取关键点与特征描述子

 void computeKeyPointsAndDesp( FRAME& frame, string detector, string descriptor );

 // estimateMotion 计算两个帧之间的运动

 // 输入：帧1和帧2, 相机内参

 RESULT_OF_PNP estimateMotion( FRAME& frame1, FRAME& frame2, CAMERA_INTRINSIC_PARAMETERS& camera );

　　我们把关键帧和PnP的结果都封成了结构体，以便将来别的程序调用。这两个函数的实现如下：

　　src/slamBase.cpp

 // computeKeyPointsAndDesp 同时提取关键点与特征描述子

 void computeKeyPointsAndDesp( FRAME& frame, string detector, string descriptor )

 {

     cv::Ptr<cv::FeatureDetector> _detector;

     cv::Ptr<cv::DescriptorExtractor> _descriptor;

     cv::initModule_nonfree();

     _detector = cv::FeatureDetector::create( detector.c_str() );

     _descriptor = cv::DescriptorExtractor::create( descriptor.c_str() );

     if (!_detector || !_descriptor)

     {

         cerr<<"Unknown detector or discriptor type !"<<detector<<","<<descriptor<<endl;

         return;

     }

     _detector->detect( frame.rgb, frame.kp );

     _descriptor->compute( frame.rgb, frame.kp, frame.desp );

     return;

 }

 // estimateMotion 计算两个帧之间的运动

 // 输入：帧1和帧2

 // 输出：rvec 和 tvec

 RESULT_OF_PNP estimateMotion( FRAME& frame1, FRAME& frame2, CAMERA_INTRINSIC_PARAMETERS& camera )

 {

     static ParameterReader pd;

     vector< cv::DMatch > matches;

     cv::FlannBasedMatcher matcher;

     matcher.match( frame1.desp, frame2.desp, matches );

     cout<<"find total "<<matches.size()<<" matches."<<endl;

     vector< cv::DMatch > goodMatches;

     double minDis = ;

     double good_match_threshold = atof( pd.getData( "good_match_threshold" ).c_str() );

     for ( size_t i=; i<matches.size(); i++ )

     {

         if ( matches[i].distance < minDis )

             minDis = matches[i].distance;

     }

     for ( size_t i=; i<matches.size(); i++ )

     {

         if (matches[i].distance < good_match_threshold*minDis)

             goodMatches.push_back( matches[i] );

     }

     cout<<"good matches: "<<goodMatches.size()<<endl;

     // 第一个帧的三维点

     vector<cv::Point3f> pts_obj;

     // 第二个帧的图像点

     vector< cv::Point2f > pts_img;

     // 相机内参

     for (size_t i=; i<goodMatches.size(); i++)

     {

         // query 是第一个, train 是第二个

         cv::Point2f p = frame1.kp[goodMatches[i].queryIdx].pt;

         // 获取d是要小心！x是向右的，y是向下的，所以y才是行，x是列！

         ushort d = frame1.depth.ptr<ushort>( int(p.y) )[ int(p.x) ];

         if (d == )

             continue;

         pts_img.push_back( cv::Point2f( frame2.kp[goodMatches[i].trainIdx].pt ) );

         // 将(u,v,d)转成(x,y,z)

         cv::Point3f pt ( p.x, p.y, d );

         cv::Point3f pd = point2dTo3d( pt, camera );

         pts_obj.push_back( pd );

     }

     double camera_matrix_data[][] = {

         {camera.fx, , camera.cx},

         {, camera.fy, camera.cy},

         {, , }

     };

     cout<<"solving pnp"<<endl;

     // 构建相机矩阵

     cv::Mat cameraMatrix( , , CV_64F, camera_matrix_data );

     cv::Mat rvec, tvec, inliers;

     // 求解pnp

     cv::solvePnPRansac( pts_obj, pts_img, cameraMatrix, cv::Mat(), rvec, tvec, false, , 1.0, , inliers );

     RESULT_OF_PNP result;

     result.rvec = rvec;

     result.tvec = tvec;

     result.inliers = inliers.rows;

     return result;

 }

　　此外，我们还实现了一个简单的参数读取类。这个类读取一个参数的文本文件，能够以关键字的形式提供文本文件中的变量：

　　include/slamBase.h

 // 参数读取类

 class ParameterReader

 {

 public:

     ParameterReader( string filename="./parameters.txt" )

     {

         ifstream fin( filename.c_str() );

         if (!fin)

         {

             cerr<<"parameter file does not exist."<<endl;

             return;

         }

         while(!fin.eof())

         {

             string str;

             getline( fin, str );

             if (str[] == '#')

             {

                 // 以‘＃’开头的是注释

                 continue;

             }

             int pos = str.find("=");

             if (pos == -)

                 continue;

             string key = str.substr( , pos );

             string value = str.substr( pos+, str.length() );

             data[key] = value;

             if ( !fin.good() )

                 break;

         }

     }

     string getData( string key )

     {

         map<string, string>::iterator iter = data.find(key);

         if (iter == data.end())

         {

             cerr<<"Parameter name "<<key<<" not found!"<<endl;

             return string("NOT_FOUND");

         }

         return iter->second;

     }

 public:

     map<string, string> data;

 };

　　它读的参数文件是长这个样子的：

# 这是一个参数文件

# 去你妹的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

 /*************************************************************************

     > File Name: src/jointPointCloud.cpp

     > Author: Xiang gao

     > Mail: gaoxiang12@mails.tsinghua.edu.cn

     > Created Time: 2015年07月22日 星期三 20时46分08秒

  ************************************************************************/

 #include<iostream>

 using namespace std;

 #include "slamBase.h"

 #include <opencv2/core/eigen.hpp>

 #include <pcl/common/transforms.h>

 #include <pcl/visualization/cloud_viewer.h>

 // Eigen !

 #include <Eigen/Core>

 #include <Eigen/Geometry>

 int main( int argc, char** argv )

 {

     //本节要拼合data中的两对图像

     ParameterReader pd;

     // 声明两个帧，FRAME结构请见include/slamBase.h

     FRAME frame1, frame2;

     //读取图像

     frame1.rgb = cv::imread( "./data/rgb1.png" );

     frame1.depth = cv::imread( "./data/depth1.png", -);

     frame2.rgb = cv::imread( "./data/rgb2.png" );

     frame2.depth = cv::imread( "./data/depth2.png", - );

     // 提取特征并计算描述子

     cout<<"extracting features"<<endl;

     string detecter = pd.getData( "detector" );

     string descriptor = pd.getData( "descriptor" );

     computeKeyPointsAndDesp( frame1, detecter, descriptor );

     computeKeyPointsAndDesp( frame2, detecter, descriptor );

     // 相机内参

     CAMERA_INTRINSIC_PARAMETERS camera;

     camera.fx = atof( pd.getData( "camera.fx" ).c_str());

     camera.fy = atof( pd.getData( "camera.fy" ).c_str());

     camera.cx = atof( pd.getData( "camera.cx" ).c_str());

     camera.cy = atof( pd.getData( "camera.cy" ).c_str());

     camera.scale = atof( pd.getData( "camera.scale" ).c_str() );

     cout<<"solving pnp"<<endl;

     // 求解pnp

     RESULT_OF_PNP result = estimateMotion( frame1, frame2, camera );

     cout<<result.rvec<<endl<<result.tvec<<endl;

     // 处理result

     // 将旋转向量转化为旋转矩阵

     cv::Mat R;

     cv::Rodrigues( result.rvec, R );

     Eigen::Matrix3d r;

     cv::cv2eigen(R, r);

     // 将平移向量和旋转矩阵转换成变换矩阵

     Eigen::Isometry3d T = Eigen::Isometry3d::Identity();

     Eigen::AngleAxisd angle(r);

     cout<<"translation"<<endl;

     Eigen::Translation<double,> trans(result.tvec.at<double>(,), result.tvec.at<double>(,), result.tvec.at<double>(,));

     T = angle;

     T(,) = result.tvec.at<double>(,);

     T(,) = result.tvec.at<double>(,);

     T(,) = result.tvec.at<double>(,);

     // 转换点云

     cout<<"converting image to clouds"<<endl;

     PointCloud::Ptr cloud1 = image2PointCloud( frame1.rgb, frame1.depth, camera );

     PointCloud::Ptr cloud2 = image2PointCloud( frame2.rgb, frame2.depth, camera );

     // 合并点云

     cout<<"combining clouds"<<endl;

     PointCloud::Ptr output (new PointCloud());

     pcl::transformPointCloud( *cloud1, *output, T.matrix() );

     *output += *cloud2;

     pcl::io::savePCDFile("data/result.pcd", *output);

     cout<<"Final result saved."<<endl;

     pcl::visualization::CloudViewer viewer( "viewer" );

     viewer.showCloud( output );

     while( !viewer.wasStopped() )

     {

     }

     return ;

 }

　　重点在于59至73行，讲述了这个转换的过程。

　　变换完毕后，我们就得到了拼合的点云啦：

　　怎么样？是不是有点成就感了呢？

接下来的事……

　　至此，我们已经实现了一个只有两帧的SLAM程序。然而，也许你还不知道，这已经是一个视觉里程计(Visual Odometry)啦！只要不断地把进来的数据与上一帧对比，就可以得到完整的运动轨迹以及地图了呢！

　　小萝卜：这听着已经像是SLAM了呀！

　　师兄：嗯，要做完整的SLAM，还需要一些东西。以两两匹配为基础的里程计有明显的累积误差，我们需要通过回环检测来消除它。这也是我们后面几讲的主要内容啦！

　　小萝卜：那下一讲我们要做点什么呢？

　　师兄：我们先讲讲关键帧的处理，因为把每个图像都放进地图，会导致地图规模增长地太快，所以需要关键帧技术。然后呢，我们要做一个SLAM后端，就要用到g2o啦！

课后作业

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

未完待续

　　如果你觉得我的博客有帮助，可以进行几块钱的小额赞助，帮助我把博客写得更好。

巴特西

一起做RGB-D SLAM (4)

第四讲点云拼接

上讲回顾

拼接点云

接下来的事……

课后作业

最新文章

热门文章

巴特西

一起做RGB-D SLAM (4)

第四讲 点云拼接

上讲回顾

拼接点云

接下来的事……

课后作业

最新文章

热门文章

第四讲点云拼接