目录
一、工作空间的创建，功能包的编译等等
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博客
使用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 cloud
std::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.039569
f << 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但是，这种方式不方便数据分析，需要以一定格式存储。
参考链接：
三、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.
执行命令后报错：
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 author
pointCloudTopic: "/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 IMU
gpsTopic: "odometry/gpsz" # GPS odometry topic from navsat, see module_navsat.launch file
# Sensor Settings
sensor: 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 / 1
lidarMinRange: 1.0 # default: 1.0, minimum lidar range to be used
lidarMaxRange: 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坐标系不一致，因此使用了两个不同的旋转矩阵，但是大部分的设备两个旋转应该是设置为相同的。
5.2、修改代码（产生ring、time、保存tum格式的轨迹）
utility.h文件修改：
// add by sx
bool 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 sx
nh.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 orientationDiffimageProjection.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 projection
for (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天时间！）
rosbag play your_bag.bag -r 0.1 //以0.1倍速播放bag包
使用KITTI跑LIOSAM并完成EVO评价_小霍金的博客-CSDN博客
使用开源激光SLAM方案LIO-SAM运行KITTI数据集，如有用，请评论雷锋_我兔会飞的博客-CSDN博客
LIO-SAM：配置环境、安装测试、适配自己采集数据集_有待成长的小学生的博客-CSDN博客