文章目录
一、设备与运行结果
![](https://i-blog.csdnimg.cn/blog_migrate/2e4076f3f25888456a4a4d2f561a7f24.jpeg)
![](https://i-blog.csdnimg.cn/blog_migrate/e9018ad9e8a29fcb6faf16088ee8b924.jpeg)
![](https://i-blog.csdnimg.cn/blog_migrate/bf64cff1d4773d1c353bb04a52060fd2.jpeg)
二、R3LIVE基本介绍
R3LIVE项目地址 https://github.com/hku-mars/r3live
- R3LIVE是港大Mars实验室提出的一种实时鲁棒、带有RGB颜色信息的激光雷达-惯性-视觉紧耦合系统。
- R3LIVE包含雷达惯性里程计LIO与视觉惯性里程计VIO两个子系统:其中LIO系统利用激光雷达和惯导构建全局点云地图,而VIO系统利用RGB图像和惯导来进行点云地图颜色渲染
- 需注意R3LIVE与R2LIVE使用了相似的数据流模式,调试时不需要额外进行数据帧的时间对齐
三、编译R3LIVE
第1-4节为前置环境配置,第5节开始编译项目
1)ROS安装
- 在本例中使用的是ROS noetic版本
建议参考奥特学园的ROS教程自行安装或使用小鱼的一键安装ROS
奥特学院ROS教程 http://www.autolabor.com.cn/book/ROSTutorials/
小鱼一键安装ROS https://fishros.org.cn/forum/topic/20/小鱼的一键安装系列?lang=zh-CN
此外按照作者文档,除了ROS的基本模块,还需安装以下package
sudo apt-get install ros-XXX-cv-bridge ros-XXX-tf ros-XXX-message-filters ros-XXX-image-transport ros-XXX-image-transport*
其中,ros-XXX-cv-bridge
中的XXX
指所安装的ROS版本分支,本例中使用noetic版本即为ros-noetic-cv-bridge
2)livox_ros_driver
- livox原驱动中设置lidar、imu数据的时间戳均为基于雷达启动的时间开始的,而非基于系统的时间,而摄像头(包括本例中的RealSense)数据的时间戳通常是基于系统的
- 如果只是跑作者的数据集,便不需要改livox的ROS驱动;如果要跑自己的数据,就需要参考作者提供的修改方式进行处理
大疆Livox驱动安装 https://github.com/Livox-SDK/livox_ros_driver
作者修改的Livox驱动 https://github.com/ziv-lin/livox_ros_driver_for_R2LIVE
livox_ros_driver 安装步骤:
1、克隆仓库并建立工作空间
git clone https://github.com/Livox-SDK/livox_ros_driver.git ws_livox/src
2、进入工作空间并编译
cd ws_livox
catkin_make
3、设置环境变量
# 临时设置
source ./devel/setup.sh
# 永久设置(注意路径需根据情况调整)
echo "source ws_livox/src/devel/setup.sh" >> ~/.bashrc
source ~/.bashrc
3)CGAL and pcl_viewer(可选)
- CGAL为计算几何算法库,pcl_viewer为点云查看工具
sudo apt-get install libcgal-dev pcl-tools
生成的点云地图可通过pcl_viewer
命令查看
pcl_viewer (map_name).pcd
4)OpenCV(版本要求高于 3.3)
-
如果ubuntu系统未安装opencv,建议安装
3.3.1
,3.4.16
,4.2.1
,4.5.3
这几个已经测试没问题的版本,本例中使用4.5.2
版本也能正常运行,建议源码编译 -
ros-noetic-full安装时带有4版本的opencv,但如果装ros前已有opencv可能会导致新装的opencv不完整,编译时会出现链接库不一致的问题(Warning: /usr/bin/ld)
5)在ROS上编译R3LIVE
- 环境配置正确情况下,仅需克隆仓库并编译即可
cd ~/catkin_ws/src
git clone https://github.com/hku-mars/r3live.git
cd ../
catkin_make
source ~/catkin_ws/devel/setup.bash
然而实际测试时仍然可能会遇到一些问题,在本例中编译出现过两次问题
- opencv版本选择问题:
当系统中存在多个opencv时,不建议贸然删除某个版本,可通过cmake进行版本管理
find_package(OpenCV 4.5.2 REQUIRED) #选择opencv 4.5.2版本
![](https://i-blog.csdnimg.cn/blog_migrate/bdb0cf8c0918f75d6f5dabdc3ba7f0aa.png)
对自己编译的opencv则可以设置具体的包路径,要求给出能找到OpenCVConfig.cmake或OpenCVConfig-version.cmake
set(OpenCV_DIR ~/opencv/opencv-4.5.2/build/) #本例中opencv的cmake路径
![](https://i-blog.csdnimg.cn/blog_migrate/477202e7284de823e02ec33c706fb348.png)
- 初始cv_bridge包导致opencv库链接问题:
在编译最末尾提示warning:/usr/bin/ld
相关的警告,提示libopencv的某个库存在不同版本可能在运行时产生冲突,可能导致运行时程序突然卡死
这是因为编译库和运行库不一致导致的问题,通常是cv_bridge编译时用了系统默认的某个版本的opencv库造成的
只需要卸载现有的cv_bridge,再下载源码用我们想要的opencv版本编译安装即可解决
sudo apt-get remove ros-${ROS_DISTRO}-cv-bridge #卸载原有cv_bridge
git clone https://github.com/ros-perception/vision_opencv.git
然后在cv_bridge
包中的CMakeLists.txt
中设为自己需要的opencv
版本再编译即可解决
![](https://i-blog.csdnimg.cn/blog_migrate/51ff344dbc512825735beec5372426ce.png)
四、使用作者提供的数据集运行R3LIVE
R3LIVE数据集地址 https://github.com/ziv-lin/r3live_dataset
![](https://i-blog.csdnimg.cn/blog_migrate/0dcbccef55ce3d1d212f705d88d6c19e.png)
- 作者提供的数据集文件均为rosbag文件,用google drive或百度网盘下载数据集后,在数据集文件对应文件夹下打开终端,可通过以下命令播放rosbag,其中
XXX.bag
为你要播放的bag名称,如图中hku_park_01.bag
rosbag play --pause XXX.bag #以可暂停方式播放bag
其他选项:--topic /topic1 /topic2 指定播放话题 | -r (rate) 指定播放速度倍率
- 确保R3LIVE编译没问题,以及livox_ros_driver等包的工作空间变量已刷新
roslaunch r3live r3live_bag.launch #启动里程计
- 播放数据包
rosbag play --pause hku_campus_seq_00.bag
运行后会先通过IMU数据进行初始化,下方的Dashboard会显示当前处理的雷达和图像帧数
![](https://i-blog.csdnimg.cn/blog_migrate/53f33349efd59544387b9ae7d22c8c70.png)
![](https://i-blog.csdnimg.cn/blog_migrate/532412cd84fe3e091b1fdbb99d82cb0c.png)
五、使用自己的数据集运行R3LIVE
1)数据包录制
- livox_ros_driver设置其lidar、imu的时间戳均基于雷达启动时间开始,而非基于系统的时间,而摄像头数据的时间戳大多基于系统时间,不修改则运行时无法找到对应的lidar、camera帧
- 本例中使用RealSense提供imu与camera,仅需修改lidar的ros驱动
通常查看imu等单帧数据量比较小的消息,直接终端rostopic echo [TOPIC]
即可,但是对于相机、雷达等单帧数据量很大的消息可能终端消息会塞满,看不到时间戳,所以可以输出保存成txt文件,方便我们检查时间戳
举个例子,比如说雷达消息的话题名是/livox/lidar
,你想输出保存成一个叫做my_lidar_data.txt
的文件,你的数据包名字是my_lidar_record.bag
,那么命令就是
rostopic echo -b my_lidar_record.bag /livox/lidar >> my_lidar_data.txt
- 建议执行一小会就
Ctrl C
停掉,不然输出来文件太大不好打开,只能用VSCode打开
![](https://i-blog.csdnimg.cn/blog_migrate/1477b343195e43a05f4a54e31ed44c5a.png)
根据R3LIVE作者提供的时间戳修改方式,我们可以发现作者主要修改了livox_ros_driver
中的lddc.cpp
文件,并且是在IMU消息的发布函数中进行时间初始化,记录了雷达开始发布消息时的ROS系统时间和相对于雷达启动的时间偏移量,略过了起始的100帧数据,初始化后在IMU和点云消息发布函数中进行处理即可(作者修改的是自定义消息CustomPointcloud那个函数)
但是如果使用的Livox雷达不具有IMU,比如本例中使用的mid-70,那么就需要做一些小小的修改
具体的修改方式已经整理在
https://github.com/Cyberkona/SLAM-Tools/tree/main/livox_ros_driver_with_ros_timestamp_for_mid70
,按照R3LIVE作者提供的修改方式,对livox_ros_driver
中lddc.cpp
文件进行修改
- 首先在
lddc.cpp
文件开头,加入用于记录起始时间的相关变量
/****** Modified: add variables to change timestamp ******/
double g_ros_init_start_time = -3e8;
double init_lidar_tim = 3e10;
double init_ros_time = 0;
double skip_frame = 10;
/*********************************************************/
- 如果需要将livox自定义消息类型的时间戳转换过来,那么需要修改
lddc.cpp
中的PublishCustomPointcloud
函数,在消息Publish之前加入
/****** Modified: Setup ros timestamp to system base ******/
if (1)
{
if (skip_frame)
{
skip_frame--;
init_ros_time = ros::Time::now().toSec();
init_lidar_tim = timestamp;
g_ros_init_start_time = timestamp;
ROS_INFO("========================");
ROS_INFO("Init time stamp = %lf", g_ros_init_start_time);
ROS_INFO("========================");
}
// change timestamp in function PublishPointcloud2
cloud.header.stamp = ros::Time((timestamp - init_lidar_tim) / 1e9 + init_ros_time);
}
/**********************************************************/
- 如果需要将PointCloud2消息类型的时间戳转换过来,那么需要修改
lddc.cpp
中的PublishPointcloud2
函数,在消息Publish之前加入
/****** Modified: Setup ros timestamp to system base ******/
if (1)
{
if (skip_frame)
{
skip_frame--;
init_ros_time = ros::Time::now().toSec();
init_lidar_tim = timestamp;
g_ros_init_start_time = timestamp;
ROS_INFO("========================");
ROS_INFO("Init time stamp = %lf", g_ros_init_start_time);
ROS_INFO("========================");
}
// change timestamp in function PublishPointcloud2
cloud.header.stamp = ros::Time((timestamp - init_lidar_tim) / 1e9 + init_ros_time);
}
/**********************************************************/
- 修改后,通过catkin_make编译,连上雷达,开启你需要的launch文件,如果修改成功可以看到终端输出
Init time stamp = (时间戳信息)
,该消息数量为skip_frame
,之后可通过上述rostopic
命令检查数据时间戳 - 最后可以通过
rosbag record
命令录制所需的话题成一数据包,示例如下,推荐图像选择录制压缩的话题,不然数据包太大
rosbag record /livox/lidar /camera/imu /camera/color/image_raw/compressed -o my_data.bag
2)IMU坐标系调整
- 在R3LIVE中,由于使用MID-40雷达具有内置IMU,因而默认IMU与点云数据是相同的坐标系,当我们使用外置IMU时,则需要注意IMU坐标系是否雷达点云的坐标系匹配
对于ROS中的里程计,使用的是右手系,通常X轴指向机器人向前的运动方向,Z轴指向上方,Y轴则按照右手系可以进行判断
- 本例中使用RealSense输出IMU,由于安装方式导致IMU与雷达坐标系不匹配,可以通过ROS中的
imu_transformer
功能包进行修改,这个包需要自行下载编译
imu_pipeline
功能包集:https://github.com/ros-perception/imu_pipeline
描述:IMU Pipeline metapackage includes tools for processing and pre-processing IMU messages for easier use by later subscribers.
- 对于
imu_transformer
功能包,注意修改其中imu_transformer_node.cpp
文件中的话题名,以及启动文件ned_to_enu.launch
中的静态变换与target_frame字段;需要注意的是,建议修改静态变换所使用的tf
包至tf2_ros
包!!!,能够让数据转换更加稳定(说多了都是泪TAT)
![](https://i-blog.csdnimg.cn/blog_migrate/40f4cbe83487310b1cee36bac76eee02.png)
- 使用时,先启动
imu_transformer
的转换程序,然后可以播包或者跑实机,运行正常则会输出一个新的IMU话题,推荐实机跑的时候可以录下转换好的IMU数据,以便下一次复用
3)相机内参修改
可以查找与相机标定有关的内容,在
config/r3live_config.yaml
文件中修改
(待补充)
4)实机运行R3LIVE
- 修改
r3live_bag.launch
中订阅的IMU及图像的话题名,使用RealSense提供图像与IMU话题,则图像话题修改为/camera/color/image_raw
,IMU话题修改为/camera/imu
,本例中mid-70雷达点云话题名为默认的/livox/lidar
,不需要修改;如需要换用雷达可以更改参数文件里的lidar_type
,话题名不一致可以重映射remap
或者修改前端的接口函数重新编译
![](https://i-blog.csdnimg.cn/blog_migrate/7401709ccff12d8daed37ab247958b90.png)
- 启动
r3live_bag.launch
,如果需要IMU坐标系转换的话则启动ned_to_enu.launch
,最后播放数据包即可运行
![](https://i-blog.csdnimg.cn/blog_migrate/6ea51fe4767d127d296110d5929f8f0e.png)