在使用evo进行轨迹评估时提示时间戳没有对齐

这个问题出现在euroc数据集中，不止VINs-mono和ORB-slam会出现。

found no matching timestamps between reference and /home/xxx/uv_traj/stamped_traj_estimate.txt with max. time diff 0.01 (s) and time offset 0.0 (s)

第一个解决办法

因为生成轨迹和数据集需求轨迹不同。

ORB-slam：

轨迹保存函数是SaveTrajectoryTUM, 找到System.cc中的SaveTrajectoryTUM函数,将*lT改为1e9*(*lT)

//修改前
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;
//修改后
  f << setprecision(6) << 1e9*(*lT) << " " << setprecision(9) 
  << twc.at<float>(0) << " " << twc.at<float>(1) << " "<< twc.at<float>(2) << " "
  << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << endl;

VINs-mono:

…/VINS-Mono/vins_estimator/src/utility/visualization.cpp中的pubOdometry()函数：

//修改前
ofstream foutC(VINS_RESULT_PATH, ios::app);
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;
foutC.close();
//修改后
ofstream foutC(VINS_RESULT_PATH, ios::app);
foutC.setf(ios::fixed, ios::floatfield);
foutC.precision(0);
foutC << header.stamp.toSec() << " ";
foutC.precision(5);
foutC << estimator.Ps[WINDOW_SIZE].x() << " "
      << estimator.Ps[WINDOW_SIZE].y() << " "
      << estimator.Ps[WINDOW_SIZE].z() << " "
      << tmp_Q.x() << " "
      << tmp_Q.y() << " "
      << tmp_Q.z() << " "
      << tmp_Q.w() << endl;
foutC.close();

第二个解决办法：

关闭回环显示pose-graph，注销以下代码

//…/VINS-Mono/pose_graph/src/pose_graph.cpp中的addKeyFrame()函数：
        ofstream loop_path_file(VINS_RESULT_PATH, ios::app);
        loop_path_file.setf(ios::fixed, ios::floatfield);
        loop_path_file.precision(0);
        loop_path_file << cur_kf->time_stamp * 1e9 << ",";
        loop_path_file.precision(5);
        loop_path_file  << P.x() << ","
              << P.y() << ","
              << P.z() << ","
              << Q.w() << ","
              << Q.x() << ","
              << Q.y() << ","
              << Q.z() << ","
              << endl;*/
///VINS-Mono/pose_graph/src/pose_graph.cpp中的updatePath()函数：
            ofstream loop_path_file(VINS_RESULT_PATH, ios::app);
            loop_path_file.setf(ios::fixed, ios::floatfield);
            loop_path_file.precision(0);
            loop_path_file << (*it)->time_stamp * 1e9 << ",";
            loop_path_file.precision(5);
            loop_path_file  << P.x() << ","
                  << P.y() << ","
                  << P.z() << ","
                  << Q.w() << ","
                  << Q.x() << ","
                  << Q.y() << ","
                  << Q.z() << ","
                  << endl;