一、工作空间的创建，功能包的编译等等

二、lego_loam运行、记录traj轨迹

三、evo对比使用

四、kitti2bag转换

五、lio_sam

一、工作空间的创建，功能包的编译等等

https://blog.csdn.net/qq_40528849/article/details/124705983

二、lego_loam运行、记录traj轨迹

1.运行 launch 文件

roslaunch lego_loam run.launch

注意：参数“/ use_sim_time”，对于模拟则设置为“true”，对于使用真实机器人则设置为“false”。

2. 播放bag文件

rosbag play *.bag --clock --topic /velodyne_points /imu/data

P.S.①运行上述指令时注意cd到存放rosbag文件的路径下

②虽然 /imu/data 是可选的，但如果提供的话，它可以大大提高估计的准确性。如果你的 IMU 帧与 Velodyne 帧不对齐，使用 IMU 数据将导致明显漂移。

③如果bag文件的话题不匹配，需要remap话题，如下：LeGO-LOAM的节点话题是/kitti/velo/pointcloud，而rosbag的节点话题是velodyne_points。笔者选择更改rosbag的节点话题，运行remap命令：

格式：

rosbag play *.bag 发布的话题 := 算法接收的话题 --clock

举例：

-r 参数用来指定播放速度，3代表以3倍速度播放

rosbag play *.bag velodyne_points:=/kitti/velo/pointcloud --clock -r 3

LeGO_LOAM

rosbag play kitti_2011_10_03_drive_0034_synced_10hz.bag --clock --topics /kitti/velo/pointcloud

LIO_SAM

rosbag play kitti_2011_10_03_drive_0027_synced_100hz.bag --clock --topics /points_raw /imu_raw -r 0.1

-s 参数用于指定从几秒开始:

//从第10秒开始播放xx.bag

rosbag play -s 10 xx.bag

rosbag使用介绍_rosbag play 指定时间_春至冬去的博客-CSDN博客

ROS系统学习笔记：bag的录制和播放 - 知乎

使用LeGO-LOAM运行KITTI数据集_Ocean_Controller的博客-CSDN博客

④运动轨迹的记录、kitti的64线的配置代码修改参考如下：

//保存轨迹，需要修改轨迹的保存路径，保存的txt文件要提前建好。

在终端窗口，键入ctrl+c退出程序后，在对应路径文件下会写入tum格式的路径信息（当你在运行（lego_loam算法run.launch）的终端里按一次Ctrl + C时才会执行。按两次会保存不全，因此按一次就可以了。生成完后会自动停止。），如果想保存在其他路径下，需要在mapOptmization.cpp中的visualizeGlobalMapThread函数中修改路径：

LeGO-LOAM跑KITTI数据集评估方法【代码改】_lego loam跑kitti需要改什么_学无止境的小龟的博客-CSDN博客

值得注意的是：

我按照上面博客修改方法保存位姿，测试kitti_06序列。发现evo怎么都对不齐位姿，即使用了-as命令也不行。就是差一点点，让人感觉是不是lego算法有误差？后来发现，这个轨迹像反的，尝试着把xyz取反跑一下。结果就完美匹配。

不过呢还是存在一个问题，roll和真值相比差了一个常量，如下图：

分享一下我的代码：

//保存轨迹信息（时间戳、xyz、旋转四元数q）
void visualizeGlobalMapThread(){ros::Rate rate(0.2);while (ros::ok()){rate.sleep();publishGlobalMap();}// save final point cloudstd::cout << "start save final point cloud" << std::endl;std::cout << "======================================================" << std::endl;ofstream f;f.open("/home/nb/traj_slam/06_own_tum.txt");						f << fixed;//std::cout << "traj roll" << cloudKeyPoses6D->points[0].roll << std::endl;for(size_t i = 0;i < cloudKeyPoses3D->size();i++){float cy = cos((cloudKeyPoses6D->points[i].yaw)*0.5);float sy = sin((cloudKeyPoses6D->points[i].yaw)*0.5);float cr = cos((cloudKeyPoses6D->points[i].roll)*0.5);float sr = sin((cloudKeyPoses6D->points[i].roll)*0.5);float cp = cos((cloudKeyPoses6D->points[i].pitch)*0.5);float sp = sin((cloudKeyPoses6D->points[i].pitch)*0.5);float w = cy * cp * cr + sy * sp * sr;float x = cy * cp * sr - sy * sp * cr;float y = sy * cp * sr + cy * sp * cr;float z = sy * cp * cr - cy * sp * sr;//save the traj  减掉第0个时间戳。cloudKeyPoses6D->points[0].time == 1317400962.039569f << setprecision(6) << (cloudKeyPoses6D->points[i].time-cloudKeyPoses6D->points[0].time) << " " << setprecision(9) << -1*(cloudKeyPoses6D->points[i].x) << " " << -1*(cloudKeyPoses6D->points[i].y) << " " << -1*(cloudKeyPoses6D->points[i].z) << " " << x << " " << y << " " << z << " " << w << endl;}f.close();
}

⑤（选做）打印话题、测试信息

"a_loam和lego_loam在节点中订阅话题/aft_mapped_to_init，lio-sam要订阅lio_sam/mapping/odometryh话题（或者lio_sam/mapping/odometry_incremental，这两个好像没区别），这些都可以在算法的Map节点里找到。我看很多博客上对lego/lio的欧拉角进行了转化再保存，实际上它们都有自己发布的odom信息，直接订阅就可以保存了。"

LOAM、Lego-liom、Lio-sam轨迹保存，与Kitti数据集真值进行评估_kitti数据集轨迹_李97的博客-CSDN博客

在终端执行以下命令，可以查看打印pose：
rostopic echo /aft_mapped_to_init（odom的话题）
在终端执行以下命令，可以将pose保存到pose.txt中：
rostopic echo /aft_mapped_to_init > pose.txt
但是，这种方式不方便数据分析，需要以一定格式存储。

参考链接：

LeGO-LOAM初探：原理，安装和测试_W_Tortoise的博客-CSDN博客

三、evo对比使用

evo开源代码：https://github.com/MichaelGrupp/evo
evo 对姿态的数据格式有三种，分别为：TUM/EuRoC/Kitti

区别为：

进行odom的轨迹保存后进行评估，常用的指令主要包括三个：

1 evo_traj

evo_traj tum --ref=tum_00_gt.txt re_00_TUM.txt -p --plot_mode xz -as

其中，-p参数绘制轨迹，--ref参数指定参考轨迹， -as参数将两条轨迹进行对齐

2 evo_ape

evo_ape评价的是绝对误差随路程的累计，是一个累积量。

evo_ape tum groundtruth.txt our.txt -as

max：最大误差（m） mean：误差均值
median：误差中位数 min：最小误差
rmse：均方根误差 sse：方差
std：标准差

3 evo_rpe

evo_rpe相对位姿误差可以给出局部精度，例如slam系统每米的平移或者旋转漂移量。

其中--delta 100表示的是每隔100米统计一次误差，这样统计的其实就是误差的百分比，和kitti的odometry榜单中的距离误差指标就可以直接对应了。

evo_rpe tum Ground_Truth.txt Marked-LIEO\(ours\).txt --delta 100

轨迹评估工具使用总结（一） evo从安装到入门_evo工具起点对齐_Techblog of HaoWANG的博客-CSDN博客

lego-loam 跑 kitti00包（kitti2bag+lego-loam+evo）详细版_kitti00数据集_Cloudy_to_sunny的博客-CSDN博客

用于evo评估的里程计轨迹保存方法_evo保存轨迹_dear小王子的博客-CSDN博客

evo的旋转对齐命令，如下：

–align/-a 采用SE(3) Umeyama对齐，只处理平移和旋转
–align --correct_scale/-as 采用Sim(3) Umeyama对齐，同时处理平移旋转和尺度
–correct_scale/-s 仅对齐尺度

SLAM精度评定工具——EVO使用方法详解_有了个相册的博客-CSDN博客

evo的tum转kitti命令，如下：

evo_traj tum KeyFrameTrajectory_TUM.txt --save_as_kitti

用evo工具分析ORB-SLAM2运行TUM，KITTI，EuRoC数据集轨迹_m0_60355964的博客-CSDN博客

四、kitti2bag转换

看了几个转换算法，最后还是选用了kitti2bag.

lidar2rosbag_kitti :

这里阿里员工在github上开源的工具包，

优点：据说可以解决各种类型数据集的转换问题。

缺点：从名字可以看出来，转换后只有lidar话题数据，所以我也没试过。

kitti_lego_loam：

代码：https://github.com/Mitchell-Lee-93/kitti-lego-loam

该code里面是两个功能包，一个是将kitti数据集转换成bag，另一个就是修改了一点的lego-loam代码：1.修改雷达参数（kitti数据集的lidar是64线）2.关闭了运动补偿 3.为了评测，保存了位姿数据，也就是记录了轨迹。
测试该代码出现错误：转换出来只有4.1kb。原博主给我指出了问题：“数据集文件夹的子文件夹名字是sequence，c程序里设定是sequences，有一个s的差别，可能导致读取不到数据集。另外查看路径是否正确，文件夹名字摆放位置是否正确。生成4.1kb一般是没有读取到源文件。”但是我检查发现，命名是正确的。找不到原因，因此也就放弃了这个方法。

kitti2bag：（推荐）

（同步imu10hz）代码：GitHub - tomas789/kitti2bag: Convert KITTI dataset to ROS bag file the easy way!

（原始imu100hz）代码：GitHub - TixiaoShan/LIO-SAM: LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping

kitti2bag是kitti官网推荐的转换包，优点是既能够转换同步后的数据中，imu的频率在10Hz左右，也能转换原始的100Hz的IMU数据，该方法同时使用原始数据（extract）和同步数据（synced）制作数据包。

下面是kitti2bag的具体操作流程：

1、该博客手把手图片教学，挺方便。

将KITTI数据集转化为ROS bag包——kitti2bag使用教程_kitti rosbag_Kadima°的博客-CSDN博客

不过我看好多博客下面评论，不知道在哪下载数据集，下载真值等等。

官网如下：The KITTI Vision Benchmark Suite

先科学上网，再注册，然后找raw data，进去找residential，再按下图找对应的序列。

数据区别：100Hz的IMU数据在 [unsynced+unrectified data] 中，而 [synced+rectified data] 保存的是修正后的10Hz数据。

我按照参考方法，产生了4.1kb的bag包，报错信息说找不到相关文件。所以我新建了一个2011_09_30的文件，这下就成功了。分享一下我的文件目录。

首先cd到2011_09_30文件夹所在的目录，然后执行如下命令：

kitti2bag -t 2011_09_30 -r 0020 raw_synced

该命令中，2011_09_30下载并解压后的数据集目录，0020是drive的地址序号,raw_synced表示加载的文件是raw_data。

转换后的bag文件太大了，因此使用命令进行了压缩。

rosbag compress --lz4 *.bag

2、该博客提供了序列对应表格，序列图，最重要的是，在文末她说：“同步后的数据中，imu的频率在10Hz左右，如果需要100Hz的IMU数据，可以参考LIO-SAM【项目地址】中作者改过的kitti2bag文件。该方法同时使用原始数据（extract）和同步数据（synced）制作数据包。”

# kitti2bag（原始imu数据100hz）## How to run it?```bash
wget https://s3.eu-central-1.amazonaws.com/avg-kitti/raw_data/2011_09_26_drive_0084/2011_09_26_drive_0084_sync.zip
wget https://s3.eu-central-1.amazonaws.com/avg-kitti/raw_data/2011_09_26_drive_0084/2011_09_26_drive_0084_extract.zip
wget https://s3.eu-central-1.amazonaws.com/avg-kitti/raw_data/2011_09_26_calib.zip
unzip 2011_09_26_drive_0084_sync.zip
unzip 2011_09_26_drive_0084_extract.zip
unzip 2011_09_26_calib.zip
python kitti2bag.py -t 2011_09_26 -r 0084 raw_synced .
```That's it. You have a bag that contains your data.Other source files can be found at [KITTI raw data](http://www.cvlibs.net/datasets/kitti/raw_data.php) page.

KITTI数据集_木子雨舟的博客-CSDN博客

执行命令后报错：

Traceback (most recent call last):File "kitti2bag.py", line 17, in <module>from tqdm import tqdm
ImportError: No module named tqdm

第一次跑，报错的原因是缺少tqdm包，我们只需要安装一下就行。

pip install tqdm

成功截图：

在kitti 06序列上，进行原始数据和优化后数据的evo_traj对比，如下图：

可以看出优化后的轨迹06_own_tum更优，与真值轨迹较近。

在kitti 06序列上，进行原始数据和优化后数据的ape、rpe对比，如下图：

SLAM精度评定工具——EVO使用方法详解_有了个相册的博客-CSDN博客

五、lio_sam

为了适配kitti数据集，我们需要修改参数和代码，来满足lio_sam对ring 和 time的要求。

5.1、修改参数

params.yaml文件修改：

  # Topics  for processing kitti by authorpointCloudTopic: "/points_raw"           		# Point cloud data/  for kitti  "/kitti/velo/pointcloud"imuTopic: "/imu_raw"                         	# IMU data/ for kitti  "/kitti/oxts/imu"odomTopic: "odometry/imu"                   	# IMU pre-preintegration odometry, same frequency as IMUgpsTopic: "odometry/gpsz"                   	# GPS odometry topic from navsat, see module_navsat.launch file# Sensor Settingssensor: velodyne                            # lidar sensor type, 'velodyne' or 'ouster' or 'livox'N_SCAN: 64                                  # number of lidar channel (i.e., Velodyne/Ouster: 16, 32, 64, 128, Livox Horizon: 6)Horizon_SCAN: 1800                          # lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048, Livox Horizon: 4000)downsampleRate: 2                           # default: 1. Downsample your data if too many points. i.e., 16 = 64 / 4, 16 = 16 / 1lidarMinRange: 1.0                          # default: 1.0, minimum lidar range to be usedlidarMaxRange: 1000.0                       # default: 1000.0, maximum lidar range to be used # lidar到imu的平移extrinsicTrans: [8.086759e-01, -3.195559e-01, 7.997231e-01]# imu到lidar的旋转extrinsicRot: [9.999976e-01, 7.553071e-04, -2.035826e-03, -7.854027e-04, 9.998898e-01, -1.482298e-02, 2.024406e-03, 1.482454e-02, 9.998881e-01]extrinsicRPY: [9.999976e-01, 7.553071e-04, -2.035826e-03, -7.854027e-04, 9.998898e-01, -1.482298e-02, 2.024406e-03, 1.482454e-02, 9.998881e-01]

注意点：
1、配置文件（params.yaml）内的参数通过参数服务器传送进源程序，会覆盖掉头文件内（utility.h）的对应参数。
2、其中extrinsicRot表示imu -> lidar的坐标变换，用于旋转imu坐标系下的角速度和线加速度到lidar坐标系下，extrinsicRPY则用于旋转imu坐标系下的欧拉角（姿态信息）到lidar坐标下，由于lio-sam作者使用的imu的欧拉角旋转指向与lidar坐标系不一致，因此使用了两个不同的旋转矩阵，但是大部分的设备两个旋转应该是设置为相同的。

在Ubuntu20.04系统上LIO-SAM跑KITTI数据集和自己数据集代码修改_学无止境的小龟的博客-CSDN博客

5.2、修改代码（产生ring、time、保存tum格式的轨迹）

utility.h文件修改：

    // add by sxbool has_ring;float ang_bottom;float ang_res_y;ParamServer(){nh.param<std::string>("/robot_id", robot_id, "roboat");nh.param<std::string>("lio_sam/pointCloudTopic", pointCloudTopic, "points_raw");nh.param<std::string>("lio_sam/imuTopic", imuTopic, "imu_correct");nh.param<std::string>("lio_sam/odomTopic", odomTopic, "odometry/imu");nh.param<std::string>("lio_sam/gpsTopic", gpsTopic, "odometry/gps");// add by sxnh.param<bool>("lio_sam/has_ring", has_ring, true);nh.param<float>("lio_sam/ang_bottom", ang_bottom, 15.0);nh.param<float>("lio_sam/ang_res_y", ang_res_y, 1.0);..............}

cloud_info.msg文件修改：

# 用于改写ring和time相关
float32 startOrientation
float32 endOrientation
float32 orientationDiff

imageProjection.cpp文件修改：

   void projectPointCloud(){int cloudSize = laserCloudIn->points.size();bool halfPassed = false;cloudInfo.startOrientation = -atan2(laserCloudIn->points[0].y, laserCloudIn->points[0].x);cloudInfo.endOrientation   = -atan2(laserCloudIn->points[laserCloudIn->points.size() - 1].y,laserCloudIn->points[laserCloudIn->points.size() - 1].x) + 2 * M_PI;if (cloudInfo.endOrientation - cloudInfo.startOrientation > 3 * M_PI) {cloudInfo.endOrientation -= 2 * M_PI;} else if (cloudInfo.endOrientation - cloudInfo.startOrientation < M_PI)cloudInfo.endOrientation += 2 * M_PI;cloudInfo.orientationDiff = cloudInfo.endOrientation - cloudInfo.startOrientation;// range image projectionfor (int i = 0; i < cloudSize; ++i){PointType thisPoint;thisPoint.x = laserCloudIn->points[i].x;thisPoint.y = laserCloudIn->points[i].y;thisPoint.z = laserCloudIn->points[i].z;thisPoint.intensity = laserCloudIn->points[i].intensity;float range = pointDistance(thisPoint);if (range < lidarMinRange || range > lidarMaxRange)continue;//从此处开始           int rowIdn = -1;if (has_ring == true){rowIdn = laserCloudIn->points[i].ring;}else{float verticalAngle, horizonAngle;verticalAngle = atan2(thisPoint.z, sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y)) * 180 / M_PI;rowIdn = (verticalAngle + ang_bottom) / ang_res_y;}if (rowIdn < 0 || rowIdn >= N_SCAN)continue;if (rowIdn % downsampleRate != 0)continue;int columnIdn = -1;if (sensor == SensorType::VELODYNE || sensor == SensorType::OUSTER){float horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;static float ang_res_x = 360.0/float(Horizon_SCAN);columnIdn = -round((horizonAngle-90.0)/ang_res_x) + Horizon_SCAN/2;if (columnIdn >= Horizon_SCAN)columnIdn -= Horizon_SCAN;}else if (sensor == SensorType::LIVOX){columnIdn = columnIdnCountVec[rowIdn];columnIdnCountVec[rowIdn] += 1;}if (columnIdn < 0 || columnIdn >= Horizon_SCAN)continue;if (rangeMat.at<float>(rowIdn, columnIdn) != FLT_MAX)continue;if (has_ring == true)thisPoint = deskewPoint(&thisPoint, laserCloudIn->points[i].time);else {float ori = -atan2(thisPoint.y, thisPoint.x);if (!halfPassed) {if (ori < cloudInfo.startOrientation - M_PI / 2) {ori += 2 * M_PI;} else if (ori > cloudInfo.startOrientation + M_PI * 3 / 2) {ori -= 2 * M_PI;}if (ori - cloudInfo.startOrientation > M_PI) {halfPassed = true;}} else {ori += 2 * M_PI;if (ori < cloudInfo.endOrientation - M_PI * 3 / 2) {ori += 2 * M_PI;} else if (ori > cloudInfo.endOrientation + M_PI / 2) {ori -= 2 * M_PI;}}float relTime = (ori - cloudInfo.startOrientation) / cloudInfo.orientationDiff;// 激光雷达10Hz，一帧0.1秒laserCloudIn->points[i].time = 0.1 * relTime;thisPoint = deskewPoint(&thisPoint, laserCloudIn->points[i].time);}rangeMat.at<float>(rowIdn, columnIdn) = range;int index = columnIdn + rowIdn * Horizon_SCAN;fullCloud->points[index] = thisPoint;}}

保存tum格式的轨迹

在mapOptmization -> void publishOdometry（）增加下面代码：

//保存轨迹，path_save是文件目录,txt文件提前建好,/home/xxx/xxx.txt,
std::ofstream pose1("/home/nb/traj_slam/lio_sam/100hz_own_tum.txt", std::ios::app);
pose1.setf(std::ios::scientific, std::ios::floatfield);
pose1.precision(9);
// if(flag_tum=1){
static double timeStart = laserOdometryROS.header.stamp.toSec();
// flag_tum=0;
// }
auto T1 =ros::Time().fromSec(timeStart) ;
// tf::Quaternion quat;
// tf::createQuaternionMsgFromRollPitchYaw(double r, double p, double y);//返回四元数
pose1<< laserOdometryROS.header.stamp -T1<< " "<< -laserOdometryROS.pose.pose.position.x << " "<<- laserOdometryROS.pose.pose.position.z << " "<< -laserOdometryROS.pose.pose.position.y<< " "<< laserOdometryROS.pose.pose.orientation.x << " "<< laserOdometryROS.pose.pose.orientation.y << " "<< laserOdometryROS.pose.pose.orientation.z << " "<< laserOdometryROS.pose.pose.orientation.w << std::endl;
pose1.close();

遇到的问题：lio_sam跑kitti总跑飞，Large velocity, reset IMU-preintegration!

解决方法：在参数配置正确的情况下，降低bag播放速度。（找这个问题花了2天时间！）