evo评测vioslam步骤

参考博客：http://www.liuxiao.org/2017/11/%e4%bd%bf%e7%94%a8-evo-%e5%b7%a5%e5%85%b7%e8%af%84%e6%b5%8b-vi-orb-slam2-%e5%9c%a8-euroc-%e4%b8%8a%e7%9a%84%e7%bb%93%e6%9e%9c/#comment-3807

一.系统环境
ubuntu16.04
二.安装和更新evo
以下安装步骤均可
1）使用 pip3 安装
执行如下命令：
pip3 install evo --upgrade
2）使用源码进行安装

• git clone https://github.com/MichaelGrupp/evo.git
• cd evo
• pip install . --upgrade

三、修改 System 保存 TUM 观测值
由于 ORB SLAM2 并没有提供单目相机或者 VIO 保存 EuRoC 形式数据的方式，这里我们仍然使用 SaveTrajectoryTUM 方法保存观测值。
不过需要注意的是，由于 SaveTrajectoryTUM 不允许保存单目相机数据，我们修改一下创建一个新的函数 SaveTrajectory 用来保存单目 VIO 数据。
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;    }

四.修改ros参数文件
1）修改VIORB的ROS代码
打开/home/lry/LearnVIORB-RT/Examples/ROS/ORB_VIO/src/ros_vio.cc
修改函数为

• SLAM.SaveTrajectory(config._tmpFilePath+“trajectory.txt”);

2）修改VIORB参数文件
打开参数文件/home/lry/LearnVIORB-RT/config/euroc.yaml
修改test.InitVIOTmpPath为运行时轨迹信息的存储路径

• /home/lry/Data/EuRoc/MH_01_easy

修改bagfile为数据集的存储路径

• /home/lry/数据集/MH_01_easy.bag

五.下载euroc数据集真值数据
https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets
到上述网址下载zip后缀文件，解压后的cav后缀文件为groundtruth信息。
其中state_groundtruth_estimate0文件内的data.cav为数据集路径真值。

六.使用evo进行评测
运VIORB系统，等待数据集运行完毕后，crtl+c结束，观测值将会保存在之前设定的文件夹内。
其中trajectory.txt文件为路径观测值

• evo_ape euroc Data/EuRoC/V1_01_easy/state_groundtruth_estimate0/data2.csv
  Data/EuRoC/CameraTrajectory.txt -va --plot --save_results results/ORB.zip

运行以上命令即可得到评测数据。

运行过程中出现的问题：
在使用evo评测的过程中，可能会出现numpy的版本过低的问题，解决方法为使用sudo su得到管理员权限后终端输入以下命令完成升级：

• pip install -U numpy