使用D435i+Avia跑Fast-LIVO

前言

最近Fast-LIVO开源了，之前看它的论文的时候发现效果很优秀，于是用实验室现有的设备尝试一下。这里主要记录一下使用不带外触发功能的D435i + Avia跑Fast-LIVO的过程，为了适配代码，主要修改了雷达的驱动、相机的launch文件、以及使用了一个外部的rospkg对相机的IMU坐标系做了转换。

Fast-LIVO源码仓库：https://github.com/hku-mars/FAST-LIVO

自己搭建的设备如下：

编译Fast-LIVO

安装Sophus库

git clone https://github.com/strasdat/Sophus.git
cd Sophus
git checkout a621ff

在make之前，先修改一个东西，因为这个版本的Sophus是有bug的，解决方法如下：

打开Sophus/sophus/so2.cpp文件，将代码修改如下

// SO2::SO2()
// {
//   unit_complex_.real() = 1.;
//   unit_complex_.imag() = 0.;
// }
 SO2::SO2() 
{ 
   unit_complex_.real(1.); 
   unit_complex_.imag(0.); 
 }

修改好之后再make

mkdir build && cd build && cmake ..
make
sudo make install

安装fmt库

git clone  https://github.com/fmtlib/fmt.git
cd fmt
mkdir build
cd build
cmake ..
sudo make install

安装Vikit库

mkdir fast-livo_ws
cd fast-livo_ws & mkdir src & cd src
git clone https://github.com/uzh-rpg/rpg_vikit.git

在/src/rpg_vikit/vikit_common/CMakeLists.txt中添加

SET(Sophus_LIBRARIES libSophus.so)

否则待会编译的时候会报找不到一些库的错误

下载Fast-LIVO源码

git clone https://github.com/hku-mars/FAST-LIVO
cd ../
catkin_make

到这里就可以跑作者提供的数据集了，数据集下载可以在这里找到FAST-LIVO-Datasets

连接实物

这里我们使用自己的设备跑Fast-LIVO，我们的设备是realsense D435i + Livox Avia ，因为Fast-LIVO要求相机和IMU时钟同步，所以针对我们自己的设备需要做一些调整

可以看到作者提供的数据集里面的各个传感器是严格时钟同步的

livox驱动修改

livox原驱动中设置lidar、imu数据的时间戳均为基于雷达启动的时间开始的，而非基于系统的时间，而摄像头（包括本例中的RealSense）数据的时间戳通常是基于系统时间的。如果只是跑作者的数据集，便不需要改livox的ROS驱动；如果要跑自己的数据，就需要参考作者提供的修改方式进行处理。

• livox官方驱动： https://github.com/Livox-SDK/livox_ros_driver
• r3live作者提供的修改方案：https://github.com/ziv-lin/livox_ros_driver_for_R2LIVE

实际上我们可以直接使用r3live作者提供的修改方案，但是为了学习，我这里参考作者的方法自己下载了官方的驱动进行修改，我这里修改的这个版本和r3live作者提供的主要有如下区别：

• 设置了宏定义编译，在CMakeLists.txt中通过应该宏定义开关可以选择编译官方原生的，还是我们自己修改过后的版本的
• 仅修改雷达点云的时间戳。因为r3live作者他使用的是雷达内置的IMU，所以他把雷达的时间戳也改了，我这里没有使用雷达自带的IMU，所以没有改雷达IMU的时间戳以及它的重力尺度

其实主要是修改了livox_ros_driver中的lddc.cpp文件，修改方法如下：

1、在lddc.cpp文件开头，加入用于记录起始时间的相关变量

2、在点云消息发布之前把时间戳修改

• livox自定义消息类型

修改lddc.cpp中的PublishCustomPointcloud函数，在消息Publish之前加入如下片段

• PointCloud2消息类型

修改lddc.cpp中的PublishPointcloud2函数，在消息Publish之前加入如下片段

• CMakeLists.txt设置宏定义

添加宏定义变量如下：

修改完成之后重新编译，之后启动我们自己修改的livox_ros_driver 即可

IMU坐标系转换

由于 Avia 雷达内置IMU，如果使用它自带的IMU，那么就不需要进行坐标调整，因为雷达内置的IMU的坐标系和雷达坐标系是一致的。

因为我们这里没有使用外部的时钟触发设备，所以我们这里选择使用相机内置的IMU，在相机的ros_warp中我们可以设置图像和IMU时钟同步，这样就可以免去使用一个外部时钟触发设备。但是，如果我们使用外部的IMU（如：D435i内置的IMU），相机中内置的IMU的坐标系是在相机坐标系下的，和雷达坐标系不一样，我们需要把IMU的坐标系转换到雷达坐标系下。

这里我们通过ROS中的imu_transformer功能包进行修改，这个包需要自行下载编译

• imu_pipeline功能包集：https://github.com/ros-perception/imu_pipeline

对于imu_transformer功能包，注意修改启动文件ned_to_enu.launch中的静态变换与target_frame字段。

修改完成之后，重新编译即可。使用时，先启动imu_transformer的转换程序，再启动相机的ros_warp。

可以看到它把相机的IMU话题订阅了并重新转发了一次，转成了/trans/data，这个话题下的IMU数据和雷达坐标系是一致的

后面在我们程序中可以直接订阅这个/trans/data话题即可

D435i launch文件修改

这里我自定义了一个launch文件如下：

<launch>

  <rosparam>
      /camera/motion_module/global_time_enabled: true
      /camera/rgb_camera/global_time_enabled: true
  </rosparam>

  <arg name="serial_no"           default=""/>
  <arg name="usb_port_id"         default=""/>
  <arg name="device_type"         default=""/>
  <arg name="json_file_path"      default=""/>
  <arg name="camera"              default="camera"/>
  <arg name="tf_prefix"           default="$(arg camera)"/>
  <arg name="external_manager"    default="false"/>
  <arg name="manager"             default="realsense2_camera_manager"/>
  <arg name="output"              default="screen"/>
  <arg name="respawn"              default="false"/>

  <arg name="fisheye_width"       default="-1"/>
  <arg name="fisheye_height"      default="-1"/>
  <arg name="enable_fisheye"      default="false"/>

  <arg name="depth_width"         default="640"/>
  <arg name="depth_height"        default="480"/>
  <arg name="enable_depth"        default="false"/>

  <arg name="confidence_width"    default="-1"/>
  <arg name="confidence_height"   default="-1"/>
  <arg name="enable_confidence"   default="false"/>
  <arg name="confidence_fps"      default="-1"/>

  <!-- 红外摄像头 -->
  <arg name="infra_width"         default="640"/>
  <arg name="infra_height"        default="480"/>
  <arg name="enable_infra"        default="false"/>
  <arg name="enable_infra1"       default="false"/>
  <arg name="enable_infra2"       default="false"/>
  <arg name="infra_rgb"           default="false"/>

  <!-- RGB摄像头 -->
  <arg name="color_width"         default="640"/>
  <arg name="color_height"        default="480"/>
  <arg name="enable_color"        default="true"/>

  <arg name="fisheye_fps"         default="-1"/>
  <arg name="depth_fps"           default="30"/>
  <arg name="infra_fps"           default="30"/>
  <arg name="color_fps"           default="30"/>

  <!-- 开启IMU -->
  <arg name="gyro_fps"            default="200"/>
  <arg name="accel_fps"           default="200"/>
  <arg name="enable_gyro"         default="true"/>
  <arg name="enable_accel"        default="true"/>

  <arg name="enable_pointcloud"         default="false"/>
  <arg name="pointcloud_texture_stream" default="RS2_STREAM_COLOR"/>
  <arg name="pointcloud_texture_index"  default="0"/>
  <arg name="allow_no_texture_points"   default="false"/>
  <arg name="ordered_pc"                default="false"/>

  <!-- 开启时间戳同步 -->
  <arg name="enable_sync"               default="true"/>
  <arg name="align_depth"               default="false"/>

  <arg name="publish_tf"                default="true"/>
  <arg name="tf_publish_rate"           default="0"/>

  <arg name="filters"                   default=""/>
  <arg name="clip_distance"             default="-2"/>
  <arg name="linear_accel_cov"          default="0.01"/>
  <arg name="initial_reset"             default="false"/>
  <arg name="reconnect_timeout"         default="6.0"/>
  <arg name="wait_for_device_timeout"   default="-1.0"/>

  <!-- 陀螺仪与加速度联合发布的时候的对齐方式，这里采用了线性插值的方式 -->
  <arg name="unite_imu_method"          default="linear_interpolation"/>
  <!-- <arg name="unite_imu_method"          default="copy"/> -->
  <arg name="topic_odom_in"             default="odom_in"/>
  <arg name="calib_odom_file"           default=""/>
  <arg name="publish_odom_tf"           default="true"/>

  <arg name="stereo_module/exposure/1"  default="7500"/>
  <arg name="stereo_module/gain/1"      default="16"/>
  <arg name="stereo_module/exposure/2"  default="1"/>
  <arg name="stereo_module/gain/2"      default="16"/>
  
  <include file="$(find imu_transformer)/launch/ned_to_enu.launch"/>

  <group ns="$(arg camera)">
    <include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
      <arg name="tf_prefix"                value="$(arg tf_prefix)"/>
      <arg name="external_manager"         value="$(arg external_manager)"/>
      <arg name="manager"                  value="$(arg manager)"/>
      <arg name="output"                   value="$(arg output)"/>
      <arg name="respawn"                  value="$(arg respawn)"/>
      <arg name="serial_no"                value="$(arg serial_no)"/>
      <arg name="usb_port_id"              value="$(arg usb_port_id)"/>
      <arg name="device_type"              value="$(arg device_type)"/>
      <arg name="json_file_path"           value="$(arg json_file_path)"/>

      <arg name="enable_pointcloud"        value="$(arg enable_pointcloud)"/>
      <arg name="pointcloud_texture_stream" value="$(arg pointcloud_texture_stream)"/>
      <arg name="pointcloud_texture_index"  value="$(arg pointcloud_texture_index)"/>
      <arg name="enable_sync"              value="$(arg enable_sync)"/>
      <arg name="align_depth"              value="$(arg align_depth)"/>

      <arg name="fisheye_width"            value="$(arg fisheye_width)"/>
      <arg name="fisheye_height"           value="$(arg fisheye_height)"/>
      <arg name="enable_fisheye"           value="$(arg enable_fisheye)"/>

      <arg name="depth_width"              value="$(arg depth_width)"/>
      <arg name="depth_height"             value="$(arg depth_height)"/>
      <arg name="enable_depth"             value="$(arg enable_depth)"/>

      <arg name="confidence_width"         value="$(arg confidence_width)"/>
      <arg name="confidence_height"        value="$(arg confidence_height)"/>
      <arg name="enable_confidence"        value="$(arg enable_confidence)"/>
      <arg name="confidence_fps"           value="$(arg confidence_fps)"/>

      <arg name="color_width"              value="$(arg color_width)"/>
      <arg name="color_height"             value="$(arg color_height)"/>
      <arg name="enable_color"             value="$(arg enable_color)"/>

      <arg name="infra_width"              value="$(arg infra_width)"/>
      <arg name="infra_height"             value="$(arg infra_height)"/>
      <arg name="enable_infra"             value="$(arg enable_infra)"/>
      <arg name="enable_infra1"            value="$(arg enable_infra1)"/>
      <arg name="enable_infra2"            value="$(arg enable_infra2)"/>
      <arg name="infra_rgb"                value="$(arg infra_rgb)"/>

      <arg name="fisheye_fps"              value="$(arg fisheye_fps)"/>
      <arg name="depth_fps"                value="$(arg depth_fps)"/>
      <arg name="infra_fps"                value="$(arg infra_fps)"/>
      <arg name="color_fps"                value="$(arg color_fps)"/>
      <arg name="gyro_fps"                 value="$(arg gyro_fps)"/>
      <arg name="accel_fps"                value="$(arg accel_fps)"/>
      <arg name="enable_gyro"              value="$(arg enable_gyro)"/>
      <arg name="enable_accel"             value="$(arg enable_accel)"/>

      <arg name="publish_tf"               value="$(arg publish_tf)"/>
      <arg name="tf_publish_rate"          value="$(arg tf_publish_rate)"/>

      <arg name="filters"                  value="$(arg filters)"/>
      <arg name="clip_distance"            value="$(arg clip_distance)"/>
      <arg name="linear_accel_cov"         value="$(arg linear_accel_cov)"/>
      <arg name="initial_reset"            value="$(arg initial_reset)"/>
      <arg name="reconnect_timeout"        value="$(arg reconnect_timeout)"/>
      <arg name="wait_for_device_timeout"  value="$(arg wait_for_device_timeout)"/>
      <arg name="unite_imu_method"         value="$(arg unite_imu_method)"/>
      <arg name="topic_odom_in"            value="$(arg topic_odom_in)"/>
      <arg name="calib_odom_file"          value="$(arg calib_odom_file)"/>
      <arg name="publish_odom_tf"          value="$(arg publish_odom_tf)"/>
      <arg name="stereo_module/exposure/1" value="$(arg stereo_module/exposure/1)"/>
      <arg name="stereo_module/gain/1"     value="$(arg stereo_module/gain/1)"/>
      <arg name="stereo_module/exposure/2" value="$(arg stereo_module/exposure/2)"/>
      <arg name="stereo_module/gain/2"     value="$(arg stereo_module/gain/2)"/>

      <arg name="allow_no_texture_points"  value="$(arg allow_no_texture_points)"/>
      <arg name="ordered_pc"               value="$(arg ordered_pc)"/>
    </include>
  </group>
</launch>

主要注意以下几个地方：

1、为了保证image和IMU的时钟同步需要添加下面几个参数

2、开启IMU中的角速度和加速度联合发布，以及时间戳同步

3、设置陀螺仪与加速度联合发布的时候的对齐方式

运行

roslaunch realsense2_camera rs_livox.launch # 启动相机，这里我把imu_transform包也放到这个launch文件中启动了
roslaunch livox_ros_driver livox_lidar_msg.launch # 启动雷达
roslaunch fast_livo mapping_avi_we.launch # 启动fast_livo

建图效果如下，发现这个Fast-LIVO建图的时候比较卡，可能是作者还没优化好，针对低性能设备做优化也是一个重要的需求！