一
首先需要保存轨迹，轨迹保存参考下面的代码，最好自己
添加一个节点
（如下图），用新节点来订阅和保存轨迹至txt文件,因为直接在算法的线程中加入此步骤我试了好像保存不了，好像是在不同线程间的参数传递格式的问题（也可能是我个人的问题）。
与Kitti数据集真值评估需要将kitti 类型轨迹转化为tum格式的轨迹，用evo转化需要下载evo源码。
(223条消息) KITTI数据集基准、转换成tum以及十个groundtruth对应图_kitti转tum_小海盗haner的博客-CSDN博客
(223条消息) 【KITTI】KITTI数据集简介（四） — 标定校准数据calib_tr_velo_to_cam_Coding的叶子的博客-CSDN博客
a_loam在节点中订阅话题/aft_mapped_to_init，如下。lego同样操作订阅话题 /aft_mapped_to_init，lio-sam要订阅lio_sam/mapping/odometry话题（或者lio_sam/mapping/odometry_incremental，这两个都行，odometry_incremental是odometry使用IMU加权过的，差别不大），这些都可以在算法的Map节点里找到。我看很多博客上对lego/lio的欧拉角进行了转化再保存，实际上它们都有自己发布的odom信息，直接订阅就可以保存了。
void path_save(nav_msgs::Odometry odomAftMapped ){
//保存轨迹，path_save是文件目录,txt文件提前建好,/home/xxx/xxx.txt,
std::ofstream pose1(“path_save”, std::ios::app);
pose1.setf(std::ios::scientific, std::ios::floatfield);
pose1.precision(9);
static double timeStart = odomAftMapped.header.stamp.toSec();
auto T1 =ros::Time().fromSec(timeStart) ;
pose1<< odomAftMapped.header.stamp -T1<< " "
<< -odomAftMapped.pose.pose.position.y << " "
<< odomAftMapped.pose.pose.position.z << " "
<< odomAftMapped.pose.pose.position.x << " "
<< odomAftMapped.pose.pose.orientation.x << " "
<< odomAftMapped.pose.pose.orientation.y << " "
<< odomAftMapped.pose.pose.orientation.z << " "
<< odomAftMapped.pose.pose.orientation.w << std::endl;
pose1.close();
}
int main(int argc, char **argv){
ros::init(argc, argv, "path_save");
ros::NodeHandle nh;
ros::Subscriber save_path = nh.subscribe<nav_msgs::Odometry>("/aft_mapped_to_init", 100, path_save); //保存轨迹，a_loam直接订阅话题/aft_mapped_to_init。
ros::spin();
}
轨迹文件的时间戳得按(xxx.header.stamp -T1)这样写，
保存的是当前时间与第一帧位置时间的差值
，因为Kitti真值的轨迹时间戳是这样保存的，与真值时间戳不同步之后没法评估。数据的z、y、z写入顺序是人为的将坐标轴与Kitti数据集的对齐了（也可以按原来的写，或者用左乘外参矩阵进行调整，因为之后会用evo按时间戳对齐进行评估），如下图一KItti真值是x_z轴的，你跑出来可能是横着的，但是只要时间戳没问题你还是可以把它们对齐（图二）。对齐命令如下，具体评估可以看以下博客。
(223条消息) 轨迹评估工具使用总结（一） evo从安装到入门_evo评估工具怎么对齐起点_Techblog of HaoWANG的博客-CSDN博客
evo_traj tum 07_tum.txt lego_07.txt --ref=07_tum.txt -p -a
图一
图二
二
Kitti数据集是有外参标定信息的就是lidar->camera(world)的参数矩阵，也可以使用输出值左乘外参矩阵，参数如下，R为旋转，T为位移。
R: 7.027555e-03 -9.999753e-01 2.599616e-05 -2.254837e-03 -4.184312e-05 -9.999975e-01 9.999728e-01 7.027479e-03 -2.255075e-03
T: -7.137748e-03 -7.482656e-02 -3.336324e-01
然后把要输出的位置同样放进矩阵里，用外参矩阵进行调整就行了。也可以不旋转，只进行平移，因为evo可以根据时间戳对齐，不需要坐标轴一致。
实际测试后发现加不加外参差别好像不大。
(lidar->camera(world))
Eigen::Vector3d pose_t;
Eigen::Matrix3d pose_R;
pose_t <<-7.137748e-03 ,-7.482656e-02, -3.336324e-01;
pose_R <<7.027555e-03, -9.999753e-01, 2.599616e-05,
-2.254837e-03, -4.184312e-05 ,-9.999975e-01,
9.999728e-01 ,7.027479e-03 , -2.255075e-03
Eigen::Vector3d pose;
pose(0)=odomAftMapped.pose.pose.position.x;
pose(1)=odomAftMapped.pose.pose.position.y;
pose(2)=odomAftMapped.pose.pose.position.z;
Eigen::Vector3d pose_s;
//可以只 +pose_t
pose_s = pose_R * pose + pose_t;
总结
其实整体步骤并不多，但中间测试时遇到过很多问题，比如主线程中无法将位姿写入轨迹文件，保存的归家与真值时间戳的对齐问题，轨迹图不在一个平面上的问题等。但后来发现只要将
轨迹时间戳相互对齐
，就不用管轨迹坐标轴角度的不同，直接用evo根据时间戳对齐即可。