一起做RGB-D SLAM (4)

来源：互联网发布：linux解压rar文件编辑：程序博客网时间：2024/06/01 10:38

第四讲点云拼接

上讲回顾

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

　　首先，我们把上一讲的内容封装进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 = 9999;
double good_match_threshold = atof( pd.getData( "good_match_threshold" ).c_str() );
for ( size_t i=0; i<matches.size(); i++ )
{
if ( matches[i].distance < minDis )
minDis = matches[i].distance;
}

for ( size_t i=0; 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=0; 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 == 0)
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[3][3] = {
{camera.fx, 0, camera.cx},
{0, camera.fy, camera.cy},
{0, 0, 1}
};

cout<<"solving pnp"<<endl;
// 构建相机矩阵
cv::Mat cameraMatrix( 3, 3, CV_64F, camera_matrix_data );
cv::Mat rvec, tvec, inliers;
// 求解pnp
cv::solvePnPRansac( pts_obj, pts_img, cameraMatrix, cv::Mat(), rvec, tvec, false, 100, 1.0, 100, 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[0] == '#')            {                // 以‘＃’开头的是注释                continue;            }            int pos = str.find("=");            if (pos == -1)                continue;            string key = str.substr( 0, pos );            string value = str.substr( pos+1, 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=SIFTdescriptor=SIFTgood_match_threshold=4# cameracamera.cx=325.5;camera.cy=253.5;camera.fx=518.0;camera.fy=519.0;camera.scale=1000.0;

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

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

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

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

拼接点云

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

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

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

　　师兄：对！OpenCV认为旋转矩阵R，虽然有3×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", -1);    frame2.rgb = cv::imread( "./data/rgb2.png" );    frame2.depth = cv::imread( "./data/depth2.png", -1 );    // 提取特征并计算描述子    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,3> trans(result.tvec.at<double>(0,0), result.tvec.at<double>(0,1), result.tvec.at<double>(0,2));    T = angle;    T(0,3) = result.tvec.at<double>(0,0);     T(1,3) = result.tvec.at<double>(0,1);     T(2,3) = result.tvec.at<double>(0,2);    // 转换点云    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 0;}

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

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