一个VO(Visual Odometry)的简单实现

客官°小女子只卖身不卖艺 2022-12-11 15:16 67阅读 0赞

# 简单介绍 #

*  输入：指定数量的RGB+Depth深度图，相关参数配置文件.txt
 *  输出：实时显示的三维模型，和最终的.pcd点云图

# 主函数 #

/*************************************************************************
    	> 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秒
        > Update: add comments by john 2020-10-16
     ************************************************************************/
    
    #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
        //1. 读取当前帧的rgb+depth图像
        FRAME lastFrame = readFrame( currIndex, pd ); // 上一帧数据
        // 我们总是在比较currFrame和lastFrame
        string detector = pd.getData( "detector" );
        string descriptor = pd.getData( "descriptor" );
        //2. 获取相机内参： Tracking::mpCamera
        CAMERA_INTRINSIC_PARAMETERS camera = getDefaultCamera();
        //计算出lastFrame的关键点和描述子，存在当前帧结构中
        computeKeyPointsAndDesp( lastFrame, detector, descriptor );
        //3. 根据rgb+depth+相机内参，获取最后一帧相机坐标系下的点云。 JOHNTODO 可以使用
        PointCloud::Ptr cloud = image2PointCloud( lastFrame.rgb, lastFrame.depth, camera );
        
        //4. 定义点云视图viewer
        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() );
    
        //5.遍历每一帧
        for ( currIndex=startIndex+1; currIndex<endIndex; currIndex++ )
        {
            cout<<"Reading files "<<currIndex<<endl;
            FRAME currFrame = readFrame( currIndex,pd ); // 读取currFrame
            computeKeyPointsAndDesp( currFrame, detector, descriptor );
            
            // 比较currFrame 和 lastFrame
            // JOHNCOMMENT: 获取当前帧currFrame关于上一帧的外参,此处用到图片的 descriptor
            // 注意: 此处的相机外参 result 中的R,t 是当前帧相对于上一帧的旋转和平移
            // 相机内参可以用于: "像素点坐标"到"相机坐标系下的空间点坐标"的转换
            // 相机外参可以用于: "相机坐标系下的空间点坐标"到"世界坐标系下的对应空间点坐标"的转换
            //6. 获取相机关于最后一帧的运动
            RESULT_OF_PNP result = estimateMotion( lastFrame, currFrame, camera );
            
            //7. 过滤掉质量差的帧
            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;
            
            //8. 添加当前帧点云到历史点云中
            // JOHNCOMMENT: 将当前帧的点云加入到上一帧(或者说之前的历史)点云中，
            // 更具体地说: 将历史点云使用当前帧的位姿进行变换, 和当前点云对齐,然后加到当前点云上,
            // 使点云图在pcl的view中能一直以当前帧(或者说当前的相机位姿)的视角动起来
            // 点云变换函数: pcl::transformPointCloud( *original, *output, T.matrix());
            cloud = joinPointCloud( cloud, currFrame, T, camera );
            
            //9. 决定是否实时显示
            if ( visualize == true )
                viewer.showCloud( cloud );
    
            lastFrame = currFrame;
        }
    
        //10. 保存点云文件
        pcl::io::savePCDFile( "data/result.pcd", *cloud ); //保存最终的点云结果 JOHNTODO
        return 0;
    }
    //从配置文件pd中读取rgb+depth作为帧的属性，返回Frame
    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));
    }

## 效果 ##

![202010031845028.gif][]

完整代码： [https://github.com/gaoxiang12/rgbd-slam-tutorial-gx/tree/master/part%20V][https_github.com_gaoxiang12_rgbd-slam-tutorial-gx_tree_master_part_20V]

数据位置：[http://yun.baidu.com/s/1i33uvw5][http_yun.baidu.com_s_1i33uvw5]

参考：[一起做RGB-D SLAM (5)][RGB-D SLAM _5]

[202010031845028.gif]: /images/20221123/84d7414d0bdb4668a5e6943bc9175e2e.png
[https_github.com_gaoxiang12_rgbd-slam-tutorial-gx_tree_master_part_20V]: https://github.com/gaoxiang12/rgbd-slam-tutorial-gx/tree/master/part%20V
[http_yun.baidu.com_s_1i33uvw5]: http://yun.baidu.com/s/1i33uvw5
[RGB-D SLAM _5]: https://www.cnblogs.com/gaoxiang12/p/4719156.html