目录
一、工作空间的创建,功能包的编译等等
二、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 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
但是,这种方式不方便数据分析,需要以一定格式存储。
参考链接:
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 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坐标系不一致,因此使用了两个不同的旋转矩阵,但是大部分的设备两个旋转应该是设置为相同的。在Ubuntu20.04系统上LIO-SAM跑KITTI数据集和自己数据集代码修改_学无止境的小龟的博客-CSDN博客
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 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 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博客文章来源:https://www.toymoban.com/news/detail-454396.html
LIO-SAM:配置环境、安装测试、适配自己采集数据集_有待成长的小学生的博客-CSDN博客文章来源地址https://www.toymoban.com/news/detail-454396.html
到了这里,关于lego_loam、lio_sam运行kitti(完成kitti2bag、evo测试)的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!