一起做RGB-D SLAM (3)
roswiki
roswiki 1733 0
2016-08-10 20:16


第三讲 特征提取与配准


师兄:同学们大家好,又到我们每周一次的“一起做RGB-D SLAM”时间啦!大家还记得上周我们讲了什么东西吗?
小萝卜:等一下师兄!我们的博客什么时候变成每周一更了?
师兄:这个,这不是因为终于到暑假了,我可以专门搞学术了嘛。
小萝卜:胡说!前两天我还看到你在超市开挖掘机来着!
师兄:这事你就别提了啊……
小萝卜:我还有照片为证呢!



 
 
 
 
 
 
 
 
 
师兄:你能不能别提这个事了啊……我们赶紧开始讲课吧。
小萝卜:师兄,那可是儿童专区啊,您可是个博士,自重啊!


上讲回顾


在上一讲中,我们介绍了如何把2D图像坐标转换为3D空间坐标。然后,利用推导的数学公式,实现了把图像转化为点云的程序。在上一讲的末尾,我们给出了一道作业题,希望读者去把这两件事做成一个函数库,以供将来调用。不知道大家回去之后做了没有呢?
小萝卜:读者这么勤奋肯定已经做好了啦!
师兄:嗯,所以呢,这一讲里面我们就要用到上面两个函数了。在讲解本讲的内容之前,先介绍一下我们是如何封装上节课代码的。在 代码根目录/include 下,我们新建了一个 slamBase.h 文件,存放上一讲以及以后讲到的各种函数:
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 文件:
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的部分,和相应的库链接起来。是不是感觉代码更有条理了呢?今后,我们会在每一讲里介绍新的内容,并把实现的代码封装里这个slamBase库中。


图像配准 数学部分


SLAM是由“定位”(Localization)和“建图”(Mapping)两部分构成的。现在来看定位问题。要求解机器人的运动,首先要解决这样一个问题:给定了两个图像,如何知道图像的运动关系呢?
这个问题可以用基于特征的方法(feature-based)或直接的方法(direct method)来解。虽说直接法已经有了一定的发展,但目前主流的方法还是基于特征点的方式。在后者的方法中,首先你需要知道图像里的“特征”,以及这些特征的一一对应关系。假设我们有两个帧:<span tabindex="0" class="MathJax" id="MathJax-Element-1-Frame" role="presentation" style="display:inline-block;position:relative;" data-mathml="F1">F 1  F1 和<span tabindex="0" class="MathJax" id="MathJax-Element-2-Frame" role="presentation" style="display:inline-block;position:relative;" data-mathml="F2">F 2  F2 . 并且,我们获得了两组一一对应的特征点:
<span tabindex="0" class="MathJax" id="MathJax-Element-3-Frame" role="presentation" style="position:relative;" data-mathml="P={p1,p2,&#x2026;,pN}&#x2208;F1">P={p 1 ,p 2 ,,p N }F 1  P={p1,p2,…,pN}∈F1
<span tabindex="0" class="MathJax" id="MathJax-Element-4-Frame" role="presentation" style="position:relative;" data-mathml="Q={q1,q2,&#x2026;,qN}&#x2208;F2">Q={q 1 ,q 2 ,,q N }F 2  Q={q1,q2,…,qN}∈F2
. 其中<span tabindex="0" class="MathJax" id="MathJax-Element-5-Frame" role="presentation" style="display:inline-block;position:relative;" data-mathml="p">p p 和<span tabindex="0" class="MathJax" id="MathJax-Element-6-Frame" role="presentation" style="display:inline-block;position:relative;" data-mathml="q">q q 都是<span tabindex="0" class="MathJax" id="MathJax-Element-7-Frame" role="presentation" style="display:inline-block;position:relative;" data-mathml="R3">R 3  R3 中的点。

我们的目的是求出一个旋转矩阵<span tabindex="0" class="MathJax" id="MathJax-Element-8-Frame" role="presentation" style="display:inline-block;position:relative;" data-mathml="R">R R 和位移矢量<span tabindex="0" class="MathJax" id="MathJax-Element-9-Frame" role="presentation" style="display:inline-block;position:relative;" data-mathml="t">t t ,使得:
<span tabindex="0" class="MathJax" id="MathJax-Element-10-Frame" role="presentation" style="position:relative;" data-mathml="&#x2200;i,pi=Rqi+t">i,p i =Rq i +t ∀i,pi=Rqi+t
.


然而实际当中由于误差的存在,等号基本是不可能的。所以我们通过最小化一个误差来求解<span tabindex="0" class="MathJax" id="MathJax-Element-11-Frame" role="presentation" style="display:inline-block;position:relative;" data-mathml="R,t">R,t R,t :
<span tabindex="0" class="MathJax" id="MathJax-Element-12-Frame" role="presentation" style="position:relative;" data-mathml="minR,t&#x2211;i=1N&#x2225;pi&#x2212;(Rqi+t)&#x2225;2">min R,t i=1 N p i (Rq i +t) 2  minR,t∑i=1N∥pi−(Rqi+t)∥2



这个问题可以用经典的ICP算法求解。其核心是奇异值分解(SVD)。我们将调用OpenCV中的函数求解此问题,所以在此就不细讲ICP的具体步骤了。有兴趣的读者可以参阅1987年PAMI上的一篇文章,也是最原始的ICP方法:Least-squares fitting of two 3-D point sets。
那么从这个数学问题上来讲,我们的关键就是要获取一组一一对应的空间点,这可以通过图像的特征匹配来完成。


图像配准 编程实现


本节中,我们要匹配两对图像,并且计算它们的位移关系。它们分别是rgb1,png, rgb2.png, depth1.png, depth2.png. 人眼可以直观地看到第二个图是向左转动了一些。

 
 
 
 
 
 
 
 
 

 
 
 
 
 
 
 
 
 
 
照例,我们先给出完整的程序代码。读者可以先把代码浏览一遍,我们再加以详细的解释。因为本节的代码比较长,我把它折叠起来,否则排版就太长了。
/*************************************************************************
    &gt; File Name: detectFeatures.cpp
    &gt; Author: xiang gao
    &gt; Mail: gaoxiang12@mails.tsinghua.edu.cn
    &gt; 特征提取与匹配
    &gt; Created Time: 2015年07月18日 星期六 16时00分21秒
 ************************************************************************/
 
#include&lt;iostream&gt;
#include "slamBase.h"
using namespace std;
 
// OpenCV 特征检测模块
#include &lt;opencv2/features2d/features2d.hpp&gt;
#include &lt;opencv2/nonfree/nonfree.hpp&gt;
#include &lt;opencv2/calib3d/calib3d.hpp&gt;
 
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&lt;cv::FeatureDetector&gt; _detector;
    cv::Ptr&lt;cv::DescriptorExtractor&gt; _descriptor;
 
    // 构建提取器,默认两者都为sift
    // 构建sift, surf之前要初始化nonfree模块
    cv::initModule_nonfree();
    _detector = cv::FeatureDetector::create( "GridSIFT" );
    _descriptor = cv::DescriptorExtractor::create( "SIFT" );
 
    vector&lt; cv::KeyPoint &gt; kp1, kp2; //关键点
    _detector-&gt;detect( rgb1, kp1 );  //提取关键点
    _detector-&gt;detect( rgb2, kp2 );
 
    cout&lt;&lt;"Key points of two images: "&lt;&lt;kp1.size()&lt;&lt;", "&lt;&lt;kp2.size()&lt;&lt;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-&gt;compute( rgb1, kp1, desp1 );
    _descriptor-&gt;compute( rgb2, kp2, desp2 );
 
    // 匹配描述子
    vector&lt; cv::DMatch &gt; matches; 
    cv::FlannBasedMatcher matcher;
    matcher.match( desp1, desp2, matches );
    cout&lt;&lt;"Find total "&lt;&lt;matches.size()&lt;&lt;" matches."&lt;&lt;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&lt; cv::DMatch &gt; goodMatches;
    double minDis = 9999;
    for ( size_t i=0; i&lt;matches.size(); i++ )
    {
        if ( matches[i].distance &lt; minDis )
            minDis = matches[i].distance;
    }
 
    for ( size_t i=0; i&lt;matches.size(); i++ )
    {
        if (matches[i].distance &lt; 4*minDis)
            goodMatches.push_back( matches[i] );
    }
 
    // 显示 good matches
    cout&lt;&lt;"good matches="&lt;&lt;goodMatches.size()&lt;&lt;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&lt;cv::Point3f&gt; pts_obj;
    // 第二个帧的图像点
    vector&lt; cv::Point2f &gt; 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&lt;goodMatches.size(); i++)
    {
        // query 是第一个, train 是第二个
        cv::Point2f p = kp1[goodMatches[i].queryIdx].pt;
        // 获取d是要小心!x是向右的,y是向下的,所以y才是行,x是列!
        ushort d = depth1.ptr&lt;ushort&gt;( 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][3] = {
        {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&lt;&lt;"inliers: "&lt;&lt;inliers.rows&lt;&lt;endl;
    cout&lt;&lt;"R="&lt;&lt;rvec&lt;&lt;endl;
    cout&lt;&lt;"t="&lt;&lt;tvec&lt;&lt;endl;
 
    // 画出inliers匹配 
    vector&lt; cv::DMatch &gt; matchesShow;
    for (size_t i=0; i&lt;inliers.rows; i++)
    {
        matchesShow.push_back( goodMatches[inliers.ptr&lt;int&gt;(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;
}
 
detectFeatures.cpp

   代码的解释:

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

cv::Ptr&lt;cv::FeatureDetector&gt; _detector = cv::FeatureDetector::create( "SIFT" );
cv::Ptr&lt;cv::DescriptorExtractor&gt; _descriptor = cv:: DescriptorExtractor::create( "SIFT" );


 然后,使用 _detector->detect()函数提取关键点。值得一提的是,_detector和_descriptor的类型可以由字符串指定。如果你想构建FAST, SURF等特征,只需改后面的字符串即可。
 关键点是一种cv::KeyPoint的类型。你可以看它的API: http://docs.opencv.org/modules/features2d/doc/common_interfaces_of_feature_detectors.html?highlight=keypoint#KeyPoint 由于比较长我这里就不贴了。可以看到,KeyPoint结构中带有 Point2f pt 这个成员变量,指这个关键点的像素坐标。此外,有的关键点还有半径、角度等参数,画在图里就会像一个个的圆一样。

 
 
 
 
 
 
 
 
 
 
 
 
有了一组KeyPoint之后,就可以调用:

_descriptor-&gt;compute( image, keypoint, descriptor )


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

 
第二部分:特征匹配(53行至88行)
接下来,我们对上述的描述子进行匹配。在OpenCV中,你需要选择一个匹配算法,例如粗暴式(bruteforce),近似最近邻(Fast Library for Approximate Nearest Neighbour, FLANN)等等。这里我们构建一个FLANN的匹配算法:

vector&lt; cv::DMatch &gt; matches;
cv::FlannBasedMatcher matcher;
 matcher.match( desp1, desp2, matches );


匹配完成后,算法会返回一些 DMatch 结构。该结构含有以下几个成员:
  1. queryIdx 源特征描述子的索引(也就是第一张图像)。
  2. trainIdx 目标特征描述子的索引(第二个图像)
  3. distance 匹配距离,越大表示匹配越差。

有了匹配后,可以用drawMatch函数画出匹配的结果:

 
 
 
 
 
 
 
 
 
小萝卜:这真是“乱花渐欲迷人眼,浅草才能没马蹄”啊!
师兄:你不是机器人么,怎么开始变得文艺起来了?
不管如何说,仅靠描述子的匹配似乎是太多了些,把许多不相似的东西也匹配起来了。(由于这两个图像只有水平旋转,所以水平的匹配线才是对的,其他的都是误匹配)。因此,需要筛选一下这些匹配,例如,把distance太大的给去掉(源文件69到88行)。
筛选的准则是:去掉大于最小距离四倍的匹配。
小萝卜:为什么是四倍呢?
师兄:这只是个经验数值啦,就像你平时都买一斤半的枣糕呀,为什么不买两斤呢?
小萝卜:我喜欢吃枣糕!等这节讲完我们去吃枣糕吧!
师兄:呃……总、总之,选出来的goodmatch大概是这样的:

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

第三部分 求解PnP
求解PnP的核心是调用OpenCV里的SolvePnPRansac函数。该函数的接口如下(来自OpenCV在线API):C++: void solvePnPRansac(InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess=false, int iterationsCount=100, float reprojectionError=8.0, int minInliersCount=100, OutputArray inliers=noArray(), int flags=ITERATIVE )Python: cv2.solvePnPRansac(objectPoints, imagePoints, cameraMatrix, distCoeffs[, rvec[, tvec[, useExtrinsicGuess[, iterationsCount[, reprojectionError[, minInliersCount[, inliers[, flags]]]]]]]]) → rvec, tvec, inliers
Parameters:
  • objectPoints – Array of object points in the object coordinate space, 3xN/Nx3 1-channel or 1xN/Nx1 3-channel, where N is the number of points.  vector<Point3f> can be also passed here.
  • imagePoints – Array of corresponding image points, 2xN/Nx2 1-channel or 1xN/Nx1 2-channel, where N is the number of points.  vector<Point2f> can be also passed here.
  • cameraMatrix – Input camera matrix   .
  • distCoeffs – Input vector of distortion coefficients   of 4, 5, or 8 elements. If the vector is NULL/empty, the zero distortion coefficients are assumed.
  • rvec – Output rotation vector (see  [url=http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html?highlight=solvepnpransac#void Rodrigues(InputArray src, OutputArray dst, OutputArray jacobian)]Rodrigues()[/url] ) that, together with  tvec , brings points from the model coordinate system to the camera coordinate system.
  • tvec – Output translation vector.
  • useExtrinsicGuess – If true (1), the function uses the provided  rvec and  tvec values as initial approximations of the rotation and translation vectors, respectively, and further optimizes them.
  • iterationsCount – Number of iterations.
  • reprojectionError – Inlier threshold value used by the RANSAC procedure. The parameter value is the maximum allowed distance between the observed and computed point projections to consider it an inlier.
  • minInliersCount – Number of inliers. If the algorithm at some stage finds more inliers than minInliersCount , it finishes.
  • inliers – Output vector that contains indices of inliers in objectPoints and imagePoints .
  • flags – Method for solving a PnP problem (see  [url=http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html?highlight=solvepnpransac#bool solvePnP(InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess, int flags)]solvePnP()[/url] ).

可以看到,这个函数需要输入一组匹配好的三维点: objectPoints 和一组二维图像点: imagePoints. 返回的结果是旋转向量 rvec 和平移向量tvec。其他的都是算法中的参数。因此,我们需要想办法构建这两组输入点,它们实际上就是从goodmatches里抽取来的。
由于我们已经提取了KeyPoint,知道了它们的像素坐标。那么,利用上一节提供的库函数,就可以直接转到三维点啦。代码如下:(因为很重要就又贴了一遍)

// 计算图像间的运动关系
    // 关键函数:cv::solvePnPRansac()
    // 为调用此函数准备必要的参数
     
    // 第一个帧的三维点
    vector&lt;cv::Point3f&gt; pts_obj;
    // 第二个帧的图像点
    vector&lt; cv::Point2f &gt; 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&lt;goodMatches.size(); i++)
    {
        // query 是第一个, train 是第二个
        cv::Point2f p = kp1[goodMatches[i].queryIdx].pt;
        // 获取d是要小心!x是向右的,y是向下的,所以y才是行,x是列!
        ushort d = depth1.ptr&lt;ushort&gt;( 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][3] = {
        {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就含有了位移和旋转的信息,至此,我们已经成功地计算出两个图像的运动啦!
小萝卜:对了,那个Ransac和inlier是什么呢?
师兄:尽管经过了筛选,我们提供的匹配里还是存在误匹配的情况。根据误匹配计算运动是不靠谱的。这时该怎么办呢?OpenCV会利用一种“随机采样一致性”(Random Sample Consensus)的思路(见https://en.wikipedia.org/wiki/RANSAC)。意思即为,在现有的匹配中随机取一部分,估计其运动。因为正确的匹配结果肯定是相似的,而误匹配的结果肯定满天乱飞。只要把收敛的结果取出来即可。
小萝卜:这个就叫做“幸福的家庭都是相似的,不幸的家庭各有各的不幸”吧。
师兄:你这样理解也可以。ransac适用于数据噪声比较大的场合。这对图像的inlier大概是这样的:
小萝卜:似乎仍有一些误差的样子呢。
师兄:是的,不过可以通过调节我们之前的筛选过程以及之后的ransac参数,得到更好的结果。



本节回顾


本节中,我们介绍了如何提取、匹配图像的特征,并通过这些匹配,使用ransac方法估计图像的运动。下一节,我们将介绍如何使用刚得到的平移、旋转向量来拼接点云。至此,我们就完成了一个只有两帧的迷你SLAM程序。
课后作业:
  1. 请把计算图像运动信息封装成一个函数,放进slamBase库中。
  2. 由于我们的程序变复杂了,出现了一些内部的参数,如特征点类型,筛选准则,ransac参数等等。如果我们把这些参数值定义在源代码里,那么每修改一次,就要重新编译一遍程序。这非常麻烦。所以,我们希望把参数定义在外部文件中,在程序刚开始时读取此文件。这样一来,只要更改此文件即可完成参数的调节,不必重新编译程序了。因此,请你完成一个读取参数的类,放进slamBase中。

小萝卜:这一讲比前两节难好多啊,师兄。
师兄:是啊,然而我也不得不讲这些东西,不然讲不清楚呢。
小萝卜:嗯,那我回去好好复习啦。大家也要加油!

未完待续


尊重版权:原文取自半闲居士
分享:
游客
要评论请先登录 或者 注册
返回顶部