R3live&Fastlio2

Fastlio2

主要代码结构：

• laserMapping.cpp：fastlio2主程序，生成Preprocess和ImuProcess实例

  pcl::VoxelGrid<PointType> downSizeFilterSurf;
  

• preprocess.cpp：激光雷达点云数据处理(Preprocess类)
• IMU_Processing.hpp：IMU数据处理(ImuProcess类)
  • process()函数用来给点云去畸变

ROS转PCL：

#include 
template<PointType>
pcl::PointCloud<PointType> pcl_pointcloud;//模板点云类，模板参数是点的格式
pcl::fromROSMsg(*msg, pcl_poincloud);//把ROS消息转化成pcl点云格式
for (uint i = 0; i < pointcloud_size; i++)//遍历点云的每一个点
	pcl_pointcloud.points[i];//每一个点
}

livox雷达数据格式
livox的时间戳是以ns为单位，点云的header->stamp记录了这一帧点云的起始时间，每个点的offset_time记录了每个点相对点云stamp的偏移量。
robosense的时间戳是以s为单位，点云的header->stamp记录了这一帧点云的结束时间

问题记录：

1. 经过rs_to_velodyne处理过后和robosense_handler处理之后的点云不同
2. point_filter_num参数设置大建图效果变差。

R3live

在传统SLAM框架下搭建了一套高性能雷达odometry-彩色点云Mapping-三维重建的体系，将Lidar-相机-IMU更直接有效的融合。它在建图的同时把RGB信息通过VIO子系统给点云上色，并通过一定的方法优化这个上色。

注意： R3live默认雷达和IMU坐标系重合，在R3live发布的tf中有两个坐标系：

• world：世界坐标系
• aft_mapped：雷达和IMU坐标系

对于雷达和IMU坐标系不重合的设备，可以在Lidar_front_end.cpp中添加将雷达点云转换到IMU系的程序来适配。

前端LiDAR_front_end.cpp负责将雷达原始点云处理成pcl::PointCloudXYZINormal格式

主函数在r3live.cpp

Eigen::initParallel();//从多个线程调用Eigen时必须首先调用
R3LIVE * fast_lio_instance = new R3LIVE();//创建r3live类实例

初始化操作在R3LIVE的构造函数中：

1. 读取参数
2. 订阅对应话题
   • IMU数据 话题名从参数/IMU_topic读取 ->回调函数 R3LIVE::imu_cbk
     将IMU数据压入imu_buffer_lio和imu_buffer_vio
   • 雷达前端处理之后的点云 话题名从参数/LiDAR_pointcloud_topic读取->回调函数R3LIVE::feat_points_cbk，每一帧数据压入lidar_buffer
   • 原始图像数据 话题名从参数/Image_topic读取 ->回调函数R3LIVE::image_callback
   • 压缩图像数据 话题名/Image_topic/compressed ->回调函数R3LIVE::image_comp_callback
3. 开启LIO核心线程 service_LIO_update
   • 执行一次sync_packages获取一帧雷达点云及其对应的IMU数据 -> MeasureGroup

视觉部分

视觉部分全部放在r3live_vio.cpp中
入口是图像话题回调函数image_callback
$$\mathrm{i}\mathrm{m}\mathrm{a}\mathrm{g}\mathrm{e}\_\mathrm{c}\mathrm{a}\mathrm{l}\mathrm{l}\mathrm{b}\mathrm{a}\mathrm{c}\mathrm{k}\to \{\begin{array}{l}\mathrm{s}\mathrm{e}\mathrm{r}\mathrm{v}\mathrm{i}\mathrm{c}\mathrm{e}\_\mathrm{p}\mathrm{r}\mathrm{o}\mathrm{c}\mathrm{e}\mathrm{s}\mathrm{s}\_\mathrm{i}\mathrm{m}\mathrm{a}\mathrm{g}\mathrm{e}\_\mathrm{b}\mathrm{u}\mathrm{f}\mathrm{e}\mathrm{r}\\ \mathrm{p}\mathrm{r}\mathrm{o}\mathrm{c}\mathrm{e}\mathrm{s}\mathrm{s}\_\mathrm{i}\mathrm{m}\mathrm{a}\mathrm{g}\mathrm{e}\to \end{array}\{\begin{array}{l}\mathrm{s}\mathrm{e}\mathrm{r}\mathrm{v}\mathrm{i}\mathrm{c}\mathrm{e}\_\mathrm{V}\mathrm{I}\mathrm{O}\_\mathrm{u}\mathrm{p}\mathrm{d}\mathrm{a}\mathrm{t}\mathrm{e}\\ \mathrm{s}\mathrm{e}\mathrm{r}\mathrm{v}\mathrm{i}\mathrm{c}\mathrm{e}\_\mathrm{p}\mathrm{u}\mathrm{b}\_\mathrm{r}\mathrm{g}\mathrm{b}\_\mathrm{m}\mathrm{a}\mathrm{p}\mathrm{s}\end{array}$$

#include
cv::eigen2cv(const Eigen::Matrix& src, cv::Mat& dst);
cv::cv2eigen(const cv::Mat& src, Eigen::Matrix& dst);