ROS 軌跡は tum 形式で保存され、evo 軌跡で描画されます

evoをインストールする

evo共有两种安装方式

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

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

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

git クローン https://github.com/MichaelGrupp/evo.git

然后进入evo文件夹

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

インストール完了後、evo コマンドを実行するとコマンドが見つからない場合がありますが、コンピュータを再起動するだけで済みます。

軌跡をtum形式で保存

tum データセット形式

セーブデータのデモを読む

#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

結果

元のリンク: https://blog.csdn.net/just_do_it567/article/details/114672305