在ubuntu16.04中通过vins-fusion跑含有gps信号的kitti数据集

在ubuntu16.04中通过vins-fusion跑含有gps信号的kitti数据集

安装vins-fusion

这里是vins-fusion的github连接:https://github.com/HKUST-Aerial-Robotics/VINS-Fusion
这里没有什么问题,按照连接的指示来就行

运行kitti数据集

首先是下载kitti数据集,这部分必须翻墙,如果不想翻墙也可以通过我分享的百度云链接:复制这段内容后打开百度网盘手机App，操作更方便哦 链接:https://pan.baidu.com/s/1aAIBkDpRnWq3CUgoJmNSTw 提取码:cv75,如果链接失效就留言吧.

现在还差一步就是改动一下代码,保证有输出数据.

首先指定输出路径:打开vins-fusion/config/kitti_raw/kitti_10_03_config.yaml或者kitti_raw/kitti_09_30_config.yaml(根据数据集选择具体参数文件),第十行
output_path: "/home/tony-ws1/output/"
这里指明自己需要保存到的地址.

第二步接下来是修改代码使其输出轨迹文件,原本的vins_fusion是无轨迹文件输出的.
globalOptNode.cpp中vio_callback函数

    std::ofstream foutC("/home/tony-ws1/output/vio_global.csv", ios::app);//这里还是文件输出位置
   foutC.setf(ios::fixed, ios::floatfield);
    foutC.precision(0);
    foutC << pose_msg->header.stamp.toSec() * 1e9 << ",";
    foutC.precision(5);
    foutC << global_t.x() << ","
            << global_t.y() << ","
            << global_t.z() << ","
            << global_q.w() << ","
            << global_q.x() << ","
            << global_q.y() << ","
            << global_q.z() << endl;
    foutC.close(); 

这部分改成如下:

    std::ofstream foutC("/home/tony-ws1/output/vio_global.txt", ios::app);//这里还是文件输出位置,最好和前面位置一样
   foutC.setf(ios::fixed, ios::floatfield);
    foutC.precision(0);
    foutC << pose_msg->header.stamp.toSec() << " ";
    foutC.precision(5);
    foutC   << global_t.x() << " "
            << global_t.y() << " "
            << global_t.z() << " "
            << global_q.x() << " "
            << global_q.y() << " "
            << global_q.z() << " "
            << global_q.w() << endl;
    foutC.close(); 

改完以后保存,再改一下KITTIOdomTest.cpp中的

		
			Eigen::Matrix<double, 4, 4> pose;
			estimator.getPoseInWorldFrame(pose);
			if(outFile != NULL)
				fprintf (outFile, "%f %f %f %f %f %f %f %f %f %f %f %f \n",pose(0,0), pose(0,1), pose(0,2),pose(0,3),
																	       pose(1,0), pose(1,1), pose(1,2),pose(1,3),
																	       pose(2,0), pose(2,1), pose(2,2),pose(2,3));

改成如下

		
			Eigen::Matrix<double, 4, 4> pose;
			estimator.getPoseInWorldFrame(pose);
			if(outFile != NULL)
				fprintf (outFile, "%f %f %f %f %f %f %f %f %f %f %f %f\n",pose(0,0), pose(0,1), pose(0,2),pose(0,3),
																	       pose(1,0), pose(1,1), pose(1,2),pose(1,3),
																	       pose(2,0), pose(2,1), pose(2,2),pose(2,3));

这里输出的是kitti格式的轨迹,不过是未经回环的其实不改也无所谓,主要是globalOptNode里的代码要改那里是fusion的结果,不过是tum文件格式的.
接下来就是运行程序,按照github上面来,三个终端,分别是下面三个指令(可能要加上source ~/(你的目录)/devel/setup.bash).

    roslaunch vins vins_rviz.launch
    rosrun vins kitti_gps_test ~/catkin_ws/src/VINS-Fusion/config/kitti_raw/kitti_10_03_config.yaml YOUR_DATASET_FOLDER/2011_10_03_drive_0027_sync/ 
    rosrun global_fusion global_fusion_node

处理结果

跑完程序,在
std::ofstream foutC("/home/tony-ws1/output/vio_global.txt", ios::app);这里指定的文件夹下找到vio_global文件.使用evo分析结果,这里是tum数据格式,故使用

evo_traj tum vio_global.txt -p

这样可以看到初步的绘图结果

这里还只是初步完成了任务,接下来是对比真值,(这里有一点问题我还没明白,也希望大家可以讨论下,kitti的真值来自与卫星定位,这里视觉融合卫星定位的结果还与之前kitti的真值对比是不是不合适?)此处暂且这么做,大家有意见可以一起讨论.
因为真值是12列的kitti轨迹格式,所以要将真值转换成8列格式的tum轨迹格式,此处引用博主逆水独流的博文https://blog.csdn.net/ns2942826077/article/details/105821955
按照博文中的方法

python kitti_poses_and_timestamps_to_trajectory.py ./result/02.txt ./result/times.txt ./result/KITTI_02_gt.txt

通过该指令完成转换,详细见原博文,转换得到tum格式的真值后,我们可以进行评定,将我们得到的vio_global.txt 与这里的真值进行对比

evo_ape tum vio_global.txt   KITTI_02_gt.txt -a  -p

到了这里就大功告成了.