RGB-D SLAM学习总结(3)

最新推荐文章于 2022-04-13 10:12:09 发布

niuniu924

最新推荐文章于 2022-04-13 10:12:09 发布

阅读量476

点赞数

分类专栏： SLAM 文章标签： RGBD-SLAM

本文链接：https://blog.csdn.net/niuniu924/article/details/82822395

版权

SLAM 专栏收录该内容

7 篇文章 3 订阅

订阅专栏

第三讲特征提取和匹配

本讲主要实现图像的特征提取，图像间的匹配，以及相机转换矩阵的求解。

高博的博客中提供了两帧图像做测试，就是这两帧图像。。。千万不要另存为。。。

由于具体代码已经有详细介绍，这里只往slamBase里添加方法。

另外在使用的slambase头文件里的方法时，一定也要像链接库的头文件一样将其链接到编译的可执行文件中，就像这样：

ADD_EXECUTABLE(detect_features detectFeatures.cpp)
TARGET_LINK_LIBRARIES(detect_features slambase ${OpenCV_LIBS} ${PCL_LIBRARIES})

首先在slamBase.h中添加：

#include <opencv2/features2d/features2d.hpp>
#include <opencv2/nonfree/nonfree.hpp>
#include <opencv2/calib3d/calib3d.hpp>

// 帧结构
struct FRAME
{
    int frameID;
    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 );

特别地，我们还添加了一个参数读取类（非常值得学习的方法~），用来读取一些参数：

// 参数读取类
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;
};

它读取的就是根目录下的parameters.txt里定义的参数：

# 这是一个参数文件

# part 4 里定义的参数

detector=SIFT
descriptor=SIFT
# 定义匹配距离的阈值，小于4倍最小距离则认为是好的匹配
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;

那么，相机内参也可以通过读取此文件得到，我们继续向slamBase.h中添加方法（一个内联函数）去获得相机内参：

inline static CAMERA_INTRINSIC_PARAMETERS getDefaultCamera()
{
	ParameterReader pd;
	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() );
	return camera;
}

这样以后需要相机内参时直接调用getDefaultCamera()即可得到。

相应地，在slamBase.cpp中添加：

// computeKeyPointsAndDesp 同时提取关键点与特征描述子
void computeKeyPointsAndDesp( FRAME& frame, string detector, string descriptor )
{
    cv::Ptr<cv::FeatureDetector> _detector;
    cv::Ptr<cv::DescriptorExtractor> _descriptor;
    // 在使用SIFT/SURF特征时要注册算法，不然create会返回空指针
    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 )
{
    RESULT_OF_PNP result;
    static ParameterReader pd;
    vector< cv::DMatch > matches;
    //cv::FlannBasedMatcher matcher;
    cv::BFMatcher 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;
    }
    //cout << "min dis = " << minDis << endl;
    if(minDis<10)minDis=10;

    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;
    if (goodMatches.size() <= 5) 
    {
        result.inliers = -1;
        return result;
    }
    // 第一个帧的三维点
    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 );
    }

    if(pts_obj.size()==0 || pts_img.size()==0)
    {
	result.inliers = -1;
	return result;
    }

    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.rvec = rvec;
    result.tvec = tvec;
    result.inliers = inliers.rows;

    return result;
}

特别注意的是：

添加了这两段代码确保solvePnPRansac不报错(不能传入空的3D点和图像点)以及帧的可用性，实际上对于新来的帧，如果与参考帧匹配过少，那就表示这一帧是错误的(拍得不好)，不能添加进关键帧序列中。

if (goodMatches.size() <= 5) 
{
    result.inliers = -1;
    return result;
}

if(pts_obj.size()==0 || pts_img.size()==0)
{
    result.inliers = -1;
    return result;
}

这样特征提取和匹配就完成啦~同时我们也获得了相机的运动信息。

solvePnPRansac方法补充说明：

Ransac介绍：

转自：http://blog.csdn.net/u010128736/

随机抽样一致算法（RANdom SAmple Consensus,RANSAC）,采用迭代的方式从一组包含离群的被观测数据中估算出数学模型的参数。维基百科中的RANSAC该算法最早由Fischler和Bolles于1981年提出。RANSAC算法假设数据中包含正确数据和异常数据（或称为噪声）。正确数据记为内点（inliers），异常数据记为外点（outliers）。同时RANSAC也假设，给定一组正确的数据，存在可以计算出符合这些数据的模型参数的方法。该算法核心思想就是随机性和假设性，随机性是根据正确数据出现概率去随机选取抽样数据，根据大数定律，随机性模拟可以近似得到正确结果。假设性是假设选取出的抽样数据都是正确数据，然后用这些正确数据通过问题满足的模型，去计算其他点，然后对这次结果进行一个评分。

通过实例对算法基本思想进行描述：

（1）首先，我们知道要得到一个直线模型，我们需要两个点唯一确定一个直线方程。所以第一步我们随机选择两个点。

（2）通过这两个点，我们可以计算出这两个点所表示的模型方程y=ax+b。

（3）我们将所有的数据点套到这个模型中计算误差。

（4）我们找到所有满足误差阈值的点，第4幅图中可以看到，只有很少的点支持这个模型。

（5）然后我们再重复（1）~（4）这个过程，直到达到一定迭代次数后，我们选出那个被支持的最多的模型，作为问题的解。

PnP方法介绍：

参考：《视觉SLAM十四讲》7.7

PnP(Perspective-n-Point)是求解3D到2D点对运动的方法。它描述了当知道n个3D空间点及其投影位置时，如何估计相机的位姿。PnP有很多种求解方法，例如，用3对点估计位姿的P3P、直接线性变换(DLT)、EPnP(Efficient PnP)、UPnP，等等。此外，还能用非线性优化的方法，构建最小二乘问题并迭代求解，也就是万金油式的Bundle Adjustment。

solvePnPRansac函数介绍：

参考：opencv在线API

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 )

Parameters
objectPoints	3D空间点。
imagePoints	3D空间点的投影位置。
cameraMatrix	相机内参。
distCoeffs	相机畸变系数，4,5,8个系数构成的矩阵或为零失真矩阵。
rvec	返回旋转量。
tvec	返回平移量。
useExtrinsicGuess	如果为真(1)，则函数使用提供的rvec和tvec值分别作为旋转和平移向量的初始近似值，并进一步优化它们。 emmmm...没懂这个值。
iterationsCount	迭代次数。
reprojectionError	RANSAC过程中使用的内阈值。参数值是观测点和计算点投影之间的最大允许距离，将其视为内点。
minInliersCount	内点数目。如果在某个阶段的算法找到比这个值更多的内点，它就结束了。
inliers	输出向量，包含对象点和图像点中的内点索引。
flags	使用的PnP方法，见下表。

flags
CV_ITERATIVE	迭代法是基于Levenberg Marquardt优化的。在这种情况下，该函数找到这样的姿态，该姿态使重新投影误差最小化，即所观察的图像点与3D投影点之间的平方距离之和（BA — 最小二乘）。
CV_P3P	方法基于X.S. Gao, X.-R. Hou, J. Tang, H.-F. Chang “Complete Solution Classification for the Perspective-Three-Point Problem”。在这种情况下，函数需要精确的四组对象和图像点。
CV_EPNP	F.Moreno-Noguer, V.Lepetit and P.Fua in the paper “EPnP: Efficient Perspective-n-Point Camera Pose Estimation”一文中介绍了此方法。

niuniu924

关注

0
点赞
踩
1

收藏

觉得还不错? 一键收藏
0
评论
RGB-D SLAM学习总结(3)

第三讲特征提取和匹配本讲主要实现图像的特征提取，图像间的匹配，以及相机转换矩阵的求解。高博的博客中提供了两帧图像做测试，就是这两帧图像。。。千万不要另存为。。。由于具体代码已经有详细介绍，这里只往slamBase里添加方法。另外在使用的slambase头文件里的方法时，一定也要像链接库的头文件一样将其链接到编译的可执行文件中，就像这样：ADD_EXECUTABLE(dete...
复制链接

扫一扫