Evo评测工具对VIORBSLAM2进行评估（详细步骤!!!）

本文链接：https://blog.csdn.net/qq_40464599/article/details/113482823

VIORBSLAM实在orbslam2的基础上进行改进的，使用单目相机和IMU进行状态估计。由于 VI ORB SLAM2 并没有官方的版本，我们主要使用王京实现的一个版本（https://github.com/jingpang/LearnVIORB）作为基础更改来进行测试。

王京的版本实在ROS下运行的。

由于EuRoC 测试集有 VIO 的数据，我使用了 EuRoC 测试集进行测试。

在测试前需要对王京的代码进行修改：

一、修改 System 保存 TUM 观测值：
我们修改一下创建一个新的函数 SaveTrajectory 用来保存单目 VIO 数据，其实原来的 SaveKeyFrameTrajectoryNavState的函数保存的轨迹txt格式文件是16列，和tum文件不一致，tum文件是8列，包括(time, t x , t y , t z , q x , q y , q z , q w )，所以其实可以默认输出原来的16列txt文件在进行裁剪（裁剪用到的指令为

 cut --delimiter " " --fields 1-8 KeyFrameNavStateTrajectory.txt > clean.tum

其中 KeyFrameNavStateTrajectory.txt为 SaveKeyFrameTrajectoryNavState的函数保存的轨迹txt格式文件，clean.tum是裁剪后的tum文件是8列。

）。

我们为了避免每次都要运行上述裁剪命令，所以我们创建一个新的函数 SaveTrajectory 用来保存成tum文件是8列包括(time, t x , t y , t z , q x , q y , q z , q w )。

1）在 System.h 中添加函数头

 void SaveTrajectory(const string &filename);

2）在 System.cc 中添加函数体：

 void System::SaveTrajectory(const string &filename)
    {
        cout << endl << "Saving camera trajectory to " << filename << " ..." << endl;
        
        vector<KeyFrame*> vpKFs = mpMap->GetAllKeyFrames();
        sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId);
        
        // Transform all keyframes so that the first keyframe is at the origin.
        // After a loop closure the first keyframe might not be at the origin.
        cv::Mat Two = vpKFs[0]->GetPoseInverse();
        
        ofstream f;
        f.open(filename.c_str());
        f << fixed;
        
        // Frame pose is stored relative to its reference keyframe (which is optimized by BA and pose graph).
        // We need to get first the keyframe pose and then concatenate the relative transformation.
        // Frames not localized (tracking failure) are not saved.
        
        // For each frame we have a reference keyframe (lRit), the timestamp (lT) and a flag
        // which is true when tracking failed (lbL).
        list<ORB_SLAM2::KeyFrame*>::iterator lRit = mpTracker->mlpReferences.begin();
        list<double>::iterator lT = mpTracker->mlFrameTimes.begin();
        list<bool>::iterator lbL = mpTracker->mlbLost.begin();
        for(list<cv::Mat>::iterator lit=mpTracker->mlRelativeFramePoses.begin(),
            lend=mpTracker->mlRelativeFramePoses.end();lit!=lend;lit++, lRit++, lT++, lbL++)
        {
            if(*lbL)
                continue;
            
            KeyFrame* pKF = *lRit;
            
            cv::Mat Trw = cv::Mat::eye(4,4,CV_32F);
            
            // If the reference keyframe was culled, traverse the spanning tree to get a suitable keyframe.
            while(pKF->isBad())
            {
                Trw = Trw*pKF->mTcp;
                pKF = pKF->GetParent();
            }
            
            Trw = Trw*pKF->GetPose()*Two;
            
            cv::Mat Tcw = (*lit)*Trw;
            cv::Mat Rwc = Tcw.rowRange(0,3).colRange(0,3).t();
            cv::Mat twc = -Rwc*Tcw.rowRange(0,3).col(3);
            
            vector<float> q = Converter::toQuaternion(Rwc);
            
            f << setprecision(6) << *lT << " " <<  setprecision(9) << twc.at<float>(0) << " " << twc.at<float>(1) << " " << twc.at<float>(2) << " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << endl;
        }
        f.close();
        cout << endl << "trajectory saved!" << endl;
    }

3) 需要改动主函数，这里需要注意，你得清楚自己用的是哪个主函数，由于本文王京的基于ROS，所以需要在主函数中ros_vio.cc中添加：

   SLAM.SaveTrajectory(config._tmpFilePath+"trajectory.txt");

此时，保存后的轨迹就是trajectory.txt了。

4）最后一部就是关于你保存的txt的路径了，需要在config文件夹下的euroc.yaml找到test.InitVIOTmpPath=“改成你自己的路径”

我的是保存在/home/juchunyu/data/output2/路径下。

二、测试运行：

1）运行代码：

roslaunch ORB_VIO testeuroc.launch  //启动viorbslam算法
rosbag play MH_01_easy.bag          //启动数据集MH_01_easy

运行效果：

2） APE评估：

evo_ape tum trajectory.txt groundtruth_data.tum -va --plot --plot_mode xz
trajectory.txt是viorbslam算法估计的轨迹，groundtruth_data.tum是真实的轨迹。evo_ape是估计绝对误差。

关于APE参见我之前的博客。

3）绘制轨迹:

evo_traj tum trajectory.txt --ref=groundtruth_data.tum -va -p --plot_mode=xz --correct_scale

20210131 鞠春宇