代码轨迹输出修改
- visualization.cpp
VINS-Fusion-master\vins_estimator\src\utility\visualization.cpp的150行左右:
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();
- pose_graph.cpp(如果没开回环可以不改)
VINS-Fusion-master\loop_fusion\src\pose_graph.cpp的190行左右
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();
}
- globalOptNode.cpp
VINS-Fusion-master\global_fusion\src\globalOptNode.cpp的150行左右
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评测
- 数据集真值格式处理
下载的数据集中真值以.csv格式存储,转换为.tum格式
evo_traj euroc data.csv --save_as_tum
- 计算绝对位姿误差
evo_ape tum data.tum loop_result.csv -va --plot
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)