【激光】SLAM相关工具

于 2025-03-18 15:41:01 发布
阅读量520 收藏 9
点赞数 7
文章标签： 3d
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.youkuaiyun.com/qq_46842599/article/details/146329387
版权

SLAM相关工具

KITTI数据集

序列对应

00: 2011_10_03_drive_0027
01: 2011_10_03_drive_0042
02: 2011_10_03_drive_0034
03: 2011_09_26_drive_0067
04: 2011_09_30_drive_0016
05: 2011_09_30_drive_0018
06: 2011_09_30_drive_0020
07: 2011_09_30_drive_0027
08: 2011_09_30_drive_0028
09: 2011_09_30_drive_0033
10: 2011_09_30_drive_0034

00-21sequence的相机内参

● 00sequence
f x = 718.8560 ，c x = 607.1928 ，f y = 718.8560 ，c y = 185.2157 和b = 0.5372
● 01sequence
f x = 718.8560 ，c x = 607.1928 ，f y = 718.8560 ，c y = 185.2157 和b = 0.5372
● 02sequence
f x = 718.8560 ，c x = 607.1928 ，f y = 718.8560 ，c y = 185.2157 和b = 0.5372

● 03sequence
f x = 721.5377 ，c x = 609.5593 ，f y = 721.5377 ，c y = 183.1104 和b = 0.5372

● 04sequence
f x = 707.0912 ，c x = 601.8873 ，f y = 707.0912 ，c y = 172.8540 和b = 0.5372
● 05sequence
f x = 707.0912 ，c x = 601.8873 ，f y = 707.0912 ，c y = 183.1104 和b = 0.5372
● 06sequence
f x = 707.0912 ，c x = 601.8873 ，f y = 707.0912 ，c y = 183.1104 和b = 0.5372
● 07sequence
f x = 707.0912 ，c x = 601.8873 ，f y = 707.0912 ，c y = 183.1104和b = 0.5372
● 08sequence
f x = 707.0912 ，c x = 601.8873 ，f y = 707.0912 ，c y = 183.1104 和b = 0.5372
● 09sequence
f x = 707.0912 ，c x = 601.8873 ，f y = 707.0912 ，c y = 183.1104 和b = 0.5372
● 10sequence
f x = 707.0912 ，c x = 601.8873 ，f y = 707.0912 ，c y = 183.1104 和b = 0.5372
● 11sequence
f x = 707.0912 ，c x = 601.8873 ，f y = 707.0912 ，c y = 183.1104 和b = 0.5372
● 12sequence
f x = 707.0912 ，c x = 601.8873 ，f y = 707.0912 ，c y = 183.1104 和b = 0.5372

● 13sequence
f x = 718.8560 ，c x = 607.1928 ，f y = 718.8560 ，c y = 185.2157 和b = 0.5372
● 14sequence
f x = 718.8560 ，c x = 607.1928 ，f y = 718.8560 ，c y = 185.2157 和b = 0.5372
● 15sequence
f x = 718.8560 ，c x = 607.1928 ，f y = 718.8560 ，c y = 185.2157 和b = 0.5372
● 16sequence
f x = 718.8560 ，c x = 607.1928 ，f y = 718.8560 ，c y = 185.2157 和b = 0.5372
● 17sequence
f x = 718.8560 ，c x = 607.1928 ，f y = 718.8560 ，c y = 185.2157 和b = 0.5372
● 18sequence
f x = 718.8560 ，c x = 607.1928 ，f y = 718.8560 ，c y = 185.2157 和b = 0.5372
● 19sequence
f x = 718.8560 ，c x = 607.1928 ，f y = 718.8560 ，c y = 185.2157 和b = 0.5372
● 20sequence
f x = 718.8560 ，c x = 607.1928 ，f y = 718.8560 ，c y = 185.2157 和b = 0.5372
● 21sequence
f x = 718.8560 ，c x = 607.1928 ，f y = 718.8560 ，c y = 185.2157 和b = 0.5372

ALOAM跑kitti数据集

A-LOAM运行KITTI数据集需要运行kittiHelper节点
在自己的源码仓库里有文件
在A-LOAM的源码中有该程序，launch文件如下

<launch>
    <arg name="number" default="05" />
    <arg name="output_file" default="/home/john/rosbag/kitti_rosbag" />
    <!-- kittiHelper是使用kitti数据集的一个启动launch -->
    <node name="kittiHelper" pkg="aloam_velodyne" type="kittiHelper" output="screen">
        <!-- 数据集文件夹位置：${HOME}是~文件夹 -->
        <param name="dataset_folder" type="string" value="/home/john/rosbag/kitti_origin/dataset/" />
        <!-- 顺序数字 -->
        <param name="sequence_number" type="string" value="$(arg number)" />
        <!-- 计算轨迹的同时打包成bag输出 -->
        <!-- <param name="to_bag" type="bool" value="false" /> -->
        <param name="to_bag" type="bool" value="true" />
        <!-- 输出为bag的位置，需采用绝对路径 -->
        <param name="output_bag_file" type="string" value="$(arg output_file)/kitti_$(arg number).bag" /> 
        <!-- replace with your output folder -->
        <!-- <param name="output_bag_file" type="string" value="/home/john/rosbag/kitti_rosbag/kitti_00.bag" />  -->
        <!-- 发布延迟 -->
        <param name="publish_delay" type="int" value="1" />
    </node>
</launch>

cpp文件可以不做修改，主要是发布的"/camera_init"改为"camera_init"的小毛病，不影响观看

kitti数据集结构：

├── poses
│   ├── 00.txt
│   ├── ...
└── sequences
    ├── 00
    │   ├── calib.txt
    │   ├── image_0
    │   │   ├── 000000.png
    │   │   ├── ...
    │   ├── image_1
    │   │   ├── 000000.png
    │   │   ├── ...
    │   ├── image_2
    │   │   ├── 000000.png
    │   │   ├── ...
    │   ├── image_3
    │   │   ├── 000000.png
    │   │   ├── ...
    │   ├── times.txt
    │   ├── tum_00_gt.txt
    │   ├── velodyne
    │   │   ├── 000000.bin
    │   │   ├── ...
    ├── ...

修改好之后，运行launch文件，打开rviz逐帧显示KITTI数据集：

roslaunch aloam_velodyne kitti_helper.launch

接下来运行SLAM：

roslaunch aloam_velodyne kitti_helper.launch
roslaunch aloam_velodyne aloam_velodyne_HDL_64.launch

aloam跑图的结果
aloam跑图的结果
修改launch文件，自动生成rosbag：

<!-- <param name="to_bag" type="bool" value="false"/> -->
<param name="to_bag" type="bool" value="true"/>
<param name="output_bag_file" type="string" value="/tmp/kitti.bag" />
<!-- replace with your output folder -->

未包含IMU的数据集信息：
kitti转成的rosbag数据信息

LIO-SAM跑KITTI数据集

如果要保存点云及轨迹等结果，在参数文件params.yaml中打开并修改保存路径:

savePCD: true
savePCDDirectory: "/Downloads/LIOSAM/"

park数据集可验证GPS松组合效果，如果要使用GPS数据，则需要进行如下修改：

gpsTopic: "odometry/gpsz"  改为 gpsTopic: "odometry/gps"  
useImuHeadingInitialization: false 改为 useImuHeadingInitialization: true

kitti2bag工具输出的bag文件LIO-SAM是不能跑的，IMU频率跟雷达一样很低频，无法满足代码需求，而且没有ring和time标签。LIO-SAM作者自己改了kitti2bag就在源码的文件夹LIO-SAM/config/doc/kitti2bag下，你需要准备如下文件（文件位置需对应）：

├── 2011_09_30_drive_0018_extract
│   ├── image_00
│   ├── image_01
│   ├── image_02
│   ├── image_03
│   ├── oxts
│   └── velodyne_points
├── 2011_09_30_drive_0018_sync
│   ├── image_00
│   ├── image_01
│   ├── image_02
│   ├── image_03
│   ├── oxts
│   └── velodyne_points
├── calib_cam_to_cam.txt
├── calib_imu_to_velo.txt
└── calib_velo_to_cam.txt

下载使用wget：

wget https://s3.eu-central-1.amazonaws.com/avg-kitti/raw_data/2011_09_30_calib.zip
wget https://s3.eu-central-1.amazonaws.com/avg-kitti/raw_data/2011_09_30_drive_0018/2011_09_30_drive_0018_sync.zip
wget https://s3.eu-central-1.amazonaws.com/avg-kitti/raw_data/2011_09_30_drive_0018/2011_09_30_drive_0018_extract.zip

解压后就是上述文件夹层级
可以参考下文的evo工具
然后，在2011_09_30文件夹的上一级目录，即kitti2bag.py文件下打开终端，输入：

python3 kitti2bag.py -t 2011_09_30 -r 0018 raw_synced

运行结果（注意文件目录结构一定按照上面的要求来，否则无法运行）：
包含IMU信息的数据集
带有IMU的KITTI数据集
参数文件配置，主要修改以下两个文件的内容：

# params_kitti.yaml

# Topics
pointCloudTopic: "/points_raw"               # Point cloud data
imuTopic: "/imu_raw"                         # IMU data

# KITTI
sensor: velodyne                            # lidar sensor type, either 'velodyne' or 'ouster'
N_SCAN: 64                                  # number of lidar channel (i.e., 16, 32, 64, 128)
Horizon_SCAN: 2083                          # lidar horizontal resolution (Velodyne:1800, Ouster:512,1024,2048)
downsampleRate: 1                           # 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

# IMU内参影响不大，这里也没改

# kitti外参
# extrinsicTrans、extrinsicRot表示imu -> lidar的坐标变换
extrinsicTrans: [-8.086759e-01, 3.195559e-01, -7.997231e-01]
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用于旋转imu坐标系下的欧拉角（姿态信息）到lidar坐标下，由于lio-sam作者使用的imu的欧拉角旋转指向与lidar坐标系不一致（左手系），因此使用了两个不同的旋转矩阵，但是大部分的设备两个旋转应该是设置为相同的
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]

<!-- run_kitti.launch -->
<!-- Parameters -->
<rosparam file="$(find lio_sam)/config/params_kitti.yaml" command="load" />

运行：

roslaunch lio_sam run.launch
rosbag play kitti_2011_09_30_drive_0033_synced.bag

lio-sam的跑图结果：
lio-sam的运行结果

封面照片

evo工具

evo程序github地址
使用KITTI跑LIOSAM并完成EVO评价
下载前10个序列的raw data（有真值）
00: 2011_10_03_drive_0027
01: 2011_10_03_drive_0042
02: 2011_10_03_drive_0034
03: 2011_09_26_drive_0067
04: 2011_09_30_drive_0016
05: 2011_09_30_drive_0018
06: 2011_09_30_drive_0020
07: 2011_09_30_drive_0027
08: 2011_09_30_drive_0028
09: 2011_09_30_drive_0033
10: 2011_09_30_drive_0034
下载calib，extract，sync的数据，默认解压后的目录存放格式如下，就可以
kitti数据集的文件格式

在“2011_09_30”的文件目录，运行：

kitti2bag -t 2011_09_30 -r 0018 raw_synced

行后会出现“kitti_2011_09_30_drive_0018_synced.bag”
注意：不要妄想通过路径选择“2011_09_30”文件夹
需安装kitti2bag

pip install pykitti
pip install kitti2bag

sudo pip install pandas==0.23.0 -i http://pypi.douban.com/simple/ --trusted-host pypi.douban.com
sudo pip install pykitti -i http://pypi.douban.com/simple --trusted-host pypi.douban.com
sudo pip install kitti2bag  -i http://pypi.douban.com/simple --trusted-host pypi.douban.com

如果使用LIO-SAM作者的kitti2bag.py文件：

kitti2bag源程序

在liosam源码中：
config/doc/kitti2bag/kitti2bag.py

python3 kitti2bag.py -t 2011_09_30 -r 0018 raw_synced

测试的lio-sam修改后代码地址：
https://github.com/wwtx/lio_sam_kitti.git
需修改mapOptmization.cpp中的路径

std::ofstream pose2("/home/john/liosam_ws/save_trajectory/pose2.txt", std::ios::app);
std::ofstream pose1("/home/john/liosam_ws/save_trajectory/pose1.txt", std::ios::app);

运行：

source devel/setup.bash
roslaunch lio_sam kitti_run.launch

运行Rosbag的数据集：

rosbag play kitti_2011_09_30_drive_0018_synced.bag

结果：
转换为lio-sam可用的kitti数据集

source devel/setup.bash
rosservice call \
/lio_sam/save_map 0.2 \
"/liosam_ws/save_map/"

kitti数据集真值
安装evo
evo的安装可以参考evo安装与使用和EVO安装与问题解决，这里介绍快捷安装：

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

首先找到evo中的转换真值的程序
下载evo
在evo/contrib/kitti_poses_and_timestamps_to_trajectory.py
知道对应序列：
05: 2011_09_30_drive_0018

python3 \
kitti_poses_and_timestamps_to_trajectory.py \
poses/05.txt \
sequences/05/times.txt \
results/tum_05_gt.txt

查看路径：

evo_traj tum results/tum_05_gt.txt  -p

10个全部转换的SHELL脚本

for i in {0..10}
do
	if [ $i -le 9 ]; then
# python3 kitti_poses_and_timestamps_to_trajectory.py poses/0$i.txt sequences/0$i/times.txt sequences/0$i/tum_0${i}_gt.txt
python3 kitti_poses_and_timestamps_to_trajectory.py poses/0$i.txt sequences/0$i/times.txt results/tum_0${i}_gt.txt
	else
# python3 kitti_poses_and_timestamps_to_trajectory.py poses/$i.txt sequences/$i/times.txt sequences/$i/tum_${i}_gt.txt
python3 kitti_poses_and_timestamps_to_trajectory.py poses/$i.txt sequences/$i/times.txt results/tum_${i}_gt.txt
	fi
done

APE评价

evo_ape tum \
/home/john/rosbag/kitti_origin/dataset/results/tum_05_gt.txt \
/home/john/liosam_ws/save_trajectory/pose1.txt \
-r trans_part -va --plot --plot_mode xz \
--save_plot /home/john/rosbag/kitti_origin/dataset/plot_results/05/APE_plot.pdf \
--save_results /home/john/rosbag/kitti_origin/dataset/save_results/05/APE_result.zip

● evo_rpe：evo 工具包的一个子命令，用于计算相对姿态误差 (Relative Pose Error, RPE)，这是分析SLAM/VO轨迹与参考轨迹之间误差的常用指标。
● tum：输入文件格式的类型，tum 表示 TUM 格式的轨迹文件。
● -r trans_part：指定计算RPE时使用的误差类型。在这里使用 trans_part，意味着只计算平移部分的误差，而不考虑旋转误差。
-r 参数用于指定误差计算的类型，默认情况下通常是 full_trans，也就是平移和旋转误差都会计算。
trans_part：仅计算平移误差（translation part），忽略旋转误差。
rot_part：仅计算旋转误差（rotation part），忽略平移误差。
full_trans（默认）：计算平移和旋转误差，这是平移和旋转的组合误差。
● -va：表示启用详细模式 (verbose mode) 和所有统计信息 (all statistics)，这将输出更详细的评估信息。
● --plot：生成轨迹的误差图表。
● --plot_mode xz：设置绘图模式为 xz 平面，通常这是俯视平面的轨迹显示模式，可以更直观地看到平面内的运动误差。
● --save_plot：指定保存生成的图表的文件路径。
● --save_results：指定保存分析结果的路径。

RPE评价

evo_rpe tum \
/home/john/rosbag/kitti_origin/dataset/results/tum_05_gt.txt \
/home/john/liosam_ws/save_trajectory/pose1.txt \
-r trans_part -va --plot --plot_mode xz \
--save_plot /home/john/rosbag/kitti_origin/dataset/plot_results/05/RPE_plot.pdf \
--save_results /home/john/rosbag/kitti_origin/dataset/save_results/05/RPE_result.zip

在 evo_ape 命令中，-r 参数用于指定误差类型，一般适用于整体轨迹的误差评估：
● rmse：计算均方根误差。
● mean：计算均值误差。
● median：计算中位数误差。
● std：计算标准差。
● min：计算最小误差。
● max：计算最大误差。
使用发现：evo_ape和evo_rpe的-r参数相同：
● -r full表示同时考虑旋转和平移误差得到的ape,无单位（unit-less）
● -r trans_part表示考虑平移部分得到的ape，单位为m；
● -r rot_part表示考虑旋转部分得到的ape，无单位（unit-less）；
● -r angle_deg表示考虑旋转角得到的ape,单位°（deg）；
● -r angle_rad表示考虑旋转角得到的ape,单位弧度（rad）;

RES评价

evo_res \
/home/john/rosbag/kitti_origin/dataset/save_results/05/*.zip \
-p --use_filenames \
--save_table /home/john/rosbag/kitti_origin/dataset/table_results/05/RES_table.csv

–save_table /home/john/rosbag/kitti_origin/dataset/table_results/05/RES_table.csv
它不使用 -r 参数。
evo_res 主要依赖输入的结果文件并以指定格式输出。

轨迹输出

evo_traj tum \
/home/john/rosbag/kitti_origin/dataset/results/tum_05_gt.txt \
/home/john/liosam_ws/save_trajectory/pose1.txt \
--ref=/home/john/rosbag/kitti_origin/dataset/results/tum_05_gt.txt \
-p -a --plot_mode xz \
--save_plot /home/john/rosbag/kitti_origin/dataset/plot_results/05/TRA_plot.pdf

并不使用 -r 参数。

Greary
关注
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值