生成A-LOAM运行轨迹文件并用EVO工具绘制

---

Windows 10(64bits) + VMware 16 Pro + Ubuntu 20.04 + noetic

---

针对轨迹绘制问题，主要包含两个具体问题：其一，怎么让算法生成轨迹文件？其二，怎么使用EVO工具？A-LOAM算法作为最简单的激光SLAM算法，故本文以该算法为例进行学习和调试。

一、基本调试

1.1 EVO工具安装

直接在终端输入以下代码进行快捷安装就可以，非常简单。

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

1.2 EVO绘制KITTI数据集真值轨迹

在轨迹文件存放位置打开终端，输入以下命令即可用EVO工具绘制轨迹图，同时可以得到姿态角变化。
注意：

• kitti代表数据格式，主要有kitti、tum、euroc、ros格式，本文选择生成tum格式的轨迹，相比于kitti格式，多个时间戳。

evo_traj kitti 00.txt -p

序列00的轨迹和姿态角如下图。

1.3 同时绘制两条轨迹

绘制kitti数据集00和08轨迹。

evo_traj kitti 00.txt 08.txt -p

1.4 确定发布轨迹的topic以及所在文件

根据算法节点图查看topic和node结构，确定发布轨迹topic的代码在laserMapping.cpp里。

使用以下命令查看topic的输出内容，一个是高帧率的轨迹，一个是建图后的低帧率轨迹。

rostopic echo /laser_odom_path
rostopic echo /aft_mapped_path

二、A-LOAM算法生成轨迹文件

经过查阅，发现这方面的分享博客很少，借鉴少有的几份代码，发现主要是采用C++的输出文件流ofstream函数实现。

2.1 修改laserMapping.cpp代码

在发布里程计topic代码后面增加代码，并重新catkin_make编译。

********************原代码，方便找代码添加位置*****************
nav_msgs::Odometry odomAftMapped;
	odomAftMapped.header.frame_id = "camera_init";
	odomAftMapped.child_frame_id = "/aft_mapped";
	odomAftMapped.header.stamp = laserOdometry->header.stamp;
	odomAftMapped.pose.pose.orientation.x = q_w_curr.x();
	odomAftMapped.pose.pose.orientation.y = q_w_curr.y();
	odomAftMapped.pose.pose.orientation.z = q_w_curr.z();
	odomAftMapped.pose.pose.orientation.w = q_w_curr.w();
	odomAftMapped.pose.pose.position.x = t_w_curr.x();
	odomAftMapped.pose.pose.position.y = t_w_curr.y();
	odomAftMapped.pose.pose.position.z = t_w_curr.z();
	pubOdomAftMappedHighFrec.publish(odomAftMapped);
	
****************************加在这里******************************
    //根据自己的文件存放位置修改路径和文件名
	std::ofstream pose1("/路径/文件名.txt", std::ios::app);
	pose1.setf(std::ios::scientific, std::ios::floatfield);
	pose1.precision(8);
	
	static double timeStart = odometryBuf.front()-> header.stamp.toSec();
	auto T1 = ros::Time().fromSec(timeStart);
	pose1 << odomAftMapped.header.stamp - T1  << " "
              << odomAftMapped.pose.pose.position.x << " "
              << odomAftMapped.pose.pose.position.y << " "
              << odomAftMapped.pose.pose.position.z << " "
              << odomAftMapped.pose.pose.orientation.x << " "
              << odomAftMapped.pose.pose.orientation.y << " "
              << odomAftMapped.pose.pose.orientation.z << " "
              << odomAftMapped.pose.pose.orientation.w << std::endl;
	pose1.close();

2.2 运行代码并播放数据集

播放nsh_indoor_outdoor数据集，可以看到在选择的路径下面生成了一个.txt文件，存放了tum格式的轨迹，包括时间戳，位置，姿态四元数。
注意：

• 结果数据精度取决于precision后面括号里的数。

2.3 EVO绘制轨迹

注意：

• 我们是按照tum格式生成的文件，所以把格式类型从“kitti”改为“tum”

evo_traj tum aloam2.txt -p

三、尚存问题

1、绘制的kitti数据集的轨迹，为什么坐标轴不对
2、轨迹误差分析