【Basalt】Basalt主流程梳理

一句话总结这个工程里的 VIO 主流程：

图像特征跟踪 + IMU 预积分预测 -> 关键帧管理与三角化建点 -> 滑窗联合优化 -> 边缘化 -> 输出位姿/速度/零偏

如果你愿意，我下一步可以继续帮你把这个文件画成一张"函数调用关系图"或者"时序图"，把 vio_run_mcap.cpp -> SqrtKeypointVioEstimator::initialize -> measure -> optimize -> marginalize 这一条链再梳成更直观的图。

这个文件本质上不是"VIO 算法实现本体"，而是一个把"数据读取 -> 前端光流/特征跟踪 -> 后端 VIO -> 可视化/轨迹输出"串起来的运行入口。主入口在 src/vio_run_mcap.cpp。

主流程
main() 先解析命令行、加载配置和标定，然后把数据集读进来，再创建前端和后端：

• 标定加载：load_data(...) 或 loadCalibrationFromMcap(...)，见 src/vio_run_mcap.cpp
• 数据集加载：DatasetIoFactory::getDatasetIo(...)->read(...)，见 src/vio_run_mcap.cpp
• 前端创建：OpticalFlowFactory::getOpticalFlow(vio_config, calib)，见 src/vio_run_mcap.cpp
• 后端创建：VioEstimatorFactory::getVioEstimator(...)，见 src/vio_run_mcap.cpp
• 后端启动：vio->initialize(...)，见 src/vio_run_mcap.cpp

然后程序起几个线程把数据流真正跑起来：

• feed_images() 把图像按时间戳推入 opt_flow_ptr->input_queue，见 src/vio_run_mcap.cpp
• feed_imu() 把 IMU 推入 vio->imu_data_queue，见 src/vio_run_mcap.cpp
• 光流输出先进入 vision_forward_queue，再转发给 vio->vision_data_queue，见 src/vio_run_mcap.cpp
• 后端估计出的状态从 out_state_queue 取出，累计成轨迹，见 src/vio_run_mcap.cpp

所以从程序视角看，链路就是：

Dataset -> feed_images/feed_imu -> OpticalFlow -> VIO Estimator -> 状态/轨迹输出

算法调用的主要接口

你如果要找"算法真正从哪里进"，主要盯这几个接口：

• OpticalFlowFactory::getOpticalFlow(...)

  作用：创建视觉前端。

  位置：src/vio_run_mcap.cpp

• VioEstimatorFactory::getVioEstimator(...)

  作用：创建 VIO 后端估计器。

  位置：src/vio_run_mcap.cpp

• VioEstimatorBase::initialize(...)

  作用：启动后端处理线程。

  接口定义在 include/basalt/vi_estimator/vio_estimator.h

• vision_data_queue / imu_data_queue

  作用：前后端之间最核心的数据入口队列。

  定义在 include/basalt/vi_estimator/vio_estimator.h

• out_state_queue

  作用：后端每处理完一帧后输出当前 PoseVelBiasState。

  定义在 include/basalt/vi_estimator/vio_estimator.h

==========================================================================

==========================================================================

如果往算法实现里继续钻，这个工程当前用的核心实现是 SqrtKeypointVioEstimator，主逻辑在 src/vi_estimator/sqrt_keypoint_vio.cpp。

SLAM/VIO 的主流程

按算法逻辑，可以概括成这条线：

1. 图像进入前端，提取/跟踪特征，形成 OpticalFlowResult

   入口连接点在 src/vio_run_mcap.cpp

2. IMU 连续进入后端队列

   见 src/vio_run_mcap.cpp

3. 后端处理线程启动后，循环取一帧视觉数据 curr_frame

   见 src/vi_estimator/sqrt_keypoint_vio.cpp

4. 如果系统还没初始化，就用最早 IMU 的重力方向估初始姿态

   见 src/vi_estimator/sqrt_keypoint_vio.cpp

5. 对相邻图像帧之间的 IMU 做预积分，得到 IntegratedImuMeasurement

   见 src/vi_estimator/sqrt_keypoint_vio.cpp

6. 调用 measure(curr_frame, meas) 处理当前帧

   这是每帧视觉惯导融合的核心入口，见 src/vi_estimator/sqrt_keypoint_vio.cpp

7. 在 measure() 里先用 IMU 预测当前状态
   meas->predictState(...)

   见 src/vi_estimator/sqrt_keypoint_vio.cpp

8. 把当前帧观测和已有地图点关联，给老地图点加新观测
   lmdb.addObservation(...)

   见 src/vi_estimator/sqrt_keypoint_vio.cpp

9. 判断是否插入关键帧

   判断依据是已连接点比例是否太低，见 src/vi_estimator/sqrt_keypoint_vio.cpp

10. 若插入关键帧，则对新特征做三角化，生成新 landmark
    lmdb.addLandmark(...)

    见 src/vi_estimator/sqrt_keypoint_vio.cpp

11. 调用 optimize_and_marg(...)

    先优化，再边缘化，见 src/vi_estimator/sqrt_keypoint_vio.cpp

12. optimize()

    构建滑窗状态的线性系统，视觉残差和 IMU 残差一起进优化，LM 迭代求解

    见 src/vi_estimator/sqrt_keypoint_vio.cpp

13. marginalize()

    对旧状态、旧关键帧、丢失地标做边缘化，维持固定大小滑窗

    见 src/vi_estimator/sqrt_keypoint_vio.cpp

14. 输出当前 PoseVelBiasState

    推到 out_state_queue，外层线程把它存成轨迹

    见 src/vi_estimator/sqrt_keypoint_vio.cpp