【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 的主流程

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

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

入口连接点在 src/vio_run_mcap.cpp
IMU 连续进入后端队列

见 src/vio_run_mcap.cpp
后端处理线程启动后，循环取一帧视觉数据 curr_frame

见 src/vi_estimator/sqrt_keypoint_vio.cpp
如果系统还没初始化，就用最早 IMU 的重力方向估初始姿态

见 src/vi_estimator/sqrt_keypoint_vio.cpp
对相邻图像帧之间的 IMU 做预积分，得到 IntegratedImuMeasurement

见 src/vi_estimator/sqrt_keypoint_vio.cpp
调用 measure(curr_frame, meas) 处理当前帧

这是每帧视觉惯导融合的核心入口，见 src/vi_estimator/sqrt_keypoint_vio.cpp
在 measure() 里先用 IMU 预测当前状态
meas->predictState(...)

见 src/vi_estimator/sqrt_keypoint_vio.cpp
把当前帧观测和已有地图点关联，给老地图点加新观测
lmdb.addObservation(...)

见 src/vi_estimator/sqrt_keypoint_vio.cpp
判断是否插入关键帧

判断依据是已连接点比例是否太低，见 src/vi_estimator/sqrt_keypoint_vio.cpp
若插入关键帧，则对新特征做三角化，生成新 landmark
lmdb.addLandmark(...)

见 src/vi_estimator/sqrt_keypoint_vio.cpp
调用 optimize_and_marg(...)

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

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

见 src/vi_estimator/sqrt_keypoint_vio.cpp
marginalize()

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

见 src/vi_estimator/sqrt_keypoint_vio.cpp
输出当前 PoseVelBiasState

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

见 src/vi_estimator/sqrt_keypoint_vio.cpp