FAST-LIVO2实机复现（ROS1）
FAST-LIVO2实机复现（ROS1）
环境介绍
- Ubuntu 20.04
- ROS Noetic
复现设备
雷达：Livox Mid360
相机：RealSense D435i
注：这里为了方便，直接用从狗上拆下来的雷达和相机，因为D435i做不了硬同步，所以这里的相机可以换成任意USB相机。
一根M12一分三线（连接雷达，不做硬同步的话也可以去买一分二线，也就是只有电源线和信号传输）
一根USB线（连接相机）
雷达相机支架（找好兄弟帮忙打了一个（倾斜30度的））
主机（笔记本、Jetson都可以）
一、FAST-LIVO2环境安装
1、安装ROS Notice
推荐使用鱼香ROS一键安装
wgethttp://fishros.com/install-Ofishros&&.fishros安装完成后终端输入
roscore另开终端输入
rosrun turtlesim turtlesim_node看到有小乌龟即为安装成功
2、检查环境依赖
FAST-LIVO2 需要 PCL >= 1.8, Eigen >= 3.3.4, OpenCV >= 4.2。一般安装ROS Notice回自动安装符合要求的版本
apt-cacheshow libpcl-dev|grepVersion pkg-config--modversioneigen3 pkg-config--modversionopencv43、安装Sophus库
FAST-LIVO2依赖老版本Sophus，与新版存在兼容性问题，需要手动修改源码。
（1）下载源码
mkdir-p~/codecd~/codegitclone https://github.com/strasdat/Sophus.gitcdSophusgitcheckout a621ff（2）修改源码
打开ophus/so2.cpp文件
找到
unit_complex_.real() = 1.; unit_complex_.imag() = 0.;修改为
unit_complex_.real(1.); unit_complex_.imag(0.);保存关闭
（3）编译文件
在Sophus文件下
mkdirbuild&&cdbuild&&cmake..makesudomakeinstall4、下载Vikit
mkdir-p~/catkin_ws/srccd~/catkin_ws/srcgitclone https://github.com/xuankuzcr/rpg_vikit.git5、下载FAST-LIVO2源码并构建
cd~/catkin_ws/srcgitclone https://github.com/hku-mars/FAST-LIVO2cd..catkin_makesource~/catkin_ws/devel/setup.bash6、启动FAST-LIVO2
roslaunch fast_livo mapping_avia.launch可以使用官方数据集网页链接测试。
下载好数据包，另开终端播包
rosbag play XXX.bag此时fast-livo终端是有三维彩色点云图像。
二、雷达、相机驱动安装
1、相机驱动安装
如果是普通的usb相机的话，直接安装usb_cam驱动
sudoapt-getupdatesudoapt-getinstallros-noetic-usb-cam然后连接摄像头运行
rosrun usb_cam usb_cam_node就可以发布一个名为/usb_cam/image_raw的topic
D435i有一颗RGB摄像头，也可以使用usb_cam驱动
这里我直接安装RealSense的驱动，功能更多
2、雷达驱动安装
用的是mid360雷达，需要安装LIVOX-SDK2
git clone https://github.com/Livox-SDK/Livox-SDK.git cd Livox-SDK cd build && cmake .. make sudo make install接下来安装驱动，安装之前设置系统的有线网络，固定有线连接IP——设置IP地址为雷达同网段下的其他地址，例如雷达地址为192.168.1.20，可设置有线IP为192.168.1.50，同时设置子网掩码255.255.255.0和网关192.168.1.50
下载FAST-LIVO2官方推荐的驱动包https://github.com/xuankuzcr/LIV_handhold
解压找到其中名为livox_ros_driver2的驱动包，单独拿出
修改雷达的配置文件mid360_config.json（这是mid360的配置文件），设置其中的雷达地址和主机地址，以及其他雷达配置。
在工作空间下运行，进行编译：
catkin_make -DROS_EDITION=ROS1而后运行
roslaunch livox_ros_driver2 rviz_MID360.launchRVIZ上有点云，即为成功
三、参数标定
1、相机内参标定
准备内参标定板
下载内参标定包
sudo apt install ros-$ROS_DISTRO-camera-calibration启动相机驱动
rosrun usb_cam usb_cam_node确定相机内参标定板的角点个数和方格尺寸，例如8*11个角点，方格15mm，启动节点进行标定
rosrun camera_calibration cameracalibrator.py --size 8x11 --square 0.015 image:=/usb_cam/image_raw标定完成后得到一组参数为相机内参fx, fy, cx, cy, k1, k2, p1, p2。
2、相机雷达外参标定
使用 FAST-Calib包进行联合标定，首先需要准备 FAST-Calib标定板
FAST-Calib标定分为单场景标定和多场景联合标定。
（1）准备数据，拿出我们的设备连接主机，平稳放置在标定板正前方，录制雷达与相机的rosbag包，称之为bag1，几秒钟就行；可以从不同角度录制bag2、bag3，做多场景标定。
（2）下载编译 FAST-Calib包
mkdir -p ~/calib_ws/src cd ~/calib_ws/src #PCL>=1.8、OpenCV>=4.0 git clone https://github.com/hku-mars/FAST-Calib.git cd ~/calib_ws catkin_make（3）修改配置文件qr_params.yaml，具体修改如下
a、修改相机内参
源文件的第一部分，是相机的内参，改为我们之前获取到的相机内参即可。
b、修改标定板尺寸
这里的标定板尺寸是按照官方给出的，按照实际的标定板尺寸去改就行。
marker_size —— aruco的边长
delta_width_qr_center —— 两个水平aruco中心的距离 的一半
delta_height_qr_center —— 两个垂直aruco中心的距离 的一半
delta_width_circles —— 两个水平圆心的距离
delta_height_circles —— 两个垂直圆心的距离
circle_radius —— 圆半径
c、修改点云过滤参数
通俗来讲，我们需要过滤掉bag包中无效的点云，只留下标定板的点云，FAST-Calib程序会在这个参数范围内匹配你的标定板点云。
为了更加精确，可以使用python脚本去精确查找标定板位置，在FAST-Calib/scripts/distance_filter_tool.py中，注意要修改其中的雷达话题为自己的话题。
d、路径修改
这里就是数据的输入和输出路径，根据自己的情况修改即可
（4）运行FAST-Calib，在输出路径得到一组外参
source devel/setup.zsh roslaunch fast_calib calib.launch四、运行演示
运行演示之前，需要修改FAST-LIVO2的两个配置文件——avia.yaml和camera_pinhole.yaml。
将相机内参写入到camera_pinhole.yaml文件中，雷达相机外参写入到avia.yaml文件中，同时需要改接收的话题为雷达、imu和相机发出的话题。
运行FAST-LIVO2
先启动雷达和相机节点，最后启动fast_livo2的launch
roslaunch fast_livo mapping_avia.launch主要问题：
1、雷达和相机发布的频率不一样，如果不做时间同步，雷达点云和相机画面无法匹配，rviz上就不会成像。
2、这里imu已经产生了漂移，感觉是标定没做好，回头试一下联合标定。