< >
Home » 一起做RGB-D SLAM » 一起做RGB-D SLAM (5)-Visual Odometry (视觉里程计)

一起做RGB-D SLAM (5)-Visual Odometry (视觉里程计)

一起做RGB-D SLAM (5)-Visual Odometry (视觉里程计)

说明：

  • 上节介绍两张图像间的匹配与运动估计
  • 本节介绍视频流处理,完成一个视觉里程计(visual odometry)的编写

测试数据：

  • 网盘：http://yun.baidu.com/s/1i33uvw5 (400多M)
  • 打开这个数据集，你会看到里头有 和 两个文件夹，分别是RGB图与深度图。
  • 前几讲的数据也是取自这里.
  • 取自nyuv2数据集：http://cs.nyu.edu/~silberman/datasets/nyu_depth_v2.html
  • 这可是一个国际上认可的，相当有名的数据集哦。
  • 如果你想要跑自己的数据，当然也可以，不过需要你进行一些预处理啦

视觉里程计

  • 什么是视觉里程计呢？
  • 简而言之，就是把新来的数据与上一帧进行匹配，估计其运动，然后再把运动累加起来的东西。
  • 画成示意图的话，就是下面这个样子：

请输入图片描述

  • 这实际上和滤波器很像，通过不断的两两匹配，估计机器人当前的位姿，过去的就给丢弃了。
  • 这个思路比较简单，实际当中也比较有效，能够保证局部运动的正确性。
  • 下面我们来实现一下visual odometry

函数封装：

  • 首先工具函数：将cv的旋转矢量与位移矢量转换为变换矩阵，类型为Eigen::Isometry3d
  • 编辑文件：vim src/slamBase.cpp
  • 代码如下：
// cvMat2Eigen
Eigen::Isometry3d cvMat2Eigen( cv::Mat& rvec, cv::Mat& tvec )
{
    cv::Mat R;
    cv::Rodrigues( rvec, R );
    Eigen::Matrix3d r;
    cv::cv2eigen(R, r);
  
    // 将平移向量和旋转矩阵转换成变换矩阵
    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();

    Eigen::AngleAxisd angle(r);
    Eigen::Translation<double,3> trans(tvec.at<double>(0,0), tvec.at<double>(0,1), tvec.at<double>(0,2));
    T = angle;
    T(0,3) = tvec.at<double>(0,0); 
    T(1,3) = tvec.at<double>(0,1); 
    T(2,3) = tvec.at<double>(0,2);
    return T;
}
  • 另一个函数：将新的帧合并到旧的点云里：
// joinPointCloud 
// 输入：原始点云，新来的帧以及它的位姿
// 输出：将新来帧加到原始帧后的图像
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() );
    *newCloud += *output;

    // Voxel grid 滤波降采样
    static pcl::VoxelGrid<PointT> voxel;
    static ParameterReader pd;
    double gridsize = atof( pd.getData("voxel_grid").c_str() );
    voxel.setLeafSize( gridsize, gridsize, gridsize );
    voxel.setInputCloud( newCloud );
    PointCloud::Ptr tmp( new PointCloud() );
    voxel.filter( *tmp );
    return tmp;
}
  • 在parameters.txt中，我们增加了几个参数，以便调节程序的性能：
# part 5 
# 数据相关
# 起始与终止索引
start_index=1
end_index=700
# 数据所在目录
rgb_dir=../data/rgb_png/
rgb_extension=.png
depth_dir=../data/depth_png/
depth_extension=.png
# 点云分辨率
voxel_grid=0.02
# 是否实时可视化
visualize_pointcloud=yes
# 最小匹配数量
min_good_match=10
# 最小内点
min_inliers=5
# 最大运动误差
max_norm=0.3
  • 前面几个参数是相当直观的：指定RGB图与深度图所在的目录，起始与终止的图像索引（也就是第1张到第700张的slam啦）。
  • 后面几个参数，会在后面进行解释。

实现VO

  • 最后，利用之前写好的工具函数，实现一个VO: vim src/visualOdometry.cpp
  • 代码如下：
/*************************************************************************
    > File Name: rgbd-slam-tutorial-gx/part V/src/visualOdometry.cpp
    > Author: xiang gao
    > Mail: gaoxiang12@mails.tsinghua.edu.cn
    > Created Time: 2015年08月01日 星期六 15时35分42秒
 ************************************************************************/

#include <iostream>
#include <fstream>
#include <sstream>
using namespace std;

#include "slamBase.h"

// 给定index，读取一帧数据
FRAME readFrame( int index, ParameterReader& pd );
// 度量运动的大小
double normofTransform( cv::Mat rvec, cv::Mat tvec );

int main( int argc, char** argv )
{
    ParameterReader pd;
    int startIndex  =   atoi( pd.getData( "start_index" ).c_str() );
    int endIndex    =   atoi( pd.getData( "end_index"   ).c_str() );

    // initialize
    cout<<"Initializing ..."<<endl;
    int currIndex = startIndex; // 当前索引为currIndex
    FRAME lastFrame = readFrame( currIndex, pd ); // 上一帧数据
    // 我们总是在比较currFrame和lastFrame
    string detector = pd.getData( "detector" );
    string descriptor = pd.getData( "descriptor" );
    CAMERA_INTRINSIC_PARAMETERS camera = getDefaultCamera();
    computeKeyPointsAndDesp( lastFrame, detector, descriptor );
    PointCloud::Ptr cloud = image2PointCloud( lastFrame.rgb, lastFrame.depth, camera );
    
    pcl::visualization::CloudViewer viewer("viewer");

    // 是否显示点云
    bool visualize = pd.getData("visualize_pointcloud")==string("yes");

    int min_inliers = atoi( pd.getData("min_inliers").c_str() );
    double max_norm = atof( pd.getData("max_norm").c_str() );

    for ( currIndex=startIndex+1; currIndex<endIndex; currIndex++ )
    {
        cout<<"Reading files "<<currIndex<<endl;
        FRAME currFrame = readFrame( currIndex,pd ); // 读取currFrame
        computeKeyPointsAndDesp( currFrame, detector, descriptor );
        // 比较currFrame 和 lastFrame
        RESULT_OF_PNP result = estimateMotion( lastFrame, currFrame, camera );
        if ( result.inliers < min_inliers ) //inliers不够，放弃该帧
            continue;
        // 计算运动范围是否太大
        double norm = normofTransform(result.rvec, result.tvec);
        cout<<"norm = "<<norm<<endl;
        if ( norm >= max_norm )
            continue;
        Eigen::Isometry3d T = cvMat2Eigen( result.rvec, result.tvec );
        cout<<"T="<<T.matrix()<<endl;
        
        //cloud = joinPointCloud( cloud, currFrame, T.inverse(), camera );
        cloud = joinPointCloud( cloud, currFrame, T, camera );
        
        if ( visualize == true )
            viewer.showCloud( cloud );

        lastFrame = currFrame;
    }

    pcl::io::savePCDFile( "data/result.pcd", *cloud );
    return 0;
}

FRAME readFrame( int index, ParameterReader& pd )
{
    FRAME f;
    string rgbDir   =   pd.getData("rgb_dir");
    string depthDir =   pd.getData("depth_dir");
    
    string rgbExt   =   pd.getData("rgb_extension");
    string depthExt =   pd.getData("depth_extension");

    stringstream ss;
    ss<<rgbDir<<index<<rgbExt;
    string filename;
    ss>>filename;
    f.rgb = cv::imread( filename );

    ss.clear();
    filename.clear();
    ss<<depthDir<<index<<depthExt;
    ss>>filename;

    f.depth = cv::imread( filename, -1 );
    return f;
}

double normofTransform( cv::Mat rvec, cv::Mat tvec )
{
    return fabs(min(cv::norm(rvec), 2*M_PI-cv::norm(rvec)))+ fabs(cv::norm(tvec));
}

  • FRAME readFrame( int index, ParameterReader& pd ) 是读取帧数据的函数。告诉它我要读第几帧的数据，它就会乖乖的把数据给找出来，返回一个FRAME结构体。

  • 在得到匹配之后，我们判断了匹配是否成功，并把失败的数据丢弃。为什么这样做呢？

  • 因为之前的算法，对于任意两张图像都能做出一个结果。对于无关的图像，就明显是不对的。所以要去除匹配失败的情形。

  • 如何检测匹配失败呢？我们采用了三个方法：

    • 去掉goodmatch太少的帧，最少的goodmatch定义为：min_good_match=10
    • 去掉solvePnPRASNAC里，inlier较少的帧，同理定义为：min_inliers=5
    • 去掉求出来的变换矩阵太大的情况。因为假设运动是连贯的，两帧之间不会隔的太远：max_norm=0.3

请输入图片描述

  • 经过这三道工序处理后，vo的结果基本能保持正确啦。图示：

请输入图片描述

提示：

  • 当点云出现时，可按5显示颜色，然后按r重置视角，快速查看点云；
  • 可以调节parameters.txt中的voxel_grid值来设置点云分辨率。0.01表示每1cm3
    的格子里有一个点
  • 如果在编译时期出现Link错误，请检查你是否链接到了PCL的visualization模块。如果实在不确定，就照着github源码抄一遍。
  • 在运动时期，由于存在两张图像完全一样的情况，导致匹配时距离为0。由于本节程序的设置，这种情况会被当成没有匹配，导致VO丢失。请你自己fix一下这个bug，我在下一节当中也进行了检查。

参考：

  • 本讲代码：https://github.com/gaoxiang12/rgbd-slam-tutorial-gx/tree/master/part%20V
  • 数据链接：http://yun.baidu.com/s/1i33uvw5

纠错,疑问,交流: 请进入讨论区或 点击加入Q群

获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号


标签: 一起做rgb-d slam