vins-fusion 怎么输出文件？ vio_global,vio.txt,vio.csv内容与位置的修改

提示：文章写完后，目录可以自动生成，如何生成可参考右边的帮助文档

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进行对比了