作者 | 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设为10cmfeatsArray：以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);函数，然后更新ikdtreem_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线程
相关变量
除了系统状态g_lio_state和地图点云m_map_rgb_pts外，vio的一些关键变量如下：g_received_compressed_img_msg、g_received_img_msg（未使用）是保存图像msg的队列
m_queue_image_with_pose 是保存等待vio处理的图像队列
img_pose：std::shared_ptr< Image_frame > 类型，其中Image_frame（在rgb_map/Image_frame.hpp中定义）主要保存时间戳、相机位姿、相机内参，并提供3d点投影到2d等成员函数
op_track：Rgbmap_tracker类型（在rgb_map/rgbmap_tracker.hpp中定义），主要保存当前帧和上一帧图像跟踪点的像素坐标，提供光流跟踪、更新新增跟踪点等成员函数
m_map_rgb_pts.m_voxels_recent_visited 保存active的voxel，voxel中是若干点（包含位置，rgb，rgb方差，在前后帧图像中坐标等）
主要流程
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求职交流等方向；
添加汽车人助理微信邀请入群
备注：学校/公司+方向+昵称
423
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
