LOAM、Lego-liom、Lio-sam轨迹保存，与Kitti数据集真值进行评估_legoloam保存轨迹

   一

        首先需要保存轨迹，轨迹保存参考下面的代码，最好自己添加一个节点（如下图），用新节点来订阅和保存轨迹至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根据时间戳对齐即可。