前言
最近Fast-LIVO开源了，之前看它的论文的时候发现效果很优秀，于是用实验室现有的设备尝试一下。这里主要记录一下使用不带外触发功能的D435i + Avia跑Fast-LIVO的过程，为了适配代码，主要修改了雷达的驱动、相机的launch文件、以及使用了一个外部的rospkg对相机的IMU坐标系做了转换。
Fast-LIVO源码仓库：https://github.com/hku-mars/FAST-LIVO
自己搭建的设备如下：
文章目录
- 前言
- 编译Fast-LIVO
- 安装Sophus库
- 安装fmt库
- 安装Vikit库
- 下载Fast-LIVO源码
- 连接实物
- livox驱动修改
- IMU坐标系转换
- D435i launch文件修改
- 运行
编译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建图的时候比较卡，可能是作者还没优化好，针对低性能设备做优化也是一个重要的需求！
