learn viorbslam

evo评测vioslam步骤

 weixin_41969600  2019-05-06 21:14  489 查看

https://www.geek-share.com/detail/2768148463.html

参考博客：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 中添加函数体：

[plain] view plaincopy

1. void System::SaveTrajectory(const string &filename)    {  
2. cout << endl << "Saving camera trajectory to " << filename << " ..." << endl;  
3.   
4. vector<KeyFrame*> vpKFs = mpMap->GetAllKeyFrames();  
5. sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId);  
6.   
7. // Transform all keyframes so that the first keyframe is at the origin.  
8. // After a loop closure the first keyframe might not be at the origin.  
9. cv::Mat Two = vpKFs[0]->GetPoseInverse();  
10.   
11. ofstream f;  
12. f.open(filename.c_str());  
13. f << fixed;  
14.   
15. // Frame pose is stored relative to its reference keyframe (which is optimized by BA and pose graph).  
16. // We need to get first the keyframe pose and then concatenate the relative transformation.  
17. // Frames not localized (tracking failure) are not saved.  
18.   
19. // For each frame we have a reference keyframe (lRit), the timestamp (lT) and a flag  
20. // which is true when tracking failed (lbL).  
21. list<ORB_SLAM2::KeyFrame*>::iterator lRit = mpTracker->mlpReferences.begin();  
22. list<double>::iterator lT = mpTracker->mlFrameTimes.begin();  
23. list<bool>::iterator lbL = mpTracker->mlbLost.begin();  
24. for(list<cv::Mat>::iterator lit=mpTracker->mlRelativeFramePoses.begin(),  
25. lend=mpTracker->mlRelativeFramePoses.end();lit!=lend;lit++, lRit++,      lT++, lbL++)  
26. {  
27. if(*lbL)  
28. continue;  
29.   
30. KeyFrame* pKF = *lRit;  
31.   
32. cv::Mat Trw = cv::Mat::eye(4,4,CV_32F);  
33.   
34. // If the reference keyframe was culled, traverse the spanning tree to get a suitable keyframe.  
35. while(pKF->isBad())  
36. {  
37. Trw = Trw*pKF->mTcp;  
38. pKF = pKF->GetParent();  
39. }  
40.   
41. Trw = Trw*pKF->GetPose()*Two;  
42.   
43. cv::Mat Tcw = (*lit)*Trw;  
44. cv::Mat Rwc = Tcw.rowRange(0,3).colRange(0,3).t();  
45. cv::Mat twc = -Rwc*Tcw.rowRange(0,3).col(3);  
46.   
47. vector<float> q = Converter::toQuaternion(Rwc);  
48.   
49. 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;  
50. }  
51. f.close();  
52. 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