两两帧的视觉里程计

1. TUM数据集

https://vision.in.tum.de/data/datasets/rgbd-dataset/download

选择fr1/rpy下载：

在解压后，你将看到以下这些文件：

1. rgb.txt 和 depth.txt 记录了各文件的采集时间和对应的文件名。
2. rgb/ 和 depth/目录存放着采集到的 png 格式图像文件。彩色图像为八位三通道，深度图为 16 位单通道图像。文件名即采集时间。
3. groundtruth.txt 为外部运动捕捉系统采集到的相机位姿，格式为 (time, tx, ty, tz, qx, qy, qz, qw), 我们可以把它看成标准轨迹。

请注意彩色图、深度图和标准轨迹的采集都是独立的，轨迹的采集频率比图像高很多。在使用数据之前，需要根据采集时间，对数据进行一次时间上的对齐，以便对彩色图和深度图进行配对。TUM 提供了一个 python 脚本“associate.py”（https://vision.in.tum.de/data/datasets/rgbd-dataset/tools）帮我们完成这件事。请把此文件放到数据集目录下，运行：

python associate.py rgb.txt depth.txt > associate.txt

这段脚本会根据输入两个文件中的采集时间进行配对，最后输出到一个文件 associate.txt。输出文件含有被配对的两个图像的时间、文件名信息，可以作为后续处理的来源。

2. 两两帧的视觉里程计

两两帧之间的 VO 工作示意图如图所示。在这种 VO 里，我们定义了参考帧（Reference）和当前帧（Current）这两个概念。以参考帧为坐标系，我们把当前帧与它进行特征匹配，并估计运动关系。假设参考帧相对世界坐标的变换矩阵为 Trw，当前帧与世界坐标系间为 Tcw，则待估计的运动与这两个帧的变换矩阵构成左乘关系：

Tcr, s.t. Tcw = TcrTrw.

在 t - 1 到 t 时刻，我们以 t -1 为参考，求取 t 时刻的运动。然后，在 t 到 t + 1 时刻，我们又以 t 时刻为参考帧，考虑 t 到 t + 1 间的运动关系。如此往复，就得到了一条运动轨迹。

在匹配特征点的方式中，最重要的参考帧与当前帧之间的特征匹配关系，它的流程可归纳如下：

1. 对新来的当前帧，提取关键点和描述子。

2. 如果系统未初始化，以该帧为参考帧，根据深度图计算关键点的 3D 位置，返回 1。

3. 估计参考帧与当前帧间的运动。

4. 判断上述估计是否成功。

5. 若成功，把当前帧作为新的参考帧，回 1。

6. 若失败，计连续丢失帧数。当连续丢失超过一定帧数，置 VO 状态为丢失，算法结束。若未超过，返回 1。

VisualOdometry 类给出了上述算法的实现。

3. 测试示例

// -------------- test the visual odometry -------------
#include <fstream>
#include <boost/timer.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/viz.hpp>

#include "myslam/config.h"
#include "myslam/visual_odometry.h"

int main ( int argc, char** argv )
{
    if ( argc != 2 )
    {
        cout<<"usage: run_vo parameter_file"<<endl;
        return 1;
    }

    myslam::Config::setParameterFile ( argv[1] );                 //获取config配置文件
    myslam::VisualOdometry::Ptr vo ( new myslam::VisualOdometry );//创建视觉里程计对象

    //获取数据集地址
    string dataset_dir = myslam::Config::get<string> ( "dataset_dir" );
    cout<<"dataset: "<<dataset_dir<<endl;
    //获取数据对应表
    ifstream fin ( dataset_dir+"/associate.txt" );
    if ( !fin )
    {
        cout<<"please generate the associate file called associate.txt!"<<endl;
        return 1;
    }

    //读取rgb图获取时间以及rgb图名称，深度图获取时间以及深度图名称
    vector<string> rgb_files, depth_files;
    vector<double> rgb_times, depth_times;
    while ( !fin.eof() )
    {
        string rgb_time, rgb_file, depth_time, depth_file;
        fin>>rgb_time>>rgb_file>>depth_time>>depth_file;
        rgb_times.push_back ( atof ( rgb_time.c_str() ) );
        depth_times.push_back ( atof ( depth_time.c_str() ) );
        rgb_files.push_back ( dataset_dir+"/"+rgb_file );
        depth_files.push_back ( dataset_dir+"/"+depth_file );

        if ( fin.good() == false )
            break;
    }

    myslam::Camera::Ptr camera ( new myslam::Camera );//创建相机模型，并在构造函数中读取相机参数
    
    // visualization 3D可视化窗口
    cv::viz::Viz3d vis("Visual Odometry");
    cv::viz::WCoordinateSystem world_coor(1.0), camera_coor(0.5);
    cv::Point3d cam_pos( 0, -1.0, -1.0 ), cam_focal_point(0,0,0), cam_y_dir(0,1,0);
    cv::Affine3d cam_pose = cv::viz::makeCameraPose( cam_pos, cam_focal_point, cam_y_dir );//相机初始位置
    vis.setViewerPose( cam_pose );//设置视口的位姿
    
    world_coor.setRenderingProperty(cv::viz::LINE_WIDTH, 2.0);//设置世界坐标系
    camera_coor.setRenderingProperty(cv::viz::LINE_WIDTH, 1.0);//设置相机坐标系
    vis.showWidget( "World", world_coor );//设置显示世界坐标系窗口
    vis.showWidget( "Camera", camera_coor );//设置显示相机坐标系窗口

    cout<<"read total "<<rgb_files.size() <<" entries"<<endl;
    for ( int i=0; i<rgb_files.size(); i++ )
    {
        Mat color = cv::imread ( rgb_files[i] );       //读取当前rgb图
        Mat depth = cv::imread ( depth_files[i], -1 ); //读取当前深度图
        if ( color.data==nullptr || depth.data==nullptr )
            break;
        myslam::Frame::Ptr pFrame = myslam::Frame::createFrame();//创建帧对象
        pFrame->camera_ = camera;//设置相机参数
        pFrame->color_ = color;  //设置rgb图
        pFrame->depth_ = depth;  //设置深度图
        pFrame->time_stamp_ = rgb_times[i];//设置时间戳

        boost::timer timer;
        vo->addFrame ( pFrame );//添加帧,并进行基于特征点匹配-PnP方法估计相机运动
        cout<<"VO costs time: "<<timer.elapsed()<<endl;
        
        if ( vo->state_ == myslam::VisualOdometry::LOST )//估计失败
            break;

        SE3 Tcw = pFrame->T_c_w_.inverse();//求逆；T_c_w_：transform from world to camera
        
        //旋转矩阵和平移向量
        cv::Affine3d M(
            cv::Affine3d::Mat3( 
                Tcw.rotation_matrix()(0,0), Tcw.rotation_matrix()(0,1), Tcw.rotation_matrix()(0,2),
                Tcw.rotation_matrix()(1,0), Tcw.rotation_matrix()(1,1), Tcw.rotation_matrix()(1,2),
                Tcw.rotation_matrix()(2,0), Tcw.rotation_matrix()(2,1), Tcw.rotation_matrix()(2,2)),
            cv::Affine3d::Vec3(
                Tcw.translation()(0,0), Tcw.translation()(1,0), Tcw.translation()(2,0)
            )
        );

        // show the map and the camera pose
        cv::imshow("image", color );
        cv::waitKey(3);
        vis.setWidgetPose( "Camera", M);      
        //vis.spin();//q控制切换
        vis.spinOnce(1, false);
    }

    return 0;
}

可视化当前帧的图像与它的估计位置，并画出了世界坐标系的坐标轴（大）与当前帧的坐标轴（小），颜色与轴的对应关系为：蓝色-Z，红色-X，绿色-Y。