源码解析 | R3LIVE：一个健壮、实时的顶尖SLAM框架

作者：不正经 | 2024-04-14 07:57:06

踩

update_iterated_dyn_share_modified

作者 | stella 编辑 | 汽车人

原文链接：https://zhuanlan.zhihu.com/p/480275319?

点击下方卡片，关注“自动驾驶之心”公众号

ADAS巨卷干货，即可获取

点击进入→自动驾驶之心【SLAM】技术交流群

整体介绍

R3LIVE是香港大学Mars实验室提出的一种融合imu、相机、激光的SLAM方法。系统以imu为核心，利用激光雷达和相机的观测修正系统状态。算法相当于不断以imu的预测为基础，每当得到了新的相机或激光的观测，就对系统状态进行更新，实现了多传感器信息融合。其中激光雷达的观测与LIO相同，相机的观测则分为两步：先是用跟踪点的重投影误差，再基于更新结果用跟踪点的光度误差再次进行状态更新。

R3LIVE系统有两个主要的线程：VIO和LIO，两个线程按时间顺序分别用激光或图像的观测来更新系统状态。LIO线程得到新激光雷达点云后，利用上一次状态更新之后的imu数据进行状态预测，然后利用激光的观测进行状态更新，并更新点云地图；VIO线程得到新图像后，利用上一次状态更新之后的imu数据进行状态预测，然后利用图像的观测进行状态更新，并更新点云中点的rgb信息。R3LIVE代码中用到了线程池，关于线程池可参考这篇。线程池主要解决频繁创建和销毁线程会过多占用系统资源的问题，R3LIVE主要在render的时候用到了线程池的特性。

LIO线程

相关变量

跟LIO相关的比较重要的变量是：g_lio_state：StatesGroup类型（在loam/include/common_lib.h中定义），保存了系统的状态，vio和lio都会对它进行更新，部分成员变量如下：


Eigen::Matrix3d rot_end;                                 // [0-2] the estimated attitude (rotation matrix) at the end lidar point
Eigen::Vector3d pos_end;                                 // [3-5] the estimated position at the end lidar point (world frame)
Eigen::Vector3d vel_end;                                 // [6-8] the estimated velocity at the end lidar point (world frame)
Eigen::Vector3d bias_g;                                  // [9-11] gyroscope bias
Eigen::Vector3d bias_a;                                  // [12-14] accelerator bias
Eigen::Vector3d gravity;                                 // [15-17] the estimated gravity acceleration
 
Eigen::Matrix3d rot_ext_i2c;                             // [18-20] Extrinsic between IMU frame to Camera frame on rotation.
Eigen::Vector3d pos_ext_i2c;                             // [21-23] Extrinsic between IMU frame to Camera frame on position.
double          td_ext_i2c_delta;                        // [24]    Extrinsic between IMU frame to Camera frame on position.
vec_4           cam_intrinsic;                           // [25-28] Intrinsice of camera [fx, fy, cx, cy]
Eigen::Matrix<double, DIM_OF_STATES, DIM_OF_STATES> cov; // states covariance
double last_update_time = 0;

m_map_rgb_pts：Global_map类型（在rgb_map/pointcloud_rgbd.hpp中定义），主要以voxel的形式保存RGB点云，并提供加入新点云、点云渲染、投影到图像平面等成员函数，比较重要的变量如下：

m_voxels_recent_visited:保存了近期lidar扫描到的voxel（目前设置为仅保存最近一帧扫到的）
m_hashmap_3d_pts：hash表保存的rgb点（指针形式），点之间间隔需要大于m_minimum_pts_size，否则不会再加入地图中，主要是用来对地图点云进行降采样。目前设为1cm。
m_rgb_pts_vec：vector保存的地图rgb点（指针形式），主要用来可视化的
m_hashmap_voxels：hash表保存的voxel，每个voxel中包含若干个rgb点（指针形式），分辨率m_voxel_resolution设为10cm
featsArray：以cube为单位保存点云PointCloudXYZINormal::Ptr，目前设置为最多保存48* 48* 48个cube。主要作用是配合ikdtree，保存local map范围内的点云，在lasermap_fov_segment()中当位姿超过FOV设置的边界时会移动local map并从ikdtree删除范围外的cube。

g_camera_lidar_queue：Camera_Lidar_queue类型（在loam/include/common_lib.h中定义）主要保存lidar队列以及imu、激光、相机的时间戳等信息

成员变量m_liar_frame_buf 是保存lidar的msg的队列
成员函数bool if_camera_can_process() bool if_lidar_can_process()帮助控制激光和imu的处理顺序

Measures ：MeasureGroup类型（在loam/include/common_lib.h中定义），将一帧lidar以及对应的若干imu数据一起打包，用于LIO的状态估计

主要流程

激光点云首先在LiDAR_front_end节点中提取特征点，然后将特征点发给r3live节点，保存到g_camera_lidar_queue.m_liar_frame_buf中等待LIO线程处理。除了需要向RGB点云地图中新增点以外，整体流程和FASTLIO一致，具体可参考我之前写的。

LIO线程对应int R3LIVE::service_LIO_update()函数，主要流程如下：

首先g_camera_lidar_queue.if_lidar_can_process()对比相机和lidar队列头的时间戳，如果camera的时间戳更早则等待vio线程把更早的图像处理完


while ( g_camera_lidar_queue.if_lidar_can_process() == false )  //对比cam和lidar队列头的时间戳，如果camera时间更早则等待cam处理
{
    ros::spinOnce();
    std::this_thread::yield();  //让出时间片，把CPU让给其他进程
    std::this_thread::sleep_for( std::chrono::milliseconds( THREAD_SLEEP_TIM ) );
}

sync_packages( Measures ); 处理imu和lidar接收buffer中的数据。主要是将一帧激光点云，以及这一帧点云扫描期间的imu数据一起打包放到MeasureGroup里。
p_imu->Process( Measures, g_lio_state, feats_undistort ); 主要是调UndistortPcl 函数补偿点云畸变，并用imu数据进行系统状态预测
lasermap_fov_segment();更新localmap边界，然后降采样当前帧点云
if ( featsFromMapNum >= 5 ){...}中的代码都是用激光信息进行ESIKF状态更新，对应FASTLIO中的的kf.update_iterated_dyn_share_modified(LASER_POINT_COV, solve_H_time);函数，然后更新ikdtree
m_map_rgb_pts.append_points_to_global_map(...)向rgb点云地图中加入新点

发布的Topic

LIO发布的部分Topic如下：

/cloud_registered：发布经过imu去畸变后的当前feats_undistort或降采样后的点云，已转换到全局坐标系
/aft_mapped_to_init ：发布对应激光雷达频率的里程计，注意是imu位姿
/path：发布imu轨迹
cloud_effected：发布最后一次ESIKF更新时的当前帧有效特征点（即能匹配到地图中的有效平面，且残差小于阈值）
/Laser_map：发布ikdtree中保存的激光点云

VIO线程

主要流程

VIO线程对应int R3LIVE::service_VIO_update()函数，主要流程如下：

如果是第一帧图像，则需要等待点云地图中的点数量大于阈值后，再选取active点云投影到图像上，作为初始跟踪点，初始化跟踪器op_track。这里我比较疑惑的是需要确保一开始lidar先处理吗？个人认为好像不需要，因为假设系统一开始是静止的，即使图像先处理，也需要等待第一帧lidar把点云地图建起来才能初始化
与LIO一样，需要调g_camera_lidar_queue.if_camera_can_process()对比相机和lidar队列头的时间戳，如果lidar的时间戳更早则等待LIO线程把更早的lidar处理完。
vio_preintegration(...)函数用imu数据进行系统状态预测
set_image_pose( img_pose, state_out )用状态预测结果作为这帧图像的初始位姿。然后调用op_track.track_img( img_pose, -20 )跟踪特征点。在特征点跟踪时会先用RANSAC求基础矩阵过滤误匹配。然后调用reject_error_tracking_pts()利用预测结果过滤错误匹配（目前没用到）
op_track.remove_outlier_using_ransac_pnp( img_pose )通过pnp过滤错误匹配
res_esikf = vio_esikf( state_out, op_track ); 以跟踪点的重投影误差作为观测，进行ESIKF状态更新（包括协方差）。
res_photometric = vio_photometric( state_out, op_track, img_pose );在上一步更新的状态和方差基础上，利用跟踪点的光度误差再进行ESIKF状态更新。
申请一个线程，执行render_pts_in_voxels_mp(...)，在该函数中并行地将点云地图的active点投影到图像上，用对应的像素对地图点rgb的均值和方差用贝叶斯迭代进行更新。这里用到了opencv提供的parallel_for_机制，以及intel TBB库。
调用m_map_rgb_pts.update_pose_for_projection( img_pose, -0.4 ); 更新m_img_for_projection的位姿和id，以触发refresh线程Global_map::service_refresh_pts_for_projection()将点云的active点投影到2D（方便后面新增跟踪点）
op_track.update_and_append_track_pts(...)更新跟踪点，并根据点云地图中的active点新增跟踪点，删除重投影误差和光度误差过大的跟踪点

发布的Topic

VIO发布的部分Topic如下：

/camera_odom：发布视觉里程计，注意是相机的位姿
/camera_path：发布相机轨迹
/render_pts：本次render得到的RGB点云
/RGB_map_xx：将RGB点云地图拆分成子点云发布，由一个线程专门负责

以上仅为个人理解。本人水平有限，有错误的地方请大家指出，一起讨论。

国内首个自动驾驶学习社区

近1000人的交流社区，和20+自动驾驶技术栈学习路线，想要了解更多自动驾驶感知（分类、检测、分割、关键点、车道线、3D目标检测、多传感器融合、目标跟踪、光流估计、轨迹预测）、自动驾驶定位建图（SLAM、高精地图）、自动驾驶规划控制、领域技术方案、AI模型部署落地实战、行业动态、岗位发布，欢迎扫描下方二维码，加入自动驾驶之心知识星球，这是一个真正有干货的地方，与领域大佬交流入门、学习、工作、跳槽上的各类难题，日常分享论文+代码+视频，期待交流！

【自动驾驶之心】全栈技术交流群

自动驾驶之心是首个自动驾驶开发者社区，聚焦目标检测、语义分割、全景分割、实例分割、关键点检测、车道线、目标跟踪、3D目标检测、BEV感知、多传感器融合、SLAM、光流估计、深度估计、轨迹预测、高精地图、NeRF、规划控制、模型部署落地、自动驾驶仿真测试、产品经理、硬件配置、AI求职交流等方向；

添加汽车人助理微信邀请入群

备注：学校/公司+方向+昵称

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/不正经/article/detail/421005