手写RGB-D-SLAM之两帧点云拼接

基于RGB-D的视觉里程计

参考高博：一起做RGBD-SLAM

相关代码：放在本人github，还在调整

自己跟着写了一遍，做了更多的注释

文件结构

外层CmakeLists.txt

#设定存放编译出来的库文件的目录
SET( LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
#并且把该目录设为连接目录
LINK_DIRECTORIES( ${PROJECT_SOURCE_DIR}/lib)

#设定头文件目录
INCLUDE_DIRECTORIES( ${PROJECT_SOURCE_DIR}/include)

#增加子文件夹，也就是进入源代码文件夹继续构建
ADD_SUBDIRECTORY( ${PROJECT_SOURCE_DIR}/src)

单帧图像点云表示 imageToCloud.cpp

// C++ 标准库
#include <iostream>
#include <string>
using namespace std;

// OpenCV 库
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

// PCL 库
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

// 定义点云类型
typedef pcl::PointXYZRGBA PointT;
typedef pcl::PointCloud<PointT> PointCloud;

// 相机内参
const double camera_factor = 1000;
const double camera_cx = 325.5;
const double camera_cy = 253.5;
const double camera_fx = 518.0;
const double camera_fy = 519.0;

// 主函数
int main( int argc, char** argv )
{
    // 读取./data/rgb.png和./data/depth.png，并转化为点云

    // 图像矩阵
    cv::Mat rgb, depth;
    // 使用cv::imread()来读取图像
    
    // rgb 图像是8UC3的彩色图像
    rgb = cv::imread( "./data/rgb.png" );	
    // depth 是16UC1的单通道图像，注意flags设置-1,表示读取原始数据不做任何修改
    depth = cv::imread( "./data/depth.png", -1 );

    
    // 点云变量
    // 使用智能指针，创建一个空点云。这种指针用完会自动释放。
    PointCloud::Ptr cloud ( new PointCloud );
    // 遍历深度图
    for (int m = 0; m < depth.rows; m++)
        for (int n=0; n < depth.cols; n++)
        {
            // 获取深度图中(m,n)处的值
            ushort d = depth.ptr<ushort>(m)[n];
            // d 可能没有值，若如此，跳过此点
            if (d == 0)
                continue;
            // d 存在值，则向点云增加一个点
            PointT p;

            // 计算这个点的空间坐标
            p.z = double(d) / camera_factor;
            p.x = (n - camera_cx) * p.z / camera_fx;
            p.y = (m - camera_cy) * p.z / camera_fy;

            // 从rgb图像中获取它的颜色
            // rgb是三通道的BGR格式图，所以按下面的顺序获取颜色
            p.b = rgb.ptr<uchar>(m)[n*3];
            p.g = rgb.ptr<uchar>(m)[n*3+1];
            p.r = rgb.ptr<uchar>(m)[n*3+2];

            // 把p加入到点云中
            cloud->points.push_back( p );
        }
    // 设置并保存点云
    cloud->height = 1;
    cloud->width = cloud->points.size();
    cout<<"point cloud size = "<<cloud->points.size()<<endl;
    cloud->is_dense = false;
    pcl::io::savePCDFile( "./pointcloud.pcd", *cloud );
    // 清除数据并退出
    cloud->points.clear();
    cout<<"Point cloud saved."<<endl;
    return 0;
}

可以实现功能，但是太混乱了，根据这个函数拓展开，下面的函数封装才是正题

正题：函数封装slamBase.h

CMakeLists.txt

# 增加PCL库的依赖
FIND_PACKAGE( PCL REQUIRED COMPONENTS common io visualization)

# 增加opencv的依赖
FIND_PACKAGE( OpenCV REQUIRED )

# 添加头文件和库文件
ADD_DEFINITIONS( ${PCL_DEFINITIONS} )
INCLUDE_DIRECTORIES( ${PCL_INCLUDE_DIRS}  )
LINK_LIBRARIES( ${PCL_LIBRARY_DIRS} )

#将写好的库文件链接
ADD_LIBRARY( slambase slamBase.cpp )
TARGET_LINK_LIBRARIES( slambase
        ${OpenCV_LIBS}
        ${PCL_LIBRARIES} )

#生成可执行文件
ADD_EXECUTABLE( joinPointCloud joinPointCloud.cpp)
#可执行文件链接相应的库文件
TARGET_LINK_LIBRARIES( joinPointCloud ${OpenCV_LIBS}
        ${PCL_LIBRARIES}
        slambase)

0、参数读取ParameterReader

//定义一个管理配置文件的类
// 参数读取类
class ParameterReader
{
public:

    //构造函数,读取文件的相对路径
    ParameterReader( string filename="./config/parameters.txt" )
    {
        ifstream fin( filename.c_str() );   //读取文件
        if (!fin)
        {
            cerr<<"parameter file does not exist."<<endl;
            return;
        }
        while(!fin.eof())   //end of file？
        {
            string str;
            getline( fin, str );
            if (str[0] == '#')
            {
                // 以‘＃’开头的是注释
                continue;
            }

            int pos = str.find("=");    //find 找到返回位置，其余为false
            if (pos == -1)
                continue;
            string key = str.substr( 0, pos );  //复制子字符串，要求从指定位置开始，并具有指定的长度。如果没有指定长度_Count或_Count+_Off超出了源字符串的长度，则子字符串将延续到源字符串的结尾。
            string value = str.substr( pos+1, str.length() );
            data[key] = value;

            if ( !fin.good() )  //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;
};

1、相机内参数结构体CAMERA_INTRINSIC_PARAMETERS

// 相机内参结构，使用结构体的形式，更好的管理参数类文件，可以经常使用
struct CAMERA_INTRINSIC_PARAMETERS {
    double cx, cy, fx, fy, scale/*缩放尺度*/;
};

2、Frame结构体

每帧图像包含了很多信息，封装为结构体会结构会很清晰

// 帧结构
struct FRAME
{
    int frameID;
    cv::Mat rgb, depth; //该帧对应的彩色图与深度图
    cv::Mat desp;       //特征描述子
    vector<cv::KeyPoint> kp; //关键点
};

3、Pnp返回的结果结构体RESULT_OF_PNP

// PnP 结果
struct RESULT_OF_PNP
{
    cv::Mat rvec/*旋转向量，让点云旋转时使用的时候要转换为Eigen格式*/, tvec/*平移向量*/;
    int inliers;    //内点（就是自己定义的规则觉得比较好的点）数量
};

4、2D点转3D点point2dTo3d

由于需要用到pnp来求解位姿，所以需要一个3D点，将带深度的图像点(u,v,d)经过
相机内参转到相机坐标系下

/**
 * @brief                   将单个点从图像坐标转换为空间坐标
 * @param[in] point         图像点(u,v)+一个相对深度d  ----> (u,v,d)
 * @param[in] camera        参数
 * @return cv::Point3f
 */
cv::Point3f point2dTo3d(cv::Point3f &point, CAMERA_INTRINSIC_PARAMETERS &camera)
{
    //初始化输出3d点的类型，以及为其开辟栈区内存空间
    cv::Point3f _p;

    //相机投影的公式
    //相机投影公式出问题了，少除了个camera.scale
    //_p.z = double(point.z);
    _p.z = double(point.z) / camera.scale;
    _p.x = (point.x - camera.cx) * _p.z / camera.fx;
    _p.y = (point.y - camera.cy) * _p.z / camera.fy;

    return _p;
}

5、图像转点云函数image2PointCloud

/* *
     * @brief image2PointCloud函数，用于2D图片到3D点云图片的转换
     * @param[in]   rgb图片、深度图片、相机内参数
     * @param[out]  点云智能指针，指向点云的一个空间（栈区）
     * @return 0; 成功执行
     * @return -1; 读取图像失败，
     */
PointCloud::Ptr image2PointCloud(cv::Mat &rgb, cv::Mat &depth, CAMERA_INTRINSIC_PARAMETERS &camera)
{

    // 使用智能指针，创建一个空点云。这种指针用完会自动释放。
    PointCloud::Ptr cloud(new PointCloud);
    // 遍历深度图
    for (int m = 0; m < depth.rows; m++)
        for (int n = 0; n < depth.cols; n++) {
            // 获取深度图中(m,n)处的值
            ushort d = depth.ptr<ushort>(m)[n];
            // d 可能没有值，若如此，跳过此点
            if (d == 0)
                continue;
            // d 存在值，则向点云增加一个点
            PointT p;

            // 计算这个点的空间坐标
            p.z = double(d) / camera.scale;
            p.x = (n - camera.cx) * p.z / camera.fx;
            p.y = (m - camera.cy) * p.z / camera.fy;

            // 从rgb图像中获取它的颜色
            // rgb是三通道的BGR格式图，所以按下面的顺序获取颜色
            p.b = rgb.ptr<uchar>(m)[n * 3];
            p.g = rgb.ptr<uchar>(m)[n * 3 + 1];
            p.r = rgb.ptr<uchar>(m)[n * 3 + 2];

            // 把p加入到点云中
            cloud->points.push_back(p);
        }
    // 设置并保存点云
    cloud->height = 1;
    cloud->width = cloud->points.size();
    cout << "point cloud size = " << cloud->points.size() << endl;
    cloud->is_dense = false;
    //pcl::io::savePCDFile("./pointcloud.pcd", *cloud);
    // 清除数据并退出
    //cloud->points.clear();
    //cout << "Point cloud saved." << endl;
    return cloud;
}

6、特征点提取与描述子的计算(ORB)computeKeyPointsAndDesp

/**
 * @brief               计算关键点和描述子
 * @param[in] frame     frame传入时应该具有rgb和depth信息，传出时有kp和descriptor数据
 */
void computeKeyPointsAndDesp(FRAME &frame)
{
    // 构建提取器，默认两者都为 ORB
    cv::Ptr<cv::FeatureDetector> _detector = cv::ORB::create();
    cv::Ptr<cv::DescriptorExtractor> _descriptor = cv::ORB::create();

    _detector->detect(frame.rgb, frame.kp);  //提取图像1关键点
    if (!frame.kp.data())
    {
        cout << "关键点提取失败，没有内容,Err() \n";
        return;
    }

    cout << "Key points of image: " << frame.kp.size() << endl;

    // 计算描述子
    _descriptor->compute(frame.rgb, frame.kp, frame.desp);

}

7、Pnp estimateMotion

/**
 * @brief                   pnp：  3d - 2d 关系求解位姿变换
 * @param[in] frame1        帧1
 * @param[in] frame2        帧2
 * @param[in] camera        相机内参
 * @return RESULT_OF_PNP    pnp的结果，就是一个r 一个t，一定要分清楚，他们的坐标关系！
 */
RESULT_OF_PNP estimateMotion(FRAME &frame1, FRAME &frame2, CAMERA_INTRINSIC_PARAMETERS &camera)
{
    // 求解pnp
    RESULT_OF_PNP _res;
    // 匹配描述子
    vector<cv::DMatch> _matches;
    //用BruteForce-Hamming来计算匹配关系
    cv::Ptr<cv::DescriptorMatcher> _matcher = cv::DescriptorMatcher::create("BruteForce-Hamming");
    //cv::BFMatcher matcher;
    _matcher->match(frame1.desp, frame2.desp, _matches);
    if (_matches.data() == 0) {
        cout << "matches里面没有内容" << endl;
    }
    cout << "Find total " << _matches.size() << " matches." << endl;
    vector<cv::DMatch> _goodMatches;
    double minDis = 9999;   //这个9999哪来的？傻逼，这是随便取的，取大点就可以了
    for (size_t i = 0; i < _matches.size(); i++)
    {
        if (_matches[i].distance < minDis)
        {
            minDis = _matches[i].distance;
        }
    }
    cout << "min dis = " << minDis << endl;

    for (size_t i = 0; i < _matches.size(); i++)
    {
        if (_matches[i].distance < 10 * minDis)
        {
            _goodMatches.push_back(_matches[i]);
        }
    }


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

    //goodmatch较少的处理方式
    if (_goodMatches.size() <= 5)
    {
        _res.inliers = -1;
        return _res;
    }

    // 第一个帧的三维点
    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}
    };

    // 构建相机矩阵
    cv::Mat _cameraMatrix(3, 3, CV_64F, _camera_matrix_data);
    cv::Mat _inliers;

    //brings points from the model coordinate system to the camera coordinate system.
    //把3d点从自身坐标转化到图像坐标系下
    cv::solvePnPRansac(_pts_obj, _pts_img, _cameraMatrix, cv::Mat(), _res.rvec, _res.tvec, -1, 100, 8.0, 0.99, _inliers);
    _res.inliers = _inliers.rows;
    cout << "inliers: " << _res.inliers << endl;
    cout << "R=" << _res.rvec << endl;
    cout << "t=" << _res.tvec << endl;

    return _res;
}

8、新加入的帧转换为点云joinPointCloud

/**
 * @brief               将新加入的帧转换为点云，然后将原始点云旋转到新加入点云的坐标系下，叠加后输出
 * @param[in] original  原始点云
 * @param[in] newFrame  新来的帧
 * @param[in] T         T为上一帧到当前帧的变换
 * @param[in] camera    相机内参
 * @return              将新来帧加到原始帧后的点云图像
 */
PointCloud::Ptr joinPointCloud( PointCloud::Ptr original, FRAME& newFrame, Eigen::Isometry3d T, CAMERA_INTRINSIC_PARAMETERS& camera )
{
    //新加入的帧转换为点云
    PointCloud::Ptr _newCloud = image2PointCloud( newFrame.rgb, newFrame.depth, camera );

    // 合并点云
    PointCloud::Ptr _output (new PointCloud());  //定义一个输出点云
    pcl::transformPointCloud( *original, *_output, T.matrix() ); //将原始点云通过T（T为上一帧到当前帧的变换）转换到新加入的点云坐标系下
    *_newCloud += *_output;   //简单叠加

 	return _newCloud;
}

main函数实现，两帧图像构建点云

#include<iostream>

#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>

using namespace std;

int main( int argc, char** argv )
{
    //本节要拼合data中的两对图像
    myslam::ParameterReader pd; //参数读取器的实例化对象
    // 声明两个帧，FRAME结构请见include/slamBase.h
    myslam::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 );
    //判断读取的图像是否正常
    if(frame1.rgb.data == 0 || frame2.rgb.data == 0
            || frame1.depth.data == 0 || frame2.depth.data == 0)
    {
        cout << "Load pictures errs...." << endl;
        return -1;
    }


    // 提取特征并计算描述子
    cout<<"extracting features"<<endl;
    computeKeyPointsAndDesp(frame1);    //计算frame1描述子，默认的ORB，可以进函数改
    computeKeyPointsAndDesp(frame2);


    // 相机内参
    myslam::CAMERA_INTRINSIC_PARAMETERS camera;
    camera.fx = atof( pd.getData( "camera.fx" ).c_str());   //atof:Convert a string to a floating-point number
    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() );


    // 求解pnp
    //求出来的是第一帧(Frame1)三维点在第一帧的坐标系下，经过怎样的变换(result)到第二帧的坐标系下
    cout<<"solving pnp"<<endl;
    //自己封装的函数：：利用PNP求解相对位姿，输入两帧的结构，相机内参；返回RESULT_OF_PNP结构体
    myslam::RESULT_OF_PNP result = estimateMotion( frame1, frame2, camera );
    cout<<result.rvec<<endl<<result.tvec<<endl;


    // 处理result
    // 将旋转向量转化为旋转矩阵
    cv::Mat R;
    cv::Rodrigues( result.rvec, R );    //罗德里格斯公式，转换为旋转矩阵R
    cout << "罗德里格斯公式转换后的R = \n" << R << endl;
    Eigen::Matrix3d r;
    cv::cv2eigen(R, r);     //转换为Eigen形式


    // 将平移向量和旋转矩阵转换成变换矩阵
    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
    Eigen::AngleAxisd angle(r);     //构造一个旋转向量
    cout << "angle = \n" << angle.matrix() << endl; //angle.matrix() 和 r 相等
    cout<<"translation"<<endl;
    //T = angle;
    T.rotate(angle);    //构造变换矩阵T-----参考《视觉SLAM14讲》p63
    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 << "变换矩阵 T = \n" << T.matrix() << endl;


    // 转换点云
    cout<<"converting image to clouds"<<endl;
    //将rgb和depth转化为点云
    myslam::PointCloud::Ptr cloud1 = image2PointCloud( frame1.rgb, frame1.depth, camera );
  	
	

    // 合并点云
    cout<<"combining clouds"<<endl;
    myslam::PointCloud::Ptr output (new myslam::PointCloud());
    //这里的意思为：T左乘 *cloud1 变换到第二帧坐标系下
    output = myslam::joinPointCloud( cloud1, frame2 , T,  camera );
    
    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;
}

结果

TODO：视觉里程计