ROS轨迹保存为tum格式,并用evo轨迹绘制

安装evo

    evo共有两种安装方式

    快捷安装 ，直接安装最新的稳定发行版；

pip install evo --upgrade --no-binary evo

    源码安装 ，下载源码进行安装；
    首先在任意文件夹下下载evo，也可以在home中直接下载

git clone https://github.com/MichaelGrupp/evo.git

    然后进入evo文件夹

pip install --editable . --upgrade --no-binary evo

安装完成后运行evo命令时可能会出现找不到命令的问题,只需要重启计算机即可.

将轨迹保存为tum格式

tum数据集格式

读取保存数据demo

#include<iostream>
#include<fstream>
#include<eigen3/Eigen/Eigen>
#include<ros/ros.h>
#include<tf/transform_listener.h>

using namespace std;

ofstream foutC;


int main(int argc, char **argv){
    //string file_name;
    //ros::param::get("file_name",file_name);
    ros::init(argc, argv, "save_traj_as_tum");
    ros::NodeHandle nh;
    
    foutC.open("./test.txt");
    tf::TransformListener listener;
    ros::Rate rate(20);
    while(ros::ok()){
        tf::StampedTransform transform;
        try
        {
            listener.waitForTransform("/odom", "/base_link", ros::Time(0), ros::Duration(1));
            listener.lookupTransform("/odom", "/base_link", ros::Time(0), transform);
            foutC << transform.stamp_ << " ";
            float x = transform.getOrigin().getX();
            float y = transform.getOrigin().getY();
            float z = transform.getOrigin().getZ();
            float qx = transform.getRotation().getX();
            float qy = transform.getRotation().getY();
            float qz = transform.getRotation().getZ();
            float qw = transform.getRotation().getW();
            ROS_INFO("%f %f %f %f %f %f %f",x,y,z,qx,qy,qz,qw);
            foutC << x <<" " << y << " " << z << " " << qx << " " << qy << " " << qz << " " << qw << std::endl;
        }
        catch(const std::exception& e)
        {
            std::cerr << e.what() << '\n';
        }
        rate.sleep();
    }
    foutC.close();
    return 0;
}

保存完之后运行如下指令:

evo_traj tum test.txt -p