VINS-Fusion轨迹评估

代码轨迹输出修改

1. visualization.cpp
   VINS-Fusion-master\vins_estimator\src\utility\visualization.cpp的150行左右：

/*修改前
        // write result to file
        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();
*/
//修改后
        double truetime = header.stamp.toSec();
        ofstream foutC("/home/cy/projects/VINS-Fusion/src/VINS-Fusion-master/data/result.csv", ios::app);
        foutC.setf(ios::fixed, ios::floatfield);
        foutC << truetime <<" ";
        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();

1. pose_graph.cpp（如果没开回环可以不改）
   VINS-Fusion-master\loop_fusion\src\pose_graph.cpp的190行左右

/*修改前
    if (SAVE_LOOP_PATH)
    {
        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;
        loop_path_file.close();
    }
*/
//修改后
    if (SAVE_LOOP_PATH)
    {
        ofstream loop_path_file("/home/cy/projects/VINS-Fusion/src/VINS-Fusion-master/data/loop_result.csv", ios::app);
        double turetime = cur_kf->time_stamp;
        loop_path_file.setf(ios::fixed, ios::floatfield);
        loop_path_file << turetime << " ";
        loop_path_file  << P.x() << " "
              << P.y() << " "
              << P.z() << " "
              << Q.x() << " "
              << Q.y() << " "
              << Q.z() << " "
              << Q.w() << " "
              << endl;
        loop_path_file.close();
    }

2. globalOptNode.cpp
   VINS-Fusion-master\global_fusion\src\globalOptNode.cpp的150行左右

/*修改前
    // write result to file
    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/cy/projects/VINS-Fusion/src/VINS-Fusion-master/data/vio_global.csv", 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();

修改后重新编译运行，在data文件夹中获取结果

EVO评测

以下都是Euroc数据集的运行测评

1. 数据集真值格式处理
   下载的数据集中真值以.csv格式存储，转换为.tum格式

evo_traj euroc data.csv --save_as_tum

2. 计算绝对位姿误差

#data.tum为真值，result.csv为系统运行结果，-va显示详细计算结果，--plot显示图像(或-p)
evo_ape tum data.tum result.csv -va --plot

3. 绘制多条轨迹

#--correct_scale(尺度矫正) --align(对齐)
evo_traj tum result.csv data.tum --ref=data.tum -p --correct_scale --align

三维轨迹绘制平面投影

--plot_mode=xz

4. bag包中真值转换为tum格式

evo_traj bag /topic_name --save_as_tum

bag包播放

1. 倍速播放

rosbag play -r 2 filename.bag  #2倍速

2. 从第几秒开始播放

rosbag play -s 3 filename.bag  #第3秒

3. 仅使用包的前几秒信息

rosbag play -u 20 filename.bag  #前20秒