提示:文章写完后,目录可以自动生成,如何生成可参考右边的帮助文档
目录
vins-fusion 怎么输出文件? vio_global,vio.txt,vio.csv内容与位置的修改
跑通VINS-Fusion全流程: 参考这里
1. vio_global.csv
将路径:
std::ofstream foutC("/home/tony-ws1/output/vio_global.csv", ios::app);
改成需要存的地方:xxx是自己主机名字
std::ofstream foutC("/home/xxx/data/VINS-Fusion.csv", ios::app);
global_fusion\src\globalOptNode.cpp>146line
原始:输出fusion结果(p,q)
// write result to file
// std::ofstream foutC("/home/tony-ws1/output/vio_global.csv", ios::app);//改这里
std::ofstream foutC("/home/xxx/data/VINS-Fusion.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();
改完路径可以输出文件,但是无法通过evo直接进行对比,修改如下:
std::ofstream foutC("/home/xxx/data/VINS-Fusion.txt", ios::app);
foutC.setf(ios::fixed, ios::floatfield);
foutC.precision(5);
foutC << pose_msg->header.stamp.toSec() << " "; //下面逗号也修改成空格
//foutC << pose_msg->header.stamp.toSec() << " ";
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();
2. vio.txt
当前帧的变换矩阵
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));
vins_estimator\src\KITTIOdomTest.cpp >78line,110line
3. vio.csv
路径同样改成:
std::ofstream foutC("/home/xxx/data/VIO.csv", ios::app);
vins_estimator\src\utility\visualization.cpp >184ine
foutC.setf(ios::fixed, ios::floatfield);
foutC.precision(0);
foutC << header.stamp.toSec() * 1e9 << ",";
foutC.precision(5);
foutC << estimator.Ps[WINDOW_SIZE].x() << ","
<< estimator.Ps[WINDOW_SIZE].y() << ","
<< estimator.Ps[WINDOW_SIZE].z() << ","
<< tmp_Q.w() << ","
<< tmp_Q.x() << ","
<< tmp_Q.y() << ","
<< tmp_Q.z() << ","
<< estimator.Vs[WINDOW_SIZE].x() << ","
<< estimator.Vs[WINDOW_SIZE].y() << ","
<< estimator.Vs[WINDOW_SIZE].z() << "," << endl;
4.输出gps的xyz数据
global_fusion/src/globalOpt.cpp 中的inputGPS函数中添加
该输出的数据可以用evo tum 直接进行对比
注意更改路径"/home/xxx/data/GPS.csv"
添加头文件:
#include <iostream>
#include <stdio.h>
#include <fstream>
void GlobalOptimization::inputGPS(double t, double latitude, double longitude, double altitude, double posAccuracy)
{
/*
坐标转换xyz,存入GPSPositionMap,newGPS = true;
*/
printf("Opt.inputGPS:\n");
double xyz[3];
GPS2XYZ(latitude, longitude, altitude, xyz);
vector<double> tmp{xyz[0], xyz[1], xyz[2], posAccuracy};
//printf("new gps: t: %f x: %f y: %f z:%f \n", t, tmp[0], tmp[1], tmp[2]);
GPSPositionMap[t] = tmp;
newGPS = true;//有可用的Gps数据,将进行融合optimize
lastGps=GPSPositionMap[t];
//添加在这里:
{
std::ofstream foutC("/home/xxx/data/GPS.csv", ios::app);
foutC.setf(ios::fixed, ios::floatfield);
foutC.precision(5);
foutC << t << " "
<< xyz[0] << " "
<< xyz[1] << " "
<< xyz[2] << " "
<< 0 << " "
<< 0 << " "
<< 0 << " "
<< 0 << endl;
foutC.close();
}
}
VINS-Fusion 和GPS输出的文件可以直接用evo进行对比了