UltimateSLAM编译运行调试记录

本文最后更新于 2024年3月21日 下午

本文主要分享了自己在编译运行论文《Ultimate SLAM? Combining Events, Images, and IMU for Robust Visual SLAM in HDR and High Speed Scenarios》的源代码时的调试记录,下面简称为UltimateSLAM。

UltimateSLAM

此存储库包含运行UltimateSLAM的代码,如下两篇论文所述:

在此处查看演示视频

如果您在学术背景下使用此代码,请引用以下作品:

1
2
3
4
5
6
@InProceedings{Rosinol_2018_RAL,
author = {Antoni Rosinol Vidal and Henri Rebecq and Timo Horstschaefer and Davide Scaramuzza},
title = {Ultimate SLAM? Combining Events, Images, and IMU for Robust Visual SLAM in HDR and High Speed Scenarios},
booktitle = {{IEEE} Robotics and Automation Letters (RA-L)},
year = {2018}
}
1
2
3
4
5
6
@InProceedings{Rebecq_2017_BMVC,
author = {Henri Rebecq and Timo Horstschaefer and Davide Scaramuzza},
title = {Real-time Visual-Inertial Odometry for Event Cameras using Keyframe-based Nonlinear Optimization},
booktitle = {British Machine Vision Conference (BMVC)},
year = {2017}
}

动机

事件相机是受生物启发的视觉传感器,输出像素级亮度变化而不是标准强度帧。事件相机不受运动模糊的影响,并且具有非常高的动态范围,这使它们能够在高速运动或高动态范围的场景中提供可靠的视觉信息。然而,当运动量有限时,如在几乎静止的运动中,事件相机只输出少量信息。相反,在低速和良好照明的情况下,标准相机大部分时间提供即时丰富的环境信息,但在快速运动或困难照明(如高动态范围或低光场景)的情况下,它们严重失败。UltimateSLAM是第一个利用这两种传感器的互补优势的状态估计管道,通过紧密耦合方式融合事件、标准帧和惯性测量。在高速和高动态范围场景中,UltimateSLAM的准确性提高了高达85%,与标准基于帧的视觉-惯性测距系统相比,可以在嵌入式平台上实时运行。我们已经展示,UltimateSLAM可用于低光环境下的自主无人机飞行,甚至在一旋翼失效时保持无人机飞行(视频)。

内容

UltimateSLAM的安装

此安装指南已在Ubuntu 16.04和Ubuntu 18.04上测试。

要求

对于Ceres,您需要安装以下包:

1
sudo apt install liblapack-dev libblas-dev

安装

首先,我们需要为UltimateSLAM创建一个catkin工作空间并初始化它:

1
2
3
cd yourfolder
mkdir -p uslam_ws/src && cd uslam_ws
catkin init

然后,我们配置我们的工作空间以扩展ROS基础工作空间,并默认以发布模式(带优化)编译。请将kinetic替换为您的ROS版本(例如melodic)。

1
2
catkin config --extend /opt/ros/kinetic --cmake-args -DCMAKE_BUILD_TYPE=Release
# catkin config --extend /opt/ros/melodic --cmake-args -DCMAKE_BUILD_TYPE=Release

克隆UltimateSLAM存储库:

1
2
cd src/
git clone git@github.com:uzh-rpg/rpg_ultimate_slam_open.git

运行vcstool自动导入依赖项:

1
vcs-import < rpg_ultimate_slam_open/dependencies.yaml

下载的第三方库在./catkin_ws/src/目录下,与rpg_ultimate_slam_open文件夹同级。

为防止网路下载不稳定的情况,可以将第三方库CmakeLists.txt中的下载链接替换为本地路径。

注:URL可以直接使用本地地址,但是文件必需为压缩文件,编译时会自动解压。官方文档中文解读1中文解读2

  1. gflags_catkin。下载v2.2.1.zipyourfolder/并重命名为gflags-2.2.1.zip。打开./uslam_ws/src/gflags_catkin目录下的Cmakelists.txt文件,修改URL

    1
    2
    # URL https://github.com/gflags/gflags/archive/v2.2.1.zip
    URL "/yourfolder/gflags-2.2.1.zip"
  2. 待解决suitesparse。下载SuiteSparse-4.2.1.tar.gzyourfolder/。打开./uslam_ws/src/suitesparse/suitesparse目录下的Cmakelists.txt文件,修改DOWNLOAD_COMMAND

    1
    2
    # DOWNLOAD_COMMAND rm -f SuiteSparse-${VERSION}.tar.gz && wget --retry-connrefused --waitretry=1 --timeout=40 --tries 3 https://github.com/ethz-asl/thirdparty_library_binaries/raw/master/SuiteSparse-${VERSION}.tar.gz
    URL "/yourfolder/SuiteSparse-${VERSION}.tar.gz"

    上述方法不可行,下面的方法可行:

    打开./uslam_ws/src/suitesparse/suitesparse目录下的Cmakelists.txt文件,修改DOWNLOAD_COMMAND

    1
    2
    # DOWNLOAD_COMMAND rm -f SuiteSparse-${VERSION}.tar.gz && wget --retry-connrefused --waitretry=1 --timeout=40 --tries 3 https://github.com/ethz-asl/thirdparty_library_binaries/raw/master/SuiteSparse-${VERSION}.tar.gz
    DOWNLOAD_COMMAND ""

    复制SuiteSparse-4.2.1.tar.gz./catkin_ws/src/

    1
    2
    3
    4
    cd ./catkin_ws/src/
    # 在catkin build前运行命令:
    mkdir -p build/suitesparse/suitesparse_src-prefix/src && cp SuiteSparse-4.2.1.tar.gz ./build/suitesparse/suitesparse_src-prefix/src
    # find -name SuiteSparse-4.2.1.tar.gz
  3. ceres_catkin。下载ceres-solver-1.14.0.tar.gzyourfolder/。打开./uslam_ws/src/ceres_catkin目录下的Cmakelists.txt文件,修改GIT_REPOSITORY

    由于ceres库和Eigen库有一定的版本对应关系,因此很容易在编译期间报错。实测Eigen3.3.x与ceres-solver-1.14.0对应应该没有问题。

    1
    2
    3
    4
    # GIT_REPOSITORY https://github.com/ceres-solver/ceres-solver.git
    # GIT_TAG ${VERSION}
    URL "/yourfolder/ceres-solver-1.14.0.tar.gz"
    UPDATE_COMMAND ""
  4. yaml_cpp_catkin。下载yaml-cpp-11607eb5bf1258641d80f7051e7cf09e317b4746.zipyourfolder/。打开./uslam_ws/src/yaml_cpp_catkin目录下的Cmakelists.txt文件,修改GIT_REPOSITORY

    1
    2
    3
    4
    5
    # GIT_REPOSITORY  https://github.com/jbeder/yaml-cpp
    # GIT_TAG ${YAML_CPP_TAG}
    URL "/yourfolder/yaml-cpp-11607eb5bf1258641d80f7051e7cf09e317b4746.zip"
    URL_MD5 f2847f928634303a8ee305a3f28ebbcc
    UPDATE_COMMAND ""

你可能需要autoreconf来编译glog_catkin,使用以下命令安装autoreconf

1
2
3
4
5
# https://askubuntu.com/questions/265471/autoreconf-not-found-error-during-making-qemu-1-4-0/269423#269423
# 查询可安装的版本
apt-cache search autoreconf
sudo apt-get install autoconf # 13.04/14.04/16.04/18.04
sudo apt install dh-autoreconf # 20.04

指定项目编译使用的OpenCV版本。

1
2
3
4
5
6
7
8
9
10
# CmakeList.txt
set(OpenCV_DIR /usr/local/opencv/opencv320/share/OpenCV) # 新增
find_package(OpenCV REQUIRED)
# find_package(cv_bridge) # 如果find_package(OpenCV REQUIRED)报错
include_directories(${OpenCV_INCLUDE_DIRS}) # 新增
# 新增
message(STATUS "opencv version: ${OpenCV_VERSION}")
message(STATUS "opencv lib: ${OpenCV_LIBS}")
message(STATUS "opencv include dir: ${OpenCV_INCLUDE_DIRS}")
message(STATUS "opencv config path: ${openCV_CONFIG_PATH}")

最后,构建UltimateSLAM:

1
2
3
4
5
cd ./catkin_ws/src/
# mkdir -p build/suitesparse/suitesparse_src-prefix/src && cp SuiteSparse-4.2.1.tar.gz ./build/suitesparse/suitesparse_src-prefix/src
catkin build ze_vio_ceres
# 如果编译报错:
# catkin clean 相当于 rm -r ${build} ${devel}, 但是避免了 rm -r 这种危险的操作!

这将花费一些时间(至少几分钟)。如果遇到错误,请查看下面的疑难解答部分。

报错:

  1. ceres_catkin。
    • catkin build ceres_catkin报错:/usr/include/eigen3/Eigen/src/Core/PlainObjectBase.h:883:7: error: static assertion failed: INVALID_MATRIX_TEMPLATE_PARAMETERS
    • 解决方案:使用Eigen3.3.x和ceres-solver-1.14.0。
  2. yaml_cpp_catkin。
  3. Compilation Error NVIDIA Jetson TX2
  4. kalibr_swe_config isn't available as stated in Wiki
  5. F0712 16:55:25.590517 19922 ros_bridge.cpp:131 Unsupported pixel typergb8

如果构建成功,恭喜!您已安装UltimateSLAM。您的下一步将是运行一些示例来测试您的设置。

运行示例

在此页面上,我们将展示如何在事件相机数据集的几个数据集上离线运行UltimateSLAM。

下载数据集

首先,在当前终端中刷新UltimateSLAM:

1
source ~/uslam_ws/devel/setup.bash

现在,导航到rpg_ultimate_slam_open文件夹并创建一个data文件夹:

1
2
3
roscd ze_vio_ceres/../../
mkdir data
cd data/

下载一些示例数据集:

1
2
3
wget http://rpg.ifi.uzh.ch/datasets/davis/boxes_6dof.bag
wget http://rpg.ifi.uzh.ch/datasets/davis/dynamic_6dof.bag
wget http://rpg.ifi.uzh.ch/datasets/davis/shapes_6dof.bag

运行UltimateSLAM

UltimateSLAM可以以两种不同的模式运行:仅使用事件和IMU,或使用事件、帧和IMU。

使用事件+IMU

1
source ./devel/setup.zsh && roslaunch ze_vio_ceres ijrr17_events_only.launch bag_filename:=dynamic_6dof.bag

使用事件+帧+IMU

1
2
3
source ./devel/setup.zsh && roslaunch ze_vio_ceres ijrr17.launch bag_filename:=dynamic_6dof.bag

source ./devel/setup.zsh && roslaunch ze_vio_ceres ijrr17.launch bag_filename:=shapes_6dof.bag

启动时,应该会弹出一个 RVIZ 窗口,显示当前估计的轨迹和点云。

IMU同步和校准问题引起的运行时系统崩溃(失败,未找到好的解决办法):更改Opencv或Eigen的版本。

恭喜你!你已准备好实时运行 UltimateSLAM

相机校准

本页提供了执行相机校准的一些指导,即估算你的事件相机的内参,以及事件相机与 IMU 之间的外参校准。

DAVIS 校准

由于 DAVIS 传感器在与事件相同的传感器阵列上提供灰度图像(/dvs/image_raw),因此可以在灰度图像上使用标准校准工具来校准相机。我们建议使用 Kalibr 工具箱

为了获得最佳结果,我们建议记录两个单独的校准数据集:一个用于执行内参校准,另一个用于进行相机到 IMU 的校准。理论上,记录一个数据集以完成这两项是足够的,但是当记录两个单独的数据集时,结果通常会更好。

内参校准

为了进行内参校准,录制一个 rosbag,包含大约一分钟的灰度图像数据(/dvs/image_raw),相机从不同角度观察一个已知的校准模式(我们建议使用 Aprilgrid)。此时,较慢地移动相机会更好,以最小化图像中的运动模糊。同时,请确保尽可能覆盖许多不同的角度以获得良好的结果。

1
2
3
4
# 录制 rosbag
rosbag record -O cam_calib.bag /dvs/image_raw
# 然后,用 Kalibr 估算相机内参
kalibr_calibrate_cameras --target ~/uslam_ws/src/ultimate_slam/calibration/kalibr_targets/april_5x4.yaml --bag cam_calib.bag --models pinhole-radtan --topics /dvs/image_raw --show-extraction

相机到 IMU 的校准

为了进行相机到 IMU 的校准,录制一个 rosbag,包含大约一分钟的灰度图像 + IMU 数据(/dvs/image_raw/dvs/imu)。如前所述,相机应始终指向校准模式。不同于上述,对于这个数据集,相机移动应该稍快一些,以适当激发 IMU。然而,运动必须尽可能平稳,即避免停止或非常急促的运动。确保激活加速度计(X/Y/Z)和陀螺仪(偏航,俯仰,滚转)的所有自由度。

1
2
3
4
# 录制 rosbag
rosbag record -O imu_cam_calib.bag /dvs/image_raw /dvs/imu
# 然后,用 Kalibr 估算相机到 IMU 的外参
kalibr_calibrate_imu_camera --target ~/uslam_ws/src/ultimate_slam/calibration/kalibr_targets/april_5x4.yaml --bag imu_cam_calib.bag --cam camchain-cam_calib.yaml --imu ~/uslam_ws/src/ultimate_slam/calibration/imu/davis_mpu6150.yaml --time-calibration

转换为我们的校准格式:

1
kalibr_swe_config --cam camchain-imucam-calib.yaml --mav camera --out camera.yaml

考虑相机和 IMU 消息之间的时间延迟

一旦你运行了 kalibr_calibrate_imu_camera,别忘了在实时运行 UltimateSLAM 时使用以下标志:timeshift_cam_imu:=0.0028133308512579796,其中你需要用 Kalibr 给出的数字替换这个数字。

运行实时演示

要运行 UltimateSLAM,你需要一个带有硬件同步的惯性测量单元的事件相机。我们已经成功地测试了 UltimateSLAM 与 DAVIS240C 和 DAVIS346 相机

安装 DAVIS ROS 驱动程序

在此之前,请确保按照这里的说明安装了任何依赖项。特别要确保安装了 libcaer。然后,

  • 构建 DAVIS ROS 驱动程序:
1
catkin build davis_ros_driver
  • 需要更新 udev 规则以运行驱动程序:
1
2
3
source ~/uslam_ws/devel/setup.bash
roscd libcaer_catkin
sudo ./install.sh

如果在这一步中遇到问题,更详细的说明可在这里找到。

校准你的 DAVIS 相机

在运行 UltimateSLAM 之前,你需要校准你的 DAVIS 传感器,即估算相机的内在参数、相机到 IMU 的外部参数,以及 IMU 与事件之间的时间偏移。本页提供了一些指导。

请注意,精确的相机校准对于获得良好的跟踪结果至关重要。请确保你特别关注这一步

一旦你完成了校准,请将你的校准文件复制到 calibration 文件夹中的正确格式。

用 DAVIS 运行 UltimateSLAM 实时演示

要实时运行搭配 DAVIS 传感器的 UltimateSLAM,你将需要同时打开多个终端。我们推荐使用 Terminator,它允许你轻松地同时打开多个终端窗口并在这些窗口间轻松导航。

打开三个终端,并启动以下命令。

终端 1

启动 roscore

1
roscore

终端 2

启动 DAVIS 驱动程序:

1
rosrun davis_ros_driver davis_ros_driver

终端 3

启动 UltimateSLAM:

事件 + 帧

1
roslaunch ze_vio_ceres live_DAVIS240C.launch camera_name:=<your_camera_calibration_filename> timeshift_cam_imu:=0.0028100209382249794

仅事件

1
roslaunch ze_vio_ceres live_DAVIS240C_events_only.launch camera_name:=<your_camera_calibration_filename> timeshift_cam_imu:=0.0028100209382249794

校准文件将在 calibration/ 文件夹中被搜索。在这两种情况下,一个新的 RVIZ 窗口应该会弹出,显示当前相机位置、轨迹和当前估计的点云。

参数

  • camera_name:将其替换为calibration文件夹中你的校准文件名,不包括.yaml扩展名。
  • timeshift_cam_imu:相机和IMU之间的时间偏移。请将上面的值替换为Kalibr在校准步骤期间估计的值。
  • frame_size:每个事件帧中集成的事件数量
  • motion_correction:如果为1,将执行运动校正。否则,不执行。
  • 启动文件中还有其他参数,请参考参数调整指南了解更多详情。

重要建议

  • UltimateSLAM中的初始化相当敏感。为了获得最佳结果,我们建议从传感器静态开始(例如,放置在桌子上)。启动UltimateSLAM后,用传感器执行几秒钟的平移运动(左右移动,上下移动)以获得IMU偏差的良好初始估计。避免在传感器已经移动时初始化UltimateSLAM。避免在用相机进行大部分旋转运动时进行初始化。
  • 就像任何视觉-惯性里程计系统一样,良好的校准对于获得良好的追踪结果至关重要。
  • 适当的参数调整是必要的,以获得最佳结果。默认情况下,参数被设置为与DAVIS240C(240x180)传感器和场景中的中等纹理量很好地工作。

参数调优指南

UltimateSLAM提供了许多可以且应该为最佳跟踪性能进行调整的参数。可以通过运行以下命令获取参数的详尽列表:

1
source ./devel/setup.zsh && rosrun ze_vio_ceres ze_vio_ceres_node --help

在本页面的其余部分,我们将关注最重要的参数。

事件帧参数

以下参数允许控制如何生成事件帧。

集成参数

  • --vio_frame_size:用于绘制每个事件帧的事件数量(默认:DAVIS240C为15000)。当预期的纹理量较小时使用较小的数字(例如,对于shapes_6dof数据集为5000)。

  • --data_size_augmented_event_packet:增强事件包的大小(传递给负责事件帧绘制的前端),以事件数量表示。此值应大于--vio_frame_size

  • --vio_do_motion_correction:是否启用使用IMU、估计速度和中位场景深度的运动校正(默认:true)。除非你使用的帧大小非常小,或用于调试,否则我们建议启用它。

  • --noise_event_rate:如果局部事件率小于每帧noise_event_rate,这些事件被视为噪声,VIO将丢弃来自该事件帧的测量,对后端添加强“静态”先验。

事件帧生成频率(仅限事件管道)

在仅限事件的管道中,您可以完全指定事件帧将以以下参数下的哪个速率被创建。

在事件+帧管道中,事件帧率与灰度图像帧率相同(即,DAVIS240C为~20 Hz)

以下参数适用于仅限事件的管道。

  • --data_use_time_interval:指定事件包是以固定时间率(true)还是固定事件率(false)创建。

  • --data_interval_between_event_packets:指定两个事件包之间的间隔。如果--data_use_time_interval为true,则此值解释为毫秒,如果为false,则解释为事件数量。

校准参数

  • --timeshift_cam_imu:IMU与相机之间的时间偏移(以秒为单位)。使用Kalibr来估计它。
  • --calib_filename:YAML相机校准文件的文件名(在calibration文件夹中)。

日志记录参数

  • --log_dir:将保存日志信息(估计的相机姿势,计时信息)的文件夹。
  • --vio_trace_pose:是否将估计的姿势输出到文件。

可视化参数

  • --vio_viz_feature_tracks:是否显示特征轨迹的痕迹。

VINS-Mono

在VINS-Mono框架下复现论文《Ultimate SLAM? Combining Events, Images, and IMU for Robust Visual SLAM in HDR and High Speed Scenarios》。

代码地址

编译

  1. 新建工作区。

    1
    2
    3
    4
    5
    cd yourfolder
    catkin config --extend /opt/ros/melodic --cmake-args -DCMAKE_BUILD_TYPE=Release
    mkdir -p catkin_ws/src
    cd catkin_ws/src
    catkin_init_workspace
  2. 克隆库到./catkin_ws/src目录下。

    1. 克隆VINS-Mono-ultimate_slam
    2. 克隆catkin_simple-0e62848
    3. 克隆rpg_dvs_ros-95f08d5

    此时./catkin_ws/src目录下有VINS-Mono-ultimate_slamcatkin_simple-0e62848rpg_dvs_ros-95f08d53个文件夹和一个CMakeLists.txt文件。

  3. 配置依赖,要求同VINS-Mono

    1. 安装ROS
    2. 安装Eigen-3.3.0
    3. 安装OpenCV-3.2.0
    4. 安装Ceres-Solver-1.14.0
  4. (可选)指定项目编译使用的OpenCV版本。

    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    # CmakeList.txt
    set(OpenCV_DIR /usr/local/opencv/opencv320/share/OpenCV) # 新增
    find_package(OpenCV REQUIRED)
    # find_package(cv_bridge) # 如果find_package(OpenCV REQUIRED)报错
    include_directories(${OpenCV_INCLUDE_DIRS}) # 新增
    # 新增
    message(STATUS "opencv version: ${OpenCV_VERSION}")
    message(STATUS "opencv lib: ${OpenCV_LIBS}")
    message(STATUS "opencv include dir: ${OpenCV_INCLUDE_DIRS}")
    message(STATUS "opencv config path: ${openCV_CONFIG_PATH}")
  5. 编译。

    1
    2
    3
    catkin build  # 或 catkin_make
    # 如果编译报错:
    # catkin clean 相当于 rm -r ${build} ${devel}, 但是避免了 rm -r 这种危险的操作!

    Ceres-Solver报关于Eigen库的错误:注意此时使用的Eigen版本要与之前编译Ceres-Solver库时使用的版本一致。如果现在使用的Eigen版本变更了,那就卸载重装一遍Ceres-Solver吧。

运行

数据集下载

配置文件

以如下的rosbag包信息为例:

1
rosbag info bag_name.bag

查询结果为:

1
2
3
topics:      /davis346/events       12339 msgs    : dvs_msgs/EventArray      
/davis346/image_raw 4745 msgs : sensor_msgs/Image
/davis346/imu 205526 msgs : sensor_msgs/Imu

按实际情况修改如下配置文件:

  1. ./ultimate_slam/vins_estimator/launch/config/xxx.yaml

    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    13
    14
    #common parameters
    imu_topic: "/davis346/imu" # 修改
    # 下面是发布给 feature_trecker 的话题,与 motion_compensation_node.cpp 中的 event_image_pub_ 和 raw_image_pub_ 变量赋值相对应,不需要修改。不要修改为实际 rosbag 包的话题。
    image_topic: "/mc/image_raw"
    event_topic: "/mc/event_image"

    #camera calibration
    ...
    # Extrinsic parameter between IMU and Camera.
    ...
    #imu parameters
    ...
    #unsynchronization parameters
    ...
  2. ./ultimate_slam/vins_estimator/launch/config/xxx.launch

    1
    <arg name="config_path" default = "$(find vins_estimator)/launch/config/xxx.yaml" />
  3. ./ultimate_slam/motion_compensation/src/motion_compensation_node.cpp

    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    13
    14
    15
    16
    17
    18
    19
    20
    21
    22
    EventProcessNode::EventProcessNode(ros::NodeHandle &nh)
    : nh_(nh), it_(nh)
    {
    // fx, 0, cx, 0, fy, cy, 0, 0, 1
    cout << "Using vicon_aggressive_hdr davis346" << endl;
    K << 259.355, 0.0, 177.005, 0.0, 259.58, 137.922, 0.0, 0.0, 1.0;

    // events_sub_ = nh_.subscribe("/dvs/events", 2, &EventProcessNode::eventCallback, this);
    events_sub_ = nh_.subscribe("/davis346/events", 2, &EventProcessNode::eventCallback, this);
    // img_sub_ = nh_.subscribe("/dvs/image_raw", 2, &EventProcessNode::imageCallback, this);
    img_sub_ = nh_.subscribe("/davis346/image_raw", 2, &EventProcessNode::imageCallback, this);

    // 下面两行无需修改
    event_image_pub_ = it_.advertise("/mc/event_image", 1);
    raw_image_pub_ = it_.advertise("/mc/image_raw", 1);

    EventAccumulatedImage_ = Mat::zeros(260, 346, CV_8U);


    // imu_sub_.subscribe(nh_, "/dvs/imu", 500, ros::TransportHints().tcpNoDelay());
    imu_sub_.subscribe(nh_, "/davis346/imu", 500, ros::TransportHints().tcpNoDelay());
    }

运行

1
2
source ./devel/setup.zsh && roslaunch vins_estimator flyingroom.launch
rosbag play /yourfolder/boxes_6dof.bag

UltimateSLAM编译运行调试记录
http://zeyulong.com/posts/6d95d81a/
作者
龙泽雨
发布于
2024年2月7日
许可协议