#include <iostream>
#include <stdio.h>
#include <fstream>
#include <queue>
#include <mutex>
std::string gpssavename="/home/dongdong/v2_Project/v3_SLAM/v5_VinsFusion/catkin/src/VINS-Fusion/outdata/vio_global.csv";
std::ofstream foutC(gpssavename, 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.w() << " "
<< global_q.x() << " "
<< global_q.y() << " "
<< global_q.z() << endl;
foutC.close();