FAST-LIVO2 数据流与接口关系文档
FAST-LIVO2: Fast, Direct LiDAR-Inertial-Visual Odometry
Developer: Chunran Zheng
目录
1. 系统架构总览
系统由以下几个核心模块构成,以 LIVMapper 类作为主控制器(Orchestrator),协调各模块工作:
┌─────────────────────────────────────────────────────────────────────┐
│ main.cpp │
│ ros::init → LIVMapper → initSubPub → run() │
└──────────────────────────┬──────────────────────────────────────────┘
│
┌────────────┴────────────┐
│ LIVMapper │ (Orchestrator)
│ (LIVMapper.h/cpp) │
└────┬──────┬──────┬──────┘
│ │ │
┌──────────┘ │ └──────────┐
▼ ▼ ▼
┌───────────────┐ ┌──────────────┐ ┌──────────────────┐
│ Preprocess │ │ ImuProcess │ │ VIOManager │
│ (LiDAR前端) │ │ (IMU处理) │ │ (视觉前端) │
└───────┬───────┘ └──────┬───────┘ └────────┬─────────┘
│ │ │
└────────────────┼───────────────────┘
▼
┌──────────────────┐
│ VoxelMapManager │
│ (LiDAR地图管理) │
│ + VoxelOctoTree │
│ + VoxelPlane │
└──────────────────┘
模块职责
| 模块 | 文件 | 职责 |
|---|---|---|
| LIVMapper | LIVMapper.h/cpp | 主控制器:ROS回调、数据同步、主循环调度 |
| Preprocess | preprocess.h/cpp | LiDAR点云预处理:支持多种LiDAR格式,点特征提取 |
| ImuProcess | IMU_Processing.h/cpp | IMU数据初始化、前向传播、点云去畸变 |
| VoxelMapManager | voxel_map.h/cpp | LiDAR体素地图管理:八叉树构建、平面拟合、点面残差 |
| VIOManager | vio.h/cpp | 视觉惯性里程计:稀疏视觉地图、帧跟踪、光度误差优化 |
| Frame | frame.h/cpp | 相机帧:保存图像、特征、位姿 |
| Feature | feature.h | 图像特征点结构 |
| VisualPoint | visual_point.h/cpp | 视觉地图中的3D点,存储参考帧信息和图像块 |
| VoxelOctoTree | voxel_map.h | 八叉树体素结构,用于空间划分和平面拟合 |
关键类型别名
| 类型 | 定义 | 用途 |
|---|---|---|
PreprocessPtr | std::shared_ptr<Preprocess> | 点云预处理指针 |
ImuProcessPtr | std::shared_ptr<ImuProcess> | IMU处理指针 |
VIOManagerPtr | std::shared_ptr<VIOManager> | VIO管理器指针 |
VoxelMapManagerPtr | std::shared_ptr<VoxelMapManager> | 体素地图管理器指针 |
FramePtr | std::unique_ptr<Frame> | 相机帧独占指针 |
2. 核心数据结构
2.1 状态量: StatesGroup (common_lib.h)
系统19维状态向量(DIM_STATE = 19),用于 ESKF:
StatesGroup
├── rot_end : M3D (3×3) ← 姿态旋转矩阵 [SO(3)对应3维]
├── pos_end : V3D ← 位置 (世界系) [3维]
├── inv_expo_time : double ← 逆曝光时间 [1维]
├── vel_end : V3D ← 速度 (世界系) [3维]
├── bias_g : V3D ← 陀螺仪偏置 [3维]
├── bias_a : V3D ← 加速度计偏置 [3维]
├── gravity : V3D ← 重力加速度向量 [3维]
└── cov : 19×19 Matrix ← 状态协方差矩阵(含初始值)
状态更新运算:通过重载 +, +=, -- 运算符实现流形上的状态加/减。
2.2 LiDAR数据包: LidarMeasureGroup (common_lib.h)
LidarMeasureGroup
├── lidar_frame_beg_time : double ← LiDAR帧开始时间
├── lidar_frame_end_time : double ← LiDAR帧结束时间
├── last_lio_update_time : double ← 上一次LIO更新时间
├── lidar : PointCloudXYZI::Ptr ← 原始点云
├── pcl_proc_cur : PointCloudXYZI::Ptr ← 当前时间窗口内的待处理点云
├── pcl_proc_next : PointCloudXYZI::Ptr ← 下一时间窗口点云(用于应对高速运动)
├── measures : deque<MeasureGroup> ← 图像/IMU测量组队列
├── lio_vio_flg : EKF_STATE ← 当前处理模式 (WAIT/VIO/LIO/LO)
└── lidar_scan_index_now : int ← 当前扫描索引
2.3 测量组: MeasureGroup (common_lib.h)
MeasureGroup
├── vio_time : double ← 图像捕获时间(已补偿曝光延迟)
├── lio_time : double ← LiDAR帧结束时间
├── imu : deque<ImuConstPtr> ← IMU数据队列
└── img : cv::Mat ← 图像数据
2.4 点云点与协方差: pointWithVar (common_lib.h)
pointWithVar
├── point_b : V3D ← LiDAR body系下的点坐标
├── point_i : V3D ← IMU body系下的点坐标
├── point_w : V3D ← 世界系下的点坐标
├── var_nostate : M3D ← 去除状态协方差后的测量协方差
├── body_var : M3D ← body系下的协方差
├── var : M3D ← 总协方差
├── point_crossmat : M3D ← 点的叉积矩阵(用于Jacobian计算)
└── normal : V3D ← 法向量(体素平面拟合得到)
2.5 视觉子稀疏地图: SubSparseMap (vio.h)
SubSparseMap
├── propa_errors : vector<float> ← NCC传播误差(基于IMU先验的像素误差)
├── errors : vector<float> ← NCC最终误差
├── warp_patch : vector<vector<float>> ← 图像块(patches)
├── search_levels : vector<int> ← 搜索金字塔层级
├── voxel_points : vector<VisualPoint*> ← 视觉体素点
├── inv_expo_list : vector<double> ← 逆曝光时间列表
└── add_from_voxel_map : vector<pointWithVar> ← 来自体素地图的新点
2.6 点面约束: PointToPlane (voxel_map.h)
PointToPlane
├── point_b_ : V3D ← 点在body系下的坐标
├── point_w_ : V3D ← 点在世界系下的坐标
├── normal_ : V3D ← 平面法向量
├── center_ : V3D ← 平面中心
├── plane_var_ : 6×6 ← 平面参数协方差(含法向量+中心)
├── body_cov_ : M3D ← body系协方差
├── layer_ : int ← 八叉树层级
├── d_ : double ← 平面偏移量
├── eigen_value_ : double ← 特征值
├── is_valid_ : bool ← 是否有效
└── dis_to_plane_ : float ← 点到平面的距离
2.7 体素平面: VoxelPlane (voxel_map.h)
VoxelPlane
├── center_ : V3D ← 平面中心点
├── normal_ : V3D ← 平面法向量
├── y_normal_ : V3D ← 平面Y轴方向(用于可视化/坐标轴)
├── x_normal_ : V3D ← 平面X轴方向
├── covariance_ : M3D ← 点云协方差矩阵
├── plane_var_ : 6×6 ← 平面参数协方差
├── radius_ : float ← 平面半径
├── min/mid/max_eigen_value_ : float ← 特征值(用于平面性判断)
├── d_ : float ← 平面方程偏移量 n·p = d
├── points_size_ : int ← 支撑点数
├── is_plane_ : bool ← 是否被判定为平面
├── is_init_ : bool ← 是否已初始化
├── is_update_ : bool ← 是否已更新
└── id_ : int ← 平面ID
3. 模块接口关系
3.1 LIVMapper 与外部模块的聚合关系
LIVMapper
├── p_pre : PreprocessPtr → LiDAR点云预处理
├── p_imu : ImuProcessPtr → IMU传播/去畸变
├── voxelmap_manager : VoxelMapManagerPtr → LiDAR体素地图管理
├── vio_manager : VIOManagerPtr → 视觉前端
└── voxel_map : unordered_map<VOXEL_LOCATION, VoxelOctoTree*> → 体素地图(全局)
3.2 接口调用关系图
LIVMapper::run()
│
▼
LIVMapper::sync_packages()
┌──────────┼──────────────┐
│ │ │
▼ ▼ ▼
handleLIO handleVIO gravityAlignment
│ │
▼ ▼
processImu processImu
│ │
▼ ▼
ImuProcess:: VIOManager::
Process2() processFrame()
│ │
├──────────┤
▼ ▼
VoxelMapManager::StateEstimation()
│
├── BuildResidualListOMP()
│ └── build_single_residual()
│ └── VoxelOctoTree::find_correspond()
│ → point-to-plane residual
│
└── UpdateVoxelMap()
└── VoxelOctoTree::Insert()
└── UpdateOctoTree()
└── init_plane() / cut_octo_tree()
3.3 VIOManager 内部调用链
VIOManager::processFrame(img, pg, feat_map, img_time)
│
├── 1. updateFrameState(state) ← 更新帧位姿(从EKF状态)
├── 2. retrieveFromVisualSparseMap ← 从稀疏视觉地图检索可见点
│ └── 对每个点:
│ ├── getWarpMatrixAffine ← 计算仿射变换矩阵
│ ├── warpAffine ← 从参考帧提取图像块
│ ├── getBestSearchLevel ← 确定最佳搜索层级
│ ├── NCC跟踪(4层级金字塔) ← 归一化互相关匹配
│ └── 计算光度误差存入SubSparseMap
│
├── 3. computeJacobianAndUpdateEKF ← 计算视觉Jacobian,更新EKF
│ ├── computeProjectionJacobian ← 投影Jacobian
│ └── EKF更新公式: K = P H^T (H P H^T + R)^(-1)
│
├── 4. updateVisualMapPoints ← 更新视觉地图点
├── 5. generateVisualMapPoints ← 生成新的视觉地图点
└── 6. projectPatchFromRefToCur ← 投影参考图像块到当前帧(用于下一次跟踪)
3.4 VoxelMapManager 内部调用链
VoxelMapManager::StateEstimation(state_propagat)
│
├── TransformLidar() ← 将点云从LiDAR系变换到IMU系
├── downSizeFilterSurf ← PCL体素滤波降采样
│
├── BuildResidualListOMP() ← OMP并行构建点面残差列表
│ └── build_single_residual(pv, current_octo, layer, is_success, prob, single_ptpl)
│ ├── VoxelOctoTree::find_correspond(pw) ← 查找对应体素节点
│ ├── 计算点到平面距离 dis = n·pw - d
│ └── 计算概率 prob = exp(-0.5 * dis² / σ²)
│
├── EKF Update ← 基于残差进行EKF状态更新
│ └── 迭代 max_iterations 次
│
├── UpdateVoxelMap(pv_list) ← 更新体素地图
│ └── VoxelOctoTree::Insert(pv) ← 插入新点
│ └── UpdateOctoTree(pv) ← 更新八叉树并拟合平面
│
├── mapSliding() ← 滑动地图窗口(清除远距离体素)
└── pubVoxelMap() ← 发布体素地图可视化
3.5 LIVMapper 数据同步逻辑 sync_packages()
根据 SLAM 模式和当前 EKF 状态,采用不同的数据同步策略:
sync_packages(meas)
│
├── 模式: LIVO
│ ├── EKF状态 == WAIT
│ │ └── 从imu_buffer取出IMU数据打包
│ │
│ ├── EKF状态 == LIO (LiDAR帧已到,需要处理LiDAR)
│ │ ├── 收到新LiDAR数据 → 打包LiDAR帧(LIO模式)
│ │ ├── 收到新图像 → 根据曝光时间裁剪点云:
│ │ │ ├── 曝光前的点 → pcl_proc_cur(与当前LIO帧一起处理)
│ │ │ └── 曝光后的点 → pcl_proc_next(留到下一帧)
│ │ │ 打包图像帧(VIO模式)
│ │ └── return
│ │
│ ├── EKF状态 == VIO (图像帧已处理,等待LiDAR)
│ │ └── 触发下一帧LiDAR到达后更新LIO
│ │
│ └── EKF状态 == LO (纯LiDAR模式)
│ └── 打包LiDAR帧(LO模式)
│
├── 模式: ONLY_LIO
│ └── 打包完整LiDAR帧
│
└── 模式: ONLY_LO
└── 打包完整LiDAR帧
4. 数据流详解
4.1 完整数据处理管道
ROS Topic LIVMapper 内部
────────── ────────────────
LiDAR (livox/pcl) ───→ imu_cbk()
│
▼
Preprocess::process() → 点云预处理+特征提取
(livox/velodyne/ouster/XT32/Pandar128/Robosense)
│
lid_raw_data_buffer (deque<PointCloudXYZI::Ptr>)
lid_header_time_buffer (deque<double>)
│
IMU ───→ livox_pcl_cbk() │
standard_pcl_cbk() │
│
├── sync_packages() → LidarMeasureGroup
│ │
Image ───→ img_cbk() │ │
img_buffer │ │
img_time_buffer │ │
│ │
▼ ▼
handleLIO() / handleVIO() ←── meas.lio_vio_flg
│
▼
processImu()
ImuProcess::Process2()
├── IMU_init() ← IMU初始化(前200帧)
│ └── 初始化重力、偏置、速度
├── imu时间补偿与传播
│ └── 前向EKF传播(状态+协方差)
└── UndistortPcl() ← LiDAR点云去畸变
│
▼
stateEstimationAndMapping()
│
┌──────┴──────┐
│ │
▼ ▼
┌──────────┐ ┌──────────┐
│ VIO路径 │ │ LIO路径 │
└──────────┘ └──────────┘
│ │
▼ ▼
VIOManager:: VoxelMapManager::
processFrame() StateEstimation()
│ │
│ ▼
│ BuildResidualListOMP()
│ (点面残差)
│ │
│ ▼
│ EKF Update (迭代)
│ │
│ ▼
│ UpdateVoxelMap()
│ (地图更新)
│
▼
(可选,仅LIVO模式)
视觉全局BA→更新EKF
│
▼
publish_frame_world() ← 发布彩色点云地图
publish_odometry() ← 发布里程计
publish_path() ← 发布轨迹
publish_img_rgb() ← 发布视觉特征图像
4.2 时间同步机制
系统维护三个时间缓存队列,由 sync_packages() 统一协调:
时间轴:
LiDAR: ──●────●────●────●─────●────●────●────
│ │ │ │ │ │ │
│ └────┴────┴─────┘ │ │
│ LIO帧 │ │
│ │ │
Image: ──────■─────────────■─────■────■────
│ │ │
│ 曝光时间偏移 │ │
│ VIO帧 │ │
▼ ▼ ▼
sync模式: LIO VIO LIO LIO VIO LIO
数据切割策略:
- 当图像到达时(LIO → VIO切换):
- 将当前LiDAR点云按曝光时间分为两部分
pcl_proc_cur: 曝光时间前的点(立即处理)pcl_proc_next: 曝光时间后的点(保留到下一帧)
- 当LiDAR帧结束时(VIO → LIO切换):
- 将
pcl_proc_next合并到新的LIO帧
- 将
4.3 IMU 传播流程
ImuProcess::Process2(lidar_meas, state, cur_pcl_un_)
│
├── 首次需要初始化:
│ └── IMU_init() ← 静态初始化:计算平均加速度/角速度
│
├── 前向传播:
│ ├── 遍历imu_buffer → 积分状态
│ └── 更新协方差(离散时间状态转移)
│
├── 点云去畸变:
│ └── UndistortPcl() → 补偿运动畸变
│ 对每个LiDAR点:
│ 1. 根据时间戳插值姿态
│ 2. 将点从扫面结束时刻变换到起始时刻
│
└── 返回去畸变后的点云 cur_pcl_un_
4.4 体素地图构建流程
VoxelOctoTree 数据结构 (八叉树):
┌─────────────┐
│ Root Node │
│ layer = 0 │
└──────┬──────┘
┌────────┼────────┐
┌─────┴──┐ ... ┌──┴─────┐
│ Leaf 0 │ │ Leaf 7 │
│layer=L │ │layer=L │
│plane= │ │plane= │
│VoxelPlane│ │VoxelPlane│
└────────┘ └────────┘
建图流程:
1. Insert(pointWithVar):
- 计算点的体素坐标VOXEL_LOCATION
- 若体素内点数 < 阈值 → 存入temp_points
- 若点数 >= 阈值 → init_plane() 初始化平面
- 若体素继续分裂 → cut_octo_tree() 八叉树分裂
2. UpdateOctoTree(pointWithVar):
- 将新点加入后重新拟合平面
- 更新 plane_ptr_ 中的平面参数(中心、法向量、协方差)
3. find_correspond(V3D pw):
- 向下搜索八叉树到叶子节点
- 返回包含该点的最深层体素节点
- 用于点面残差计算
4.5 视觉前端跟踪流程
VIOManager::processFrame() 详细流程:
1. 更新帧位姿
└── updateFrameState(state)
└── new_frame_->T_f_w_ = [Rcl * R * Rli | Pcl + Rcl * P]
其中: R = rot_end, P = pos_end (来自EKF)
2. 从视觉稀疏地图检索
│ 对每个已有点:
│ ├── 投影到当前帧: pc = w2c(p_w)
│ ├── 检查是否在图像内 → 继续
│ ├── 获取仿射变换矩阵 warp matrix
│ ├── 获取参考patch, 进行warp
│ ├── 在图像金字塔上搜索匹配(4层)
│ └── 找到最佳匹配 → 计算光度误差
│
└── 存储到 visual_submap (SubSparseMap)
3. Jacobian计算与EKF更新
│ ├── 对每个有效点计算投影Jacobian
│ └── 构建 H_T_H 并执行EKF更新
│ H = [∂r/∂δθ, ∂r/∂δp, ∂r/∂δExpo, 0...]
│ 其中 r 是光度残差
4. 更新视觉地图点
└── updateVisualMapPoints()
└── 对每个点更新其在体素地图中的位置
5. 生成新地图点
└── generateVisualMapPoints()
├── 对每个不满足条件的网格:
│ └── 从体素地图获取该位置的点云
└── 创建新的 VisualPoint 并插入地图
6. 参考Patch更新
└── 将当前帧作为新的参考帧,更新所有可见点的参考Patch
5. SLAM 模式与状态机
5.1 三种 SLAM 模式
| 模式 | 枚举值 | 描述 | 输入 |
|---|---|---|---|
| ONLY_LO | 0 | 纯激光里程计 | 仅LiDAR |
| ONLY_LIO | 1 | 激光-惯性里程计 | LiDAR + IMU |
| LIVO | 2 | 激光-惯性-视觉里程计(完整模式) | LiDAR + IMU + Camera |
5.2 EKF 状态机
enum EKF_STATE { WAIT=0, VIO=1, LIO=2, LO=3 };
状态转换图 (仅 LIVO 模式):
WAIT ──(LiDAR到达+IMU初始化完成)──→ LIO
│ │
│ │
└──(图像到达)──→ VIO ←──(LiDAR未到达)──┘
│
└──(LiDAR到达)──→ LIO
LIO: LiDAR/IMU处理主导
VIO: 图像处理主导
WAIT: 等待初始化数据
5.3 主循环调度逻辑
LIVMapper::run()
│
├── ros::spinOnce() ← 处理ROS回调
│
├── processImu() ← 始终运行,传播IMU状态
│
├── sync_packages(meas) ← 数据同步
│ └── 成功?
│ ├── YES → handleLIO() / handleVIO()
│ └── NO → continue
│
├── stateEstimationAndMapping() ← EKF更新+地图构建
│
├── 发布结果:
│ ├── publish_odometry()
│ ├── publish_frame_world()
│ ├── publish_effect_world()
│ ├── publish_path()
│ ├── publish_img_rgb()
│ └── publish_mavros()
│
└── imu_prop_callback() ← 定时器,发布IMU传播里程计
6. EKF 状态向量
6.1 状态定义
状态向量 x ∈ ℝ¹⁹:
x = [δθ₁, δθ₂, δθ₃, ← 姿态误差 (SO(3)流形)
δp₁, δp₂, δp₃, ← 位置误差
δExpo, ← 逆曝光时间误差(1维)
δv₁, δv₂, δv₃, ← 速度误差
δbg₁, δbg₂, δbg₃, ← 陀螺仪偏置误差
δba₁, δba₂, δba₃, ← 加速度计偏置误差
δg₁, δg₂, δg₃]ᵀ ← 重力向量误差
6.2 协方差索引
| 索引 | 状态 | 维度 |
|---|---|---|
| 0-2 | 姿态 δθ | 3 |
| 3-5 | 位置 δp | 3 |
| 6 | 逆曝光时间 δExpo | 1 |
| 7-9 | 速度 δv | 3 |
| 10-12 | 陀螺偏置 δbg | 3 |
| 13-15 | 加速度偏置 δba | 3 |
| 16-18 | 重力 δg | 3 |
6.3 EKF 更新来源
| 更新源 | 残差类型 | 观测量维度 |
|---|---|---|
| LiDAR点面残差 | 点到平面距离 r = n·pw - d | 每点1维 |
| 视觉光度残差 | 灰度差 r = I_ref(patch) - I_cur(warped patch) | 每点patch_size²维 |
| IMU传播 | 状态预测(无额外观测残差,用于状态传播) | 过程模型 |
7. 关键函数调用链
7.1 初始化流程
main()
└── LIVMapper(nh)
├── readParameters(nh) ← 读取YAML参数
│ └── 加载:LiDAR/IMU/Camera外参、滤波器参数、模式选择
├── initializeComponents()
│ ├── p_pre = Preprocess() ← 创建点云预处理
│ ├── p_imu = ImuProcess() ← 创建IMU处理
│ ├── voxelmap_manager = new ← 创建体素地图管理器
│ ├── vio_manager = new VIOManager() ← 创建VIO管理器
│ └── 初始化各种点云、路径、发布器
│
├── initializeFiles() ← 初始化日志文件
│ └── 创建: fout_pre, fout_out, fout_visual_pos, fout_lidar_pos
│
├── initializeSubscribersAndPublishers(nh, it)
│ ├── sub_pcl ← LiDAR点云订阅
│ ├── sub_imu ← IMU订阅
│ ├── sub_img ← 图像订阅
│ └── 多种Publisher发布器
│
└── gravityAlignment() ← 重力对齐(IMU初始化)
7.2 ROS 回调函数
imu_cbk(msg_in)
└── imu_buffer.push_back(msg_in) ← 缓存IMU数据
livox_pcl_cbk(msg_in) (Livox LiDAR)
├── Preprocess::avia_handler()
│ └── 提取点云,计算曲率(curvature = 时间偏移ms)
├── lid_raw_data_buffer.push_back(pcl_out)
└── lid_header_time_buffer.push_back(time)
standard_pcl_cbk(msg_in) (Velodyne/Ouster/Hesai等)
├── Preprocess::process()
│ ├── oust64_handler()
│ ├── velodyne_handler()
│ ├── xt32_handler()
│ ├── Pandar128_handler()
│ └── robosense_handler()
├── lid_raw_data_buffer.push_back(pcl_out)
└── lid_header_time_buffer.push_back(time)
img_cbk(msg_in)
├── getImageFromMsg(msg_in) ← ROS图像→ cv::Mat
├── img_buffer.push_back(img)
└── img_time_buffer.push_back(time)
7.3 LIO 处理流程
handleLIO()
├── processImu()
│ └── p_imu->Process2(meas, _state, feats_undistort)
│ ├── IMU_init() ← 首帧进行IMU初始化
│ ├── 前向传播状态 ← 遍历所有缓存的IMU数据
│ └── UndistortPcl() ← 点云去畸变
│
└── stateEstimationAndMapping()
└── voxelmap_manager->StateEstimation(state_propagat)
├── TransformLidar() ← 点云坐标变换 LiDAR→IMU→World
├── 体素滤波降采样
├── BuildResidualListOMP() ← 构建点面残差
│ └── build_single_residual() → 计算距离和概率
├── 迭代EKF更新 (最多max_iterations次)
├── UpdateVoxelMap() ← 更新体素地图
│ └── 对每个点Insert到八叉树
└── mapSliding() ← 滑动窗口管理
7.4 VIO 处理流程
handleVIO()
├── 更新相机曝光时间补偿
│ └── exposure_time = exposure_time_init / _state.inv_expo_time
│
├── processImu()
│ └── p_imu->Process2(meas, _state, feats_undistort)
│
└── vio_manager->processFrame(img, _pv_list, voxel_map, img_time)
└── 详见第4.5节视觉前端跟踪流程
7.5 LiDAR 与 VIO 结果融合
在 LIVO 模式下,VIO 和 LIO 交替更新 EKF 状态:
时间线:
LIO更新 → 更新状态_1 → VIO更新 → 更新状态_2 → LIO更新 → ...
每次更新:
1. LIO: VoxelMapManager::StateEstimation()
└── 点面残差 → EKF更新 → 更新体素地图
2. VIO: VIOManager::processFrame()
└── 光度残差 → EKF更新 → 更新视觉地图
两种更新的EKF状态共享 _state (StatesGroup),
从而实现LiDAR和视觉的紧耦合融合。
8. ROS 接口
8.1 订阅 Topic
| Topic | 类型 | 回调函数 | 描述 |
|---|---|---|---|
lid_topic | sensor_msgs::PointCloud2 / livox_ros_driver::CustomMsg | standard_pcl_cbk() / livox_pcl_cbk() | LiDAR点云输入 |
imu_topic | sensor_msgs::Imu | imu_cbk() | IMU数据输入 |
img_topic | sensor_msgs::Image | img_cbk() | 图像数据输入 |
8.2 发布 Topic
| Topic | 类型 | 发布函数 | 描述 |
|---|---|---|---|
/aft_mapped_to_init | nav_msgs::Odometry | publish_odometry() | EKF里程计+TF |
/path | nav_msgs::Path | publish_path() | 移动轨迹 |
/cloud_registered | sensor_msgs::PointCloud2 | publish_frame_world() | 世界系点云(含RGB) |
/cloud_effected | sensor_msgs::PointCloud2 | publish_effect_world() | 有效残差点云 |
/visual_sub_map | sensor_msgs::PointCloud2 | publish_visual_sub_map() | 视觉子地图 |
/rgb_img | sensor_msgs::Image | publish_img_rgb() | 视觉特征图像 |
/voxel_map_plane | visualization_msgs::MarkerArray | voxelmap_manager->pubVoxelMap() | 体素平面可视化 |
/imu_prop_odom | nav_msgs::Odometry | imu_prop_timer回调 | IMU传播里程计(高频) |
/mavros/vision_pose/pose | geometry_msgs::PoseStamped | publish_mavros() | MAVROS飞控输出 |
8.3 参数配置 (readParameters)
| 参数 | 类型 | 默认值 | 描述 |
|---|---|---|---|
slam_mode | int | LIVO=2 | SLAM模式选择 |
lid_topic | string | - | LiDAR Topic名称 |
imu_topic | string | - | IMU Topic名称 |
img_topic | string | - | 图像Topic名称 |
lidar_en | int | 1 | 是否启用LiDAR |
img_en | int | 1 | 是否启用相机 |
imu_en | bool | true | 是否启用IMU |
lidar_type | int | AVIA=1 | LiDAR传感器类型 |
blind_rgb_points | double | 0.0 | RGB点盲区距离 |
filter_size_surf_min | double | 0.5 | 面特征滤波尺寸 |
filter_size_pcd | double | 0.5 | 点云滤波尺寸 |
max_iterations | int | 4 | EKF最大迭代次数 |
gyr_cov | double | - | 陀螺仪噪声协方差 |
acc_cov | double | - | 加速度计噪声协方差 |
gravity_est_en | bool | true | 是否估计重力 |
ba_bg_est_en | bool | true | 是否估计偏置 |
exposure_estimate_en | bool | false | 是否估计曝光时间 |
exposure_time_init | double | 0.0 | 初始曝光时间 |
IMG_POINT_COV | double | - | 视觉点协方差 |
pcd_save_en | bool | false | 是否保存PCD |
img_save_en | bool | false | 是否保存图像 |
hilti_en | bool | false | HILTI数据集兼容模式 |
附录: 文件依赖关系图
main.cpp
└── LIVMapper.h ────────────┬──────────────────────────────┐
│ │ │
▼ ▼ ▼
IMU_Processing.h ── common_lib.h ───────┬────── preprocess.h
│ │ │ │
│ │ │ ▼
│ │ │ livox_ros_driver/
│ │ │ CustomMsg.h
│ │ │
▼ ▼ ▼
(IMU_Processing.cpp) utils/so3_math.h feature.h
utils/types.h frame.h
utils/color.h visual_point.h
voxel_map.h
│
▼
vio.h
│
▼
feature.h
voxel_map.h
扫一扫
479
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
