一起做RGB-D SLAM (3)-特征提取与配准

一起做RGB-D SLAM (3)-特征提取与配准

说明：

• 介绍2D图像坐标转换为3D空间坐标
• 介绍如何封装基本函数
• 介绍如何特征提取与配准

封装基本函数

• 在 代码根目录/include 下，我们新建了一个 slamBase.h 文件

touch include/slamBase.h
vim include/slamBase.h

• 内容如下：

/*************************************************************************
    > File Name: rgbd-slam-tutorial-gx/part III/code/include/slamBase.h
    > Author: xiang gao
    > Mail: gaoxiang12@mails.tsinghua.edu.cn
    > Created Time: 2015年07月18日 星期六 15时14分22秒
    > 说明：rgbd-slam教程所用到的基本函数（C风格）
 ************************************************************************/
# pragma once

// 各种头文件 
// C++标准库
#include <fstream>
#include <vector>
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;

// 相机内参结构
struct CAMERA_INTRINSIC_PARAMETERS 
{ 
    double cx, cy, fx, fy, scale;
};

// 函数接口
// image2PonitCloud 将rgb图转换为点云
PointCloud::Ptr image2PointCloud( cv::Mat& rgb, cv::Mat& depth, CAMERA_INTRINSIC_PARAMETERS& camera );

// point2dTo3d 将单个点从图像坐标转换为空间坐标
// input: 3维点Point3f (u,v,d)
cv::Point3f point2dTo3d( cv::Point3f& point, CAMERA_INTRINSIC_PARAMETERS& camera );

• 我们把相机参数封装成了一个结构体，另外还声明了 image2PointCloud 和 point2dTo3d 两个函数
• 在 src/ 目录下新建 slamBase.cpp 文件

touch src/slamBase.cpp
vim src/slamBase.cpp

• 内容如下：

/*************************************************************************
    > File Name: src/slamBase.cpp
    > Author: xiang gao
    > Mail: gaoxiang12@mails.tsinghua.edu.cn
    > Implementation of slamBase.h
    > Created Time: 2015年07月18日 星期六 15时31分49秒
 ************************************************************************/

#include "slamBase.h"

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();
    cloud->is_dense = false;

    return cloud;
}

cv::Point3f point2dTo3d( cv::Point3f& point, CAMERA_INTRINSIC_PARAMETERS& camera )
{
    cv::Point3f p; // 3D 点
    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;
}

• 最后，在 src/CMakeLists.txt 中加入以下几行，将 slamBase.cpp 编译成一个库，供将来调用：

ADD_LIBRARY( slambase slamBase.cpp )
TARGET_LINK_LIBRARIES( slambase
    ${OpenCV_LIBS} 
    ${PCL_LIBRARIES} )

• 把slamBase.cpp编译成 slamBase 库，并把该库里调到的OpenCV和PCL的部分，和相应的库链接起来

图像配准 数学部分

• SLAM是由“定位”（Localization)和“建图”（Mapping)两部分构成的。
• 现在来看定位问题。
• 要求解机器人的运动，首先要解决这样一个问题：给定了两个图像，如何知道图像的运动关系呢？
• 这个问题可以用基于特征的方法（feature-based）或直接的方法（direct method）来解。虽说直接法已经有了一定的发展，但目前主流的方法还是基于特征点的方式。在后者的方法中，首先你需要知道图像里的“特征”，以及这些特征的一一对应关系。
• 
• 提示：由于OpenCV中没有提供ICP，我们在实现中使用PnP进行求解。

图像配准 编程实现

• 我们要匹配两对图像，并且计算它们的位移关系。它们分别是rgb1,png, rgb2.png, depth1.png, depth2.png.

• 

• 

• 人眼可以直观地看到第二个图是向左转动了一些

• 新建detectFeatures.cpp文件，实现特征提取

touch src/detectFeatures.cpp
vim src/detectFeatures.cpp

• 实现代码：

/*************************************************************************
    > File Name: detectFeatures.cpp
    > Author: xiang gao
    > Mail: gaoxiang12@mails.tsinghua.edu.cn
    > 特征提取与匹配
    > Created Time: 2015年07月18日 星期六 16时00分21秒
 ************************************************************************/

#include<iostream>
#include "slamBase.h"
using namespace std;

// OpenCV 特征检测模块
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/nonfree/nonfree.hpp>
#include <opencv2/calib3d/calib3d.hpp>

int main( int argc, char** argv )
{
    // 声明并从data文件夹里读取两个rgb与深度图
    cv::Mat rgb1 = cv::imread( "./data/rgb1.png");
    cv::Mat rgb2 = cv::imread( "./data/rgb2.png");
    cv::Mat depth1 = cv::imread( "./data/depth1.png", -1);
    cv::Mat depth2 = cv::imread( "./data/depth2.png", -1);

    // 声明特征提取器与描述子提取器
    cv::Ptr<cv::FeatureDetector> _detector;
    cv::Ptr<cv::DescriptorExtractor> _descriptor;

    // 构建提取器，默认两者都为sift
    // 构建sift, surf之前要初始化nonfree模块
    cv::initModule_nonfree();
    _detector = cv::FeatureDetector::create( "GridSIFT" );
    _descriptor = cv::DescriptorExtractor::create( "SIFT" );

    vector< cv::KeyPoint > kp1, kp2; //关键点
    _detector->detect( rgb1, kp1 );  //提取关键点
    _detector->detect( rgb2, kp2 );

    cout<<"Key points of two images: "<<kp1.size()<<", "<<kp2.size()<<endl;
    
    // 可视化， 显示关键点
    cv::Mat imgShow;
    cv::drawKeypoints( rgb1, kp1, imgShow, cv::Scalar::all(-1), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS );
    cv::imshow( "keypoints", imgShow );
    cv::imwrite( "./data/keypoints.png", imgShow );
    cv::waitKey(0); //暂停等待一个按键
   
    // 计算描述子
    cv::Mat desp1, desp2;
    _descriptor->compute( rgb1, kp1, desp1 );
    _descriptor->compute( rgb2, kp2, desp2 );

    // 匹配描述子
    vector< cv::DMatch > matches; 
    cv::FlannBasedMatcher matcher;
    matcher.match( desp1, desp2, matches );
    cout<<"Find total "<<matches.size()<<" matches."<<endl;

    // 可视化：显示匹配的特征
    cv::Mat imgMatches;
    cv::drawMatches( rgb1, kp1, rgb2, kp2, matches, imgMatches );
    cv::imshow( "matches", imgMatches );
    cv::imwrite( "./data/matches.png", imgMatches );
    cv::waitKey( 0 );

    // 筛选匹配，把距离太大的去掉
    // 这里使用的准则是去掉大于四倍最小距离的匹配
    vector< cv::DMatch > goodMatches;
    double minDis = 9999;
    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 < 4*minDis)
            goodMatches.push_back( matches[i] );
    }

    // 显示 good matches
    cout<<"good matches="<<goodMatches.size()<<endl;
    cv::drawMatches( rgb1, kp1, rgb2, kp2, goodMatches, imgMatches );
    cv::imshow( "good matches", imgMatches );
    cv::imwrite( "./data/good_matches.png", imgMatches );
    cv::waitKey(0);

    // 计算图像间的运动关系
    // 关键函数：cv::solvePnPRansac()
    // 为调用此函数准备必要的参数
    
    // 第一个帧的三维点
    vector<cv::Point3f> pts_obj;
    // 第二个帧的图像点
    vector< cv::Point2f > pts_img;

    // 相机内参
    CAMERA_INTRINSIC_PARAMETERS C;
    C.cx = 325.5;
    C.cy = 253.5;
    C.fx = 518.0;
    C.fy = 519.0;
    C.scale = 1000.0;

    for (size_t i=0; i<goodMatches.size(); i++)
    {
        // query 是第一个, train 是第二个
        cv::Point2f p = kp1[goodMatches[i].queryIdx].pt;
        // 获取d是要小心！x是向右的，y是向下的，所以y才是行，x是列！
        ushort d = depth1.ptr<ushort>( int(p.y) )[ int(p.x) ];
        if (d == 0)
            continue;
        pts_img.push_back( cv::Point2f( kp2[goodMatches[i].trainIdx].pt ) );

        // 将(u,v,d)转成(x,y,z)
        cv::Point3f pt ( p.x, p.y, d );
        cv::Point3f pd = point2dTo3d( pt, C );
        pts_obj.push_back( pd );
    }

    double camera_matrix_data[3][4] = {
        {C.fx, 0, C.cx},
        {0, C.fy, C.cy},
        {0, 0, 1}
    };

    // 构建相机矩阵
    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 );

    cout<<"inliers: "<<inliers.rows<<endl;
    cout<<"R="<<rvec<<endl;
    cout<<"t="<<tvec<<endl;

    // 画出inliers匹配 
    vector< cv::DMatch > matchesShow;
    for (size_t i=0; i<inliers.rows; i++)
    {
        matchesShow.push_back( goodMatches[inliers.ptr<int>(i)[0]] );    
    }
    cv::drawMatches( rgb1, kp1, rgb2, kp2, matchesShow, imgMatches );
    cv::imshow( "inlier matches", imgMatches );
    cv::imwrite( "./data/inliers.png", imgMatches );
    cv::waitKey( 0 );

    return 0;
}

代码的解释：

第一部分： 18行至52行，特征的检测与描述子的计算。

• 要在一个图像里提取特征，第一步是计算“关键点”，然后，针对这些关键点周围的像素，计算其“描述子”。
• 在OpenCV中，分别由cv::FeatureDetector和cv::DescriptorExtractor来计算。

cv::Ptr<cv::FeatureDetector> _detector = cv::FeatureDetector::create( "SIFT" );
cv::Ptr<cv::DescriptorExtractor> _descriptor = cv:: DescriptorExtractor::create( "SIFT" );

• 然后，使用 _detector->detect()函数提取关键点。

• 值得一提的是，_detector和_descriptor的类型可以由字符串指定。

• 如果你想构建FAST, SURF等特征，只需改后面的字符串即可。

• 关键点是一种cv::KeyPoint的类型。

• 你可以看它的API,可以看到，KeyPoint结构中带有 Point2f pt 这个成员变量，指这个关键点的像素坐标。

• 此外，有的关键点还有半径、角度等参数，画在图里就会像一个个的圆一样。
  

• 有了一组KeyPoint之后，就可以调用：

_descriptor->compute( image, keypoint, descriptor )

• 在 keypoint 上计算描述子。
• 描述子是一个cv::Mat的矩阵结构，它的每一行代表一个对应于Keypoint的特征向量。
• 当两个keypoint的描述子越相似，说明这两个关键点也就越相似。
• 我们正是通过这种相似性来检测图像之间的运动的。

第二部分：特征匹配（53行至88行）

• 接下来，我们对上述的描述子进行匹配。
• 在OpenCV中，你需要选择一个匹配算法，例如粗暴式（bruteforce），近似最近邻（Fast Library for Approximate Nearest Neighbour, FLANN）等等。
• 这里我们构建一个FLANN的匹配算法：

vector< cv::DMatch > matches;
cv::FlannBasedMatcher matcher;
matcher.match( desp1, desp2, matches );

• 匹配完成后，算法会返回一些 DMatch 结构。

• 该结构含有以下几个成员：

  • queryIdx 源特征描述子的索引（也就是第一张图像）。
  • trainIdx 目标特征描述子的索引（第二个图像）
  • distance 匹配距离，越大表示匹配越差。

• 有了匹配后，可以用drawMatch函数画出匹配的结果：
  

• 仅靠描述子的匹配似乎是太多了些，把许多不相似的东西也匹配起来了。(由于这两个图像只有水平旋转，所以水平的匹配线才是对的，其他的都是误匹配）。

• 因此，需要筛选一下这些匹配，例如，把distance太大的给去掉（源文件69到88行）。

• 筛选的准则是：去掉大于最小距离四倍的匹配。如图：
  

• 筛选之后，匹配就少了许多了，图像看起来也比较干净。

第三部分 求解PnP

• 求解PnP的核心是调用OpenCV里的SolvePnPRansac函数。该函数的接口如下（来自OpenCV在线API）：
  
  

• 这个函数需要输入一组匹配好的三维点: objectPoints 和一组二维图像点: imagePoints. 返回的结果是旋转向量 rvec 和平移向量tvec。其他的都是算法中的参数。因此，我们需要想办法构建这两组输入点，它们实际上就是从goodmatches里抽取来的。

• 由于我们已经提取了KeyPoint，知道了它们的像素坐标。那么，利用上一节提供的库函数，就可以直接转到三维点啦。代码如下：（因为很重要就又贴了一遍）

// 计算图像间的运动关系
    // 关键函数：cv::solvePnPRansac()
    // 为调用此函数准备必要的参数
    
    // 第一个帧的三维点
    vector<cv::Point3f> pts_obj;
    // 第二个帧的图像点
    vector< cv::Point2f > pts_img;

    // 相机内参
    CAMERA_INTRINSIC_PARAMETERS C;
    C.cx = 325.5;
    C.cy = 253.5;
    C.fx = 518.0;
    C.fy = 519.0;
    C.scale = 1000.0;

    for (size_t i=0; i<goodMatches.size(); i++)
    {
        // query 是第一个, train 是第二个
        cv::Point2f p = kp1[goodMatches[i].queryIdx].pt;
        // 获取d是要小心！x是向右的，y是向下的，所以y才是行，x是列！
        ushort d = depth1.ptr<ushort>( int(p.y) )[ int(p.x) ];
        if (d == 0)
            continue;
        pts_img.push_back( cv::Point2f( kp2[goodMatches[i].trainIdx].pt ) );

        // 将(u,v,d)转成(x,y,z)
        cv::Point3f pt ( p.x, p.y, d );
        cv::Point3f pd = point2dTo3d( pt, C );
        pts_obj.push_back( pd );
    }

    double camera_matrix_data[3][11] = {
        {C.fx, 0, C.cx},
        {0, C.fy, C.cy},
        {0, 0, 1}
    };

    // 构建相机矩阵
    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 );

• 求解完成后，rvec和tvec就含有了位移和旋转的信息，至此，我们已经成功地计算出两个图像的运动啦！
  

• 经过了筛选，我们提供的匹配里还是存在误匹配的情况。

• 根据误匹配计算运动是不靠谱的。这时该怎么办呢？

• OpenCV会利用一种“随机采样一致性”（Random Sample Consensus）的思路（见参考）。意思即为，在现有的匹配中随机取一部分，估计其运动。因为正确的匹配结果肯定是相似的，而误匹配的结果肯定满天乱飞。只要把收敛的结果取出来即可。

• ransac适用于数据噪声比较大的场合。这对图像的inlier大概是这样的
  

• 仍有一些误差,不过可以通过调节我们之前的筛选过程以及之后的ransac参数，得到更好的结果

课后作业：

• 请把计算图像运动信息封装成一个函数，放进slamBase库中。
• 由于我们的程序变复杂了，出现了一些内部的参数，如特征点类型，筛选准则，ransac参数等等。
• 如果我们把这些参数值定义在源代码里，那么每修改一次，就要重新编译一遍程序。这非常麻烦。
• 所以，我们希望把参数定义在外部文件中，在程序刚开始时读取此文件。
• 这样一来，只要更改此文件即可完成参数的调节，不必重新编译程序了。
• 因此，请你完成一个读取参数的类，放进slamBase中。