【Basalt】 VIO（sqrt_keypoint_vio）主流程measure函数梳理

basalt vio measure 接口解析

cpp 复制代码

改动在 src/vi_estimator/sqrt_keypoint_vio.cpp 的 measure() 里，新增了这 6 个分段标记：

step 1. IMU prediction and state propagation
step 2. Register current visual observations
step 3. Keyframe decision and landmark triangulation
step 4. Marginalization prep and optimization
step 5. Publish state and visualization
step 6. Timing and return


IMU 推进状态
记录当前视觉观测
决定是否插关键帧并三角化新点
做优化与边缘化准备
输出状态和可视化数据
统计耗时并返回

cpp 复制代码

bool SqrtKeypointVioEstimator<Scalar_>::measure(
    const OpticalFlowResult::Ptr& opt_flow_meas,
    const typename IntegratedImuMeasurement<Scalar>::Ptr& meas)

1. 这个接口的作用

本质上是 VIO 前后端的一次"测量更新入口"。

它每来一帧图像光流结果 opt_flow_meas，以及这一帧前后的 IMU 预积分 meas，就做一整套事：

用 IMU 预积分 把状态从上一时刻传播到当前图像时刻
把当前图像中观测到的特征点接入地图数据库
判断要不要插入关键帧
如果插关键帧，就对新特征做 运动三角化
调用后端 optimize_and_marg(...) 做优化和边缘化
输出状态给外部线程
输出可视化需要的数据

所以它不是单一"measure = EKF更新公式"那种窄意义接口，

而是一个 整帧视觉-惯性处理总入口。

cpp 复制代码

bool SqrtKeypointVioEstimator<Scalar_>::measure(
    const OpticalFlowResult::Ptr& opt_flow_meas,
    const typename IntegratedImuMeasurement<Scalar>::Ptr& meas) {
  stats_sums_.add("frame_id", opt_flow_meas->t_ns).format("none");
  Timer t_total;

  // step 1. IMU prediction and state propagation
  if (meas.get()) {
    BASALT_ASSERT(frame_states[last_state_t_ns].getState().t_ns ==
                  meas->get_start_t_ns());
    BASALT_ASSERT(opt_flow_meas->t_ns ==
                  meas->get_dt_ns() + meas->get_start_t_ns());
    BASALT_ASSERT(meas->get_dt_ns() > 0);

    PoseVelBiasState<Scalar> next_state =
        frame_states.at(last_state_t_ns).getState();
    meas->predictState(frame_states.at(last_state_t_ns).getState(), g,
                       next_state);

    last_state_t_ns = opt_flow_meas->t_ns;
    next_state.t_ns = opt_flow_meas->t_ns;

    frame_states[last_state_t_ns] = PoseVelBiasStateWithLin<Scalar>(next_state);

    imu_meas[meas->get_start_t_ns()] = *meas;
  }

  // step 2. Register current visual observations
  // save results
  prev_opt_flow_res[opt_flow_meas->t_ns] = opt_flow_meas;

  // Make new residual for existing keypoints
  int connected0 = 0;
  std::map<int64_t, int> num_points_connected;
  std::unordered_set<int> unconnected_obs0;
  //loop over all cameras, stereo num = 2
  for (size_t i = 0; i < opt_flow_meas->observations.size(); i++) {
    TimeCamId tcid_target(opt_flow_meas->t_ns, i);
    if (opt_flow_meas->observations[i].empty()) {
      continue;
    }
    // 遍历该相机下所有特征点观测, kv_obs.first = 特征点 ID,kv_obs.second = 该特征点的观测位置
    for (const auto& kv_obs : opt_flow_meas->observations[i]) {
      int kpt_id = kv_obs.first;

      if (lmdb.landmarkExists(kpt_id)) {
        const TimeCamId& tcid_host = lmdb.getLandmark(kpt_id).host_kf_id;//取出该路标最初被初始化时所属的 host keyframe

        KeypointObservation<Scalar> kobs;
        kobs.kpt_id = kpt_id;
        kobs.pos = kv_obs.second.translation().cast<Scalar>();//构造一个标准化的路标观测,pos 是像素坐标/归一化平面上的 2D 位置

        lmdb.addObservation(tcid_target, kobs);//把"当前帧看到了这个已有路标"的事实记入路标数据库
        // obs[tcid_host][tcid_target].push_back(kobs);

        if (num_points_connected.count(tcid_host.frame_id) == 0) {
          num_points_connected[tcid_host.frame_id] = 0;
        }
        num_points_connected[tcid_host.frame_id]++;//统计当前帧与某个 host 帧之间建立了多少连接点

        if (i == 0) connected0++;//如果是 camera 0，就把"已连接点数"加一
      } else {
        // 如果这个点还不是地图点, 且它来自 camera 0 ,那就记为"未连接特征", 这些点后面是新建地图点的候选
        if (i == 0) {
          unconnected_obs0.emplace(kpt_id);
        }
      }
    }
  }
  //控制kf的插入，防止kf过于频繁
  // TODO: 只统计 camera 0，有点偏单目 host 逻辑,判断条件改为相对于局部地图的情况，单个新的信息相对于局部多有的信息的比例，而不是单纯的相对于当前帧的观测数量的比例
  // 单目逻辑，对双目系统来说，这个信息利用不完整，它只看数量，不看质量，参考时间空间逻辑
  //vio_new_kf_keypoints_thresh = 0.7， vio_min_frames_after_kf = 5
  if (Scalar(connected0) / (connected0 + unconnected_obs0.size()) < Scalar(config.vio_new_kf_keypoints_thresh) && 
      frames_after_kf > config.vio_min_frames_after_kf) 
    take_kf = true;

  if (config.vio_debug) {
    std::cout << "connected0 " << connected0 << " unconnected0 " << unconnected_obs0.size() << std::endl;
  }

  // step 3. Keyframe decision and landmark triangulation
  if (take_kf) {
    // Triangulate new points from one of the observations (with sufficient baseline) and make keyframe for camera 0
    take_kf = false;
    frames_after_kf = 0;
    kf_ids.emplace(last_state_t_ns);

    TimeCamId tcidl(opt_flow_meas->t_ns, 0);

    int num_points_added = 0;
    // int num_stereo_triangulated = 0;  // Counter for stereo triangulation
    int num_motion_triangulated = 0;  // Counter for motion-based triangulation
    
    for (int lm_id : unconnected_obs0) {//对每个当前新出现的特征点尝试建图
      // Find all observations
      std::map<TimeCamId, KeypointObservation<Scalar>> kp_obs;

      for (const auto& kv : prev_opt_flow_res) {
        for (size_t k = 0; k < kv.second->observations.size(); k++) {
          auto it = kv.second->observations[k].find(lm_id);
          if (it != kv.second->observations[k].end()) {
            TimeCamId tcido(kv.first, k);//构造历史观测对应的 (时刻, 相机)

            KeypointObservation<Scalar> kobs;//构造观测对象
            kobs.kpt_id = lm_id;
            kobs.pos = it->second.translation().template cast<Scalar>();

            // obs[tcidl][tcido].push_back(kobs);
            kp_obs[tcido] = kobs;//收集这个点在历史所有帧/相机中的观测
          }
        }
      }

      // triangulate
      bool valid_kp = false;
      const Scalar min_triang_distance2 = Scalar(config.vio_min_triangulation_dist *
                 config.vio_min_triangulation_dist);
      // =====================================================================
      // METHOD 2: MOTION-BASED TRIANGULATION (different frames)
      // Original Basalt method - requires camera motion
      // =====================================================================
      // TODO： different obs selection strategies, e.g. if obs from stereo pair is available, 
      // prefer that for better triangulation robustness, otherwise fall back to motion-based triangulation
      // this find solution is simple but not optimal, as it doesn't consider the quality of different observations 
      // (e.g. viewing angle, baseline, reprojection error, etc.)
      for (const auto& kv_obs : kp_obs) {
        if (valid_kp) break;
        TimeCamId tcido = kv_obs.first;

        const Vec2 p0 = opt_flow_meas->observations.at(0) //当前关键帧 camera 0 中该点的 2D 观测
                            .at(lm_id)
                            .translation()
                            .cast<Scalar>();
        const Vec2 p1 = prev_opt_flow_res[tcido.frame_id] // 历史某帧某相机中该点的 2D 观测
                            ->observations[tcido.cam_id]
                            .at(lm_id)
                            .translation()
                            .template cast<Scalar>();

        Vec4 p0_3d, p1_3d;
        bool valid1 = calib.intrinsics[0].unproject(p0, p0_3d);
        bool valid2 = calib.intrinsics[tcido.cam_id].unproject(p1, p1_3d);
        if (!valid1 || !valid2) continue; //把像素点反投影成相机坐标系下的归一化视线方向
        
        // 计算当前关键帧 IMU 到历史帧 IMU 的相对位姿
        SE3 T_i0_i1 = getPoseStateWithLin(tcidl.frame_id).getPose().inverse() *
                      getPoseStateWithLin(tcido.frame_id).getPose(); 
        // 把 IMU 系相对位姿转成"当前相机0 到 历史相机"的相对位姿
        SE3 T_0_1 =
            calib.T_i_c[0].inverse() * T_i0_i1 * calib.T_i_c[tcido.cam_id];

        if (T_0_1.translation().squaredNorm() < min_triang_distance2) continue;

        //两条视线和相对位姿做三角化，得到该点在当前相机坐标系下的 3D 位置和逆深度
        Vec4 p0_triangulated = triangulate(p0_3d.template head<3>(),
                                           p1_3d.template head<3>(), T_0_1);

        if (p0_triangulated.array().isFinite().all() && p0_triangulated[3] > 0 && p0_triangulated[3] < 3.0) {
          //构造一个新路标,host 关键帧就是当前帧 camera 0,把 3D 方向参数化成 stereographic 表示,为了更稳定地做最优化，而不是直接存 3D 点坐标
          Keypoint<Scalar> kpt_pos;
          kpt_pos.host_kf_id = tcidl;
          kpt_pos.direction = StereographicParam<Scalar>::project(p0_triangulated);
          kpt_pos.inv_dist = p0_triangulated[3];
          lmdb.addLandmark(lm_id, kpt_pos);

          num_points_added++;
          num_motion_triangulated++;
          valid_kp = true;
        }
      }

      if (valid_kp) {
        //只要成功初始化，就把它历史上所有观测也一起接入, 后端优化时就能利用这些多视图观测
        for (const auto& kv_obs : kp_obs) {
          lmdb.addObservation(kv_obs.first, kv_obs.second);
        }
      }
    }
    
    // Always print triangulation statistics (useful for debugging initialization)
    // std::cout << "[VIO-Triangulate] unconnected=" << unconnected_obs0.size()
    //           << " added=" << num_points_added 
    //           << " | stereo=" << num_stereo_triangulated 
    //           << " motion=" << num_motion_triangulated << std::endl;

    num_points_kf[opt_flow_meas->t_ns] = num_points_added;
  } else {
    frames_after_kf++;
  }

  // step 4. Marginalization prep and optimization
  std::unordered_set<KeypointId> lost_landmaks;//存"当前帧没再看到"的路标点
  if (config.vio_marg_lost_landmarks) {//如果配置里允许边缘化丢失路标
    for (const auto& kv : lmdb.getLandmarks()) {//遍历所有地图点
      bool connected = false;//先假设当前帧没有观测到它
      for (size_t i = 0; i < opt_flow_meas->observations.size(); i++) {//如果当前任一相机看到了这个点，就认为它还"活着"
        if (opt_flow_meas->observations[i].count(kv.first) > 0)
          connected = true;
      }
      if (!connected) {
        lost_landmaks.emplace(kv.first);//当前完全没看到的点，加入待边缘化集合
      }
    }
  }

  //后端优化和边缘化
  optimize_and_marg(num_points_connected, lost_landmaks);

  // step 5. Publish state and visualization
  if (out_state_queue) {
    PoseVelBiasStateWithLin p = frame_states.at(last_state_t_ns);

    typename PoseVelBiasState<double>::Ptr data(
        new PoseVelBiasState<double>(p.getState().template cast<double>()));

    out_state_queue->push(data);
  }

  if (out_vis_queue) {
    VioVisualizationData::Ptr data(new VioVisualizationData);

    data->t_ns = last_state_t_ns;

    // 把所有状态位姿加入可视化轨迹
    for (const auto& kv : frame_states) {
      data->states.emplace_back(kv.second.getState().T_w_i.template cast<double>());
    }
    // 把关键帧/历史帧位姿加入可视化
    for (const auto& kv : frame_poses) {
      data->frames.emplace_back(kv.second.getPose().template cast<double>());
    }
    //获取当前地图点的 3D 位置，加入可视化
    get_current_points(data->points, data->point_ids);

    //计算当前状态下各地图点的投影,用于可视化重投影效果
    data->projections.resize(opt_flow_meas->observations.size());
    computeProjections(data->projections, last_state_t_ns);
    
    //保存当前光流观测，方便显示"观测 vs 投影"
    data->opt_flow_res = prev_opt_flow_res[last_state_t_ns];
    
    if (config.vio_debug) {
      size_t total_proj = 0;
      for (size_t i = 0; i < data->projections.size(); i++) {
        total_proj += data->projections[i].size();
      }
      size_t total_flow_obs = 0;
      if (data->opt_flow_res) {
        for (const auto& obs : data->opt_flow_res->observations) {
          total_flow_obs += obs.size();
        }
      }
      std::cout << "[VIO-Output] landmarks=" << lmdb.numLandmarks()
                << " observations=" << lmdb.numObservations()
                << " 3D_points=" << data->points.size()
                << " projections=" << total_proj
                << " flow_obs=" << total_flow_obs << std::endl;
    }

    out_vis_queue->push(data);
  }

  // step 6. Timing and return
  last_processed_t_ns = last_state_t_ns;

  stats_sums_.add("measure", t_total.elapsed()).format("ms");

  return true;
}

2. 输入输出

输入

1）`opt_flow_meas`

类型：

cpp 复制代码

const OpticalFlowResult::Ptr&

含义：

当前图像时刻的光流/特征跟踪结果，里面至少包含：

当前帧时间戳 t_ns
多相机观测 observations[i]
每个观测里是 kpt_id -> 像素坐标/平移量

它提供了：

当前帧看到了哪些特征点
这些特征点在各相机中的 2D 位置

2）`meas`

类型：

cpp 复制代码

const typename IntegratedImuMeasurement<Scalar>::Ptr&

含义：

从上一帧到当前帧之间的 IMU 预积分结果

它通常包含：

起始时刻 get_start_t_ns()
时间间隔 get_dt_ns()
预积分得到的旋转、速度、位移增量
以及可能隐含的协方差/Jacobian 信息

这里它被用于：

cpp 复制代码

meas->predictState(...)

即：从上一时刻状态预测当前状态。

输出

返回值：

cpp 复制代码

return true;

这里返回值本身没太多信息量，几乎就是"处理成功"。

真正的输出其实是副作用：

更新了 frame_states
更新了 lmdb
更新了 imu_meas
更新了 prev_opt_flow_res
调用了 optimize_and_marg(...)
往 out_state_queue 推了状态
往 out_vis_queue 推了可视化数据

所以这函数更像 系统推进一步，不是简单函数式输入输出。

3. 逐段 + 逐行代码注释版

下面我给你一个"尽量接近逐行"的注释版。

我会把每行背后的目的写出来，不只翻译注释。

3.1 函数开头

cpp 复制代码

bool SqrtKeypointVioEstimator<Scalar_>::measure(
    const OpticalFlowResult::Ptr& opt_flow_meas,
    const typename IntegratedImuMeasurement<Scalar>::Ptr& meas) {

measure(...)：VIO 处理一帧测量数据的主入口
opt_flow_meas：当前图像帧的特征跟踪结果
meas：上一帧到当前帧之间的 IMU 预积分测量

cpp 复制代码

  stats_sums_.add("frame_id", opt_flow_meas->t_ns).format("none");

记录当前处理的帧 ID（这里用时间戳代替 frame_id）
主要用于统计/调试输出

cpp 复制代码

  Timer t_total;

开一个总计时器，后面统计整个 measure() 花了多久

3.2 用 IMU 预积分做状态传播

cpp 复制代码

  if (meas.get()) {

如果提供了 IMU 预积分，则先做预测
说明这个系统支持纯视觉时刻 or 没有 IMU 的情况，但正常 VIO 流程这里通常有 IMU

cpp 复制代码

    BASALT_ASSERT(frame_states[last_state_t_ns].getState().t_ns ==
                  meas->get_start_t_ns());

断言：当前保存的"上一状态时间戳"必须等于 IMU 预积分起点
否则说明时间轴错了

cpp 复制代码

    BASALT_ASSERT(opt_flow_meas->t_ns ==
                  meas->get_dt_ns() + meas->get_start_t_ns());

断言：当前图像时间戳 = IMU 起始时间 + IMU 时间跨度
即图像时刻和预积分终点必须对齐

cpp 复制代码

    BASALT_ASSERT(meas->get_dt_ns() > 0);

断言 IMU 时间间隔必须大于 0
防止异常数据

cpp 复制代码

    PoseVelBiasState<Scalar> next_state =
        frame_states.at(last_state_t_ns).getState();

拿出上一时刻状态，复制一份到 next_state
后面会在这个基础上做预测

状态一般包含：

位姿 T_w_i
速度 v_w_i
陀螺 bias
加计 bias
时间戳

cpp 复制代码

这是你贴的原注释
观察是对的：这段代码显式只看到"状态传播"
协方差传播不在这段明写
可能：
1. 在 predictState(...) 内部做了部分事情
2. 或者这个系统主要是优化法，协方差不靠 EKF 主递推保存
3. 或者线性化信息/先验在别的结构里维护

cpp 复制代码

    meas->predictState(frame_states.at(last_state_t_ns).getState(), g,
                       next_state);

用 IMU 预积分，从上一状态预测当前状态
g 是重力
输入：上一状态
输出：next_state

这一步数学上就是：

利用预积分量更新旋转
利用重力和速度更新位置
更新 bias 相关状态（通常 bias 本身可能保持不变或随机游走）

cpp 复制代码

    last_state_t_ns = opt_flow_meas->t_ns;

最新状态时间戳更新到当前图像时刻

cpp 复制代码

    next_state.t_ns = opt_flow_meas->t_ns;

把预测出来的状态时间戳也改成当前帧时刻

cpp 复制代码

    frame_states[last_state_t_ns] = PoseVelBiasStateWithLin<Scalar>(next_state);

把当前预测状态存入 frame_states
包装成 PoseVelBiasStateWithLin
说明除了状态本身，这个结构还可能带有线性化点/增量等信息

cpp 复制代码

    imu_meas[meas->get_start_t_ns()] = *meas;

把这段 IMU 预积分保存起来
后面优化/重线性化时可能还要用

3.3 保存当前光流结果

cpp 复制代码

  prev_opt_flow_res[opt_flow_meas->t_ns] = opt_flow_meas;

按时间戳保存当前光流结果
为后续：
- 三角化
- 重投影
- 可视化
- 历史观测查找
  做准备

3.4 把当前观测接入已有路标，并统计连接情况

cpp 复制代码

  int connected0 = 0;

统计 camera 0 中，当前帧有多少特征点已经是"已知路标"

cpp 复制代码

  std::map<int64_t, int> num_points_connected;

记录：当前帧和每个 host keyframe 之间连接了多少点
后面优化/边缘化可能用到

cpp 复制代码

  std::unordered_set<int> unconnected_obs0;

记录 camera 0 中当前帧看到、但还不在地图中的特征点 ID
后面如果插关键帧，可以尝试把它们三角化成新地图点

cpp 复制代码

  for (size_t i = 0; i < opt_flow_meas->observations.size(); i++) {

遍历所有相机

cpp 复制代码

    TimeCamId tcid_target(opt_flow_meas->t_ns, i);

构造当前观测所属的 (时间戳, 相机编号) 标识

cpp 复制代码

    if (opt_flow_meas->observations[i].empty()) {
      continue;
    }

如果这个相机没有观测，跳过

cpp 复制代码

    for (const auto& kv_obs : opt_flow_meas->observations[i]) {

遍历该相机下所有特征点观测
kv_obs.first = 特征点 ID
kv_obs.second = 该特征点的观测位置

cpp 复制代码

      int kpt_id = kv_obs.first;

取出特征点 ID

cpp 复制代码

      if (lmdb.landmarkExists(kpt_id)) {

如果这个点已经是地图中的路标点

cpp 复制代码

        const TimeCamId& tcid_host = lmdb.getLandmark(kpt_id).host_kf_id;

取出该路标最初被初始化时所属的 host keyframe

cpp 复制代码

        KeypointObservation<Scalar> kobs;
        kobs.kpt_id = kpt_id;
        kobs.pos = kv_obs.second.translation().cast<Scalar>();

构造一个标准化的路标观测
pos 是像素坐标/归一化平面上的 2D 位置

cpp 复制代码

        lmdb.addObservation(tcid_target, kobs);

把"当前帧看到了这个已有路标"的事实记入路标数据库

cpp 复制代码

        if (num_points_connected.count(tcid_host.frame_id) == 0) {
          num_points_connected[tcid_host.frame_id] = 0;
        }
        num_points_connected[tcid_host.frame_id]++;

统计当前帧与某个 host 帧之间建立了多少连接点

cpp 复制代码

        if (i == 0) connected0++;

如果是 camera 0，就把"已连接点数"加一

cpp 复制代码

      } else {
        if (i == 0) {
          unconnected_obs0.emplace(kpt_id);
        }
      }

如果这个点还不是地图点
且它来自 camera 0
那就记为"未连接特征"
这些点后面是新建地图点的候选

3.5 关键帧判定

cpp 复制代码

  if (Scalar(connected0) / (connected0 + unconnected_obs0.size()) <
          Scalar(config.vio_new_kf_keypoints_thresh) &&
      frames_after_kf > config.vio_min_frames_after_kf)
    take_kf = true;

含义很关键：

如果当前 camera 0 中：

已连接点已连接点+未连接点<阈值 \frac{\text{已连接点}}{\text{已连接点}+\text{未连接点}} < \text{阈值} 已连接点+未连接点已连接点<阈值

并且距离上次关键帧已经隔了足够多帧，

那么插入关键帧。

直观理解：

如果当前帧大部分点都已经是老地图点，说明视角没怎么拓展，不急着插关键帧
如果当前帧出现很多新点，说明场景内容变了/视角拓展了，适合插关键帧

cpp 复制代码

  if (config.vio_debug) {
    std::cout << "connected0 " << connected0 << " unconnected0 "
              << unconnected_obs0.size() << std::endl;
  }

调试打印，方便看关键帧触发依据

3.6 如果要插关键帧：初始化新路标

cpp 复制代码

  if (take_kf) {

如果决定插关键帧

cpp 复制代码

    take_kf = false;

清掉标志，防止重复用

cpp 复制代码

    frames_after_kf = 0;

自上次关键帧后的帧数清零

cpp 复制代码

    kf_ids.emplace(last_state_t_ns);

把当前时刻记成关键帧 ID

cpp 复制代码

    TimeCamId tcidl(opt_flow_meas->t_ns, 0);

当前关键帧默认挂在 camera 0 上作为 host

cpp 复制代码

    int num_points_added = 0;
    int num_motion_triangulated = 0;

统计新加入多少路标
其中多少是通过运动三角化得到的

遍历所有未连接点，尝试初始化为 3D 路标

cpp 复制代码

    for (int lm_id : unconnected_obs0) {

对每个当前新出现的特征点尝试建图

cpp 复制代码

      std::map<TimeCamId, KeypointObservation<Scalar>> kp_obs;

用来收集这个点在历史所有帧/相机中的观测

cpp 复制代码

      for (const auto& kv : prev_opt_flow_res) {
        for (size_t k = 0; k < kv.second->observations.size(); k++) {
          auto it = kv.second->observations[k].find(lm_id);
          if (it != kv.second->observations[k].end()) {

遍历历史光流结果，查这个点以前是否也被看到过

cpp 复制代码

            TimeCamId tcido(kv.first, k);

构造历史观测对应的 (时刻, 相机)

cpp 复制代码

            KeypointObservation<Scalar> kobs;
            kobs.kpt_id = lm_id;
            kobs.pos = it->second.translation().template cast<Scalar>();

构造观测对象

cpp 复制代码

            kp_obs[tcido] = kobs;

存下来

设置三角化最小基线

cpp 复制代码

      bool valid_kp = false;

标记这个点最后是否初始化成功

cpp 复制代码

      const Scalar min_triang_distance2 =
          Scalar(config.vio_min_triangulation_dist *
                 config.vio_min_triangulation_dist);

设一个最小平移基线阈值的平方
基线太小不做三角化，否则深度会很不稳定

用当前帧和某个历史观测做两视图三角化

cpp 复制代码

      for (const auto& kv_obs : kp_obs) {
        if (valid_kp) break;

遍历这个点的历史观测
只要找到一组可用的，就停止

cpp 复制代码

        TimeCamId tcido = kv_obs.first;

当前候选历史观测对应的 (历史时刻, 相机号)

cpp 复制代码

        const Vec2 p0 = opt_flow_meas->observations.at(0)
                            .at(lm_id)
                            .translation()
                            .cast<Scalar>();

当前关键帧 camera 0 中该点的 2D 观测

cpp 复制代码

        const Vec2 p1 = prev_opt_flow_res[tcido.frame_id]
                            ->observations[tcido.cam_id]
                            .at(lm_id)
                            .translation()
                            .template cast<Scalar>();

历史某帧某相机中该点的 2D 观测

cpp 复制代码

        Vec4 p0_3d, p1_3d;

存放反投影后的射线/齐次方向

cpp 复制代码

        bool valid1 = calib.intrinsics[0].unproject(p0, p0_3d);
        bool valid2 = calib.intrinsics[tcido.cam_id].unproject(p1, p1_3d);

把像素点反投影成相机坐标系下的归一化视线方向

cpp 复制代码

        if (!valid1 || !valid2) continue;

如果反投影失败，这组观测无效

cpp 复制代码

        SE3 T_i0_i1 = getPoseStateWithLin(tcidl.frame_id).getPose().inverse() *
                      getPoseStateWithLin(tcido.frame_id).getPose();

计算当前关键帧 IMU 到历史帧 IMU 的相对位姿

如果记：

当前帧 IMU 位姿为 (T_{w i_0})
历史帧 IMU 位姿为 (T_{w i_1})

则：

Ti0i1=Twi0−1Twi1 T_{i_0 i_1} = T_{w i_0}^{-1} T_{w i_1} Ti0i1=Twi0−1Twi1

cpp 复制代码

        SE3 T_0_1 =
            calib.T_i_c[0].inverse() * T_i0_i1 * calib.T_i_c[tcido.cam_id];

把 IMU 系相对位姿转成"当前相机0 到历史相机"的相对位姿

如果 (T_{i c}) 表示 camera 到 imu 或 imu 到 camera，要看约定；

这里只看代码组合形式，目的是得到 两次观测相机坐标系之间的相对变换。

cpp 复制代码

        if (T_0_1.translation().squaredNorm() < min_triang_distance2) continue;

如果两次观测相机之间平移太小，不做三角化
因为小基线深度很不稳定

cpp 复制代码

        Vec4 p0_triangulated = triangulate(p0_3d.template head<3>(),
                                           p1_3d.template head<3>(), T_0_1);

用两条视线和相对位姿做三角化
输出这个点在当前相机0坐标系下的参数化结果

cpp 复制代码

        if (p0_triangulated.array().isFinite().all() &&
            p0_triangulated[3] > 0 && p0_triangulated[3] < 3.0) {

检查三角化结果是否合法
第 4 维这里大概率是 inv_dist / inverse depth / inverse distance
要求：
- 有限
- 深度为正
- 不能太近（或逆深度不能太大）

cpp 复制代码

          Keypoint<Scalar> kpt_pos;
          kpt_pos.host_kf_id = tcidl;

构造一个新路标
host 关键帧就是当前帧 camera 0

cpp 复制代码

          kpt_pos.direction =
              StereographicParam<Scalar>::project(p0_triangulated);

把 3D 方向参数化成 stereographic 表示
这是为了更稳定地做最优化，而不是直接存 3D 点坐标

cpp 复制代码

          kpt_pos.inv_dist = p0_triangulated[3];

保存逆距离参数

cpp 复制代码

          lmdb.addLandmark(lm_id, kpt_pos);

正式把这个新点加入地图数据库

cpp 复制代码

          num_points_added++;
          num_motion_triangulated++;
          valid_kp = true;

更新统计并标记成功

如果新路标建好了，把它所有历史观测也补进数据库

cpp 复制代码

      if (valid_kp) {
        for (const auto& kv_obs : kp_obs) {
          lmdb.addObservation(kv_obs.first, kv_obs.second);
        }
      }

只要成功初始化，就把它历史上所有观测也一起接入
后端优化时就能利用这些多视图观测

cpp 复制代码

    num_points_kf[opt_flow_meas->t_ns] = num_points_added;

记录当前关键帧加了多少点

cpp 复制代码

  } else {
    frames_after_kf++;
  }

如果没插关键帧，就累计"距离上次关键帧过去了多少帧"

3.7 找丢失的路标

cpp 复制代码

  std::unordered_set<KeypointId> lost_landmaks;

存"当前帧没再看到"的路标点

cpp 复制代码

  if (config.vio_marg_lost_landmarks) {

如果配置里允许边缘化丢失路标

cpp 复制代码

    for (const auto& kv : lmdb.getLandmarks()) {

遍历所有地图点

cpp 复制代码

      bool connected = false;

先假设当前帧没有观测到它

cpp 复制代码

      for (size_t i = 0; i < opt_flow_meas->observations.size(); i++) {
        if (opt_flow_meas->observations[i].count(kv.first) > 0)
          connected = true;
      }

如果当前任一相机看到了这个点，就认为它还"活着"

cpp 复制代码

      if (!connected) {
        lost_landmaks.emplace(kv.first);
      }

当前完全没看到的点，加入待边缘化集合

3.8 后端优化和边缘化

cpp 复制代码

  optimize_and_marg(num_points_connected, lost_landmaks);

这是整段代码最核心的一句之一。

含义：

根据当前所有状态、路标、观测构造优化问题
做视觉/惯性联合优化
并把该边缘化的老状态、老点、丢失点边缘化掉

所以这段函数虽然开头有 predictState，但真正的"校正"不是 EKF 一步公式，

而是在 optimize_and_marg(...) 里通过非线性优化完成。

3.9 输出状态

cpp 复制代码

  if (out_state_queue) {

如果外部订阅了状态输出

cpp 复制代码

    PoseVelBiasStateWithLin p = frame_states.at(last_state_t_ns);

取当前时刻状态

cpp 复制代码

    typename PoseVelBiasState<double>::Ptr data(
        new PoseVelBiasState<double>(p.getState().template cast<double>()));

转成 double 类型
供外部线程/模块使用

cpp 复制代码

    out_state_queue->push(data);

推到状态队列里

3.10 输出可视化数据

cpp 复制代码

  if (out_vis_queue) {
    VioVisualizationData::Ptr data(new VioVisualizationData);

如果需要可视化，则创建一个可视化数据对象

cpp 复制代码

    data->t_ns = last_state_t_ns;

当前时刻

cpp 复制代码

    for (const auto& kv : frame_states) {
      data->states.emplace_back(
          kv.second.getState().T_w_i.template cast<double>());
    }

把所有状态位姿加入可视化轨迹

cpp 复制代码

    for (const auto& kv : frame_poses) {
      data->frames.emplace_back(kv.second.getPose().template cast<double>());
    }

把关键帧/历史帧位姿加入可视化

cpp 复制代码

    get_current_points(data->points, data->point_ids);

获取当前地图点

cpp 复制代码

    data->projections.resize(opt_flow_meas->observations.size());
    computeProjections(data->projections, last_state_t_ns);

计算当前状态下各地图点的投影
用于可视化重投影效果

cpp 复制代码

    data->opt_flow_res = prev_opt_flow_res[last_state_t_ns];

保存当前光流观测，方便显示"观测 vs 投影"

cpp 复制代码

    if (config.vio_debug) {

如果开调试，则打印统计信息

cpp 复制代码

      size_t total_proj = 0;
      for (size_t i = 0; i < data->projections.size(); i++) {
        total_proj += data->projections[i].size();
      }

统计总投影数量

cpp 复制代码

      size_t total_flow_obs = 0;
      if (data->opt_flow_res) {
        for (const auto& obs : data->opt_flow_res->observations) {
          total_flow_obs += obs.size();
        }
      }

统计当前总观测数

cpp 复制代码

      std::cout << "[VIO-Output] landmarks=" << lmdb.numLandmarks()
                << " observations=" << lmdb.numObservations()
                << " 3D_points=" << data->points.size()
                << " projections=" << total_proj
                << " flow_obs=" << total_flow_obs << std::endl;

打印：
- 路标数
- 观测数
- 当前3D点数
- 投影数
- 当前光流观测数

cpp 复制代码

    out_vis_queue->push(data);

推送可视化数据

3.11 收尾

cpp 复制代码

  last_processed_t_ns = last_state_t_ns;

记录最后处理完成的时间戳

cpp 复制代码

  stats_sums_.add("measure", t_total.elapsed()).format("ms");

统计这次 measure() 耗时多少毫秒

cpp 复制代码

  return true;
}

返回成功

4. 数学推导

你要的"数学推导"，这段代码核心涉及 4 块：

IMU 预积分状态传播
关键帧判定准则
两视图运动三角化
后端优化目标（虽然代码没展开，但可以写出它在做什么）

4.1 IMU 状态传播

VIO 状态通常写成：

x∗k=(R∗wik,p∗wik,v∗wik,b∗gk,b∗ak) \mathbf{x}*k = \left( \mathbf{R}*{w i_k}, \mathbf{p}*{w i_k}, \mathbf{v}*{w i_k}, \mathbf{b}*{g_k}, \mathbf{b}*{a_k} \right) x∗k=(R∗wik,p∗wik,v∗wik,b∗gk,b∗ak)

其中：

(\mathbf{R}_{w i_k})：IMU 在世界系下旋转
(\mathbf{p}_{w i_k})：IMU 在世界系下位置
(\mathbf{v}_{w i_k})：IMU 在世界系下速度
(\mathbf{b}_{g_k})：陀螺偏置
(\mathbf{b}_{a_k})：加计偏置

IMU 原始模型：

ωm=ω+bg+ng \boldsymbol{\omega}_m = \boldsymbol{\omega} + \mathbf{b}_g + \mathbf{n}_g ωm=ω+bg+ng

am=a+ba+na \mathbf{a}_m = \mathbf{a} + \mathbf{b}_a + \mathbf{n}_a am=a+ba+na

连续时间运动学：

R˙∗wi=R∗wi[ωm−bg−n∗g]∗× \dot{\mathbf{R}}*{w i} = \mathbf{R}*{w i} [\boldsymbol{\omega}_m - \mathbf{b}_g - \mathbf{n}*g]*\times R˙∗wi=R∗wi[ωm−bg−n∗g]∗×

v˙∗wi=g+R∗wi(am−ba−na) \dot{\mathbf{v}}*{w i} = \mathbf{g} + \mathbf{R}*{w i}(\mathbf{a}_m - \mathbf{b}_a - \mathbf{n}_a) v˙∗wi=g+R∗wi(am−ba−na)

p˙∗wi=v∗wi \dot{\mathbf{p}}*{w i} = \mathbf{v}*{w i} p˙∗wi=v∗wi

预积分后，从时刻 (k) 到 (k+1) 有：

R∗wi∗k+1 \mathbf{R}*{w i*{k+1}} R∗wi∗k+1

R∗wikΔR∗k,k+1 \mathbf{R}*{w i_k}\Delta \mathbf{R}*{k,k+1} R∗wikΔR∗k,k+1

v∗wi∗k+1\mathbf{v}*{w i*{k+1}} v∗wi∗k+1
v∗wik+gΔt+R∗wikΔvk,k+1 \mathbf{v}*{w i_k} + \mathbf{g}\Delta t + \mathbf{R}*{w i_k}\Delta \mathbf{v}_{k,k+1} v∗wik+gΔt+R∗wikΔvk,k+1

p∗wik+v∗wikΔt+12gΔt2+R∗wikΔp∗k,k+1 \mathbf{p}*{w i_k} + \mathbf{v}*{w i_k}\Delta t + \frac{1}{2}\mathbf{g}\Delta t^2 + \mathbf{R}*{w i_k}\Delta \mathbf{p}*{k,k+1} p∗wik+v∗wikΔt+21gΔt2+R∗wikΔp∗k,k+1

这就是这里：

cpp 复制代码

meas->predictState(...)

背后在干的事情。

4.2 为什么你说"没看到协方差传播"

如果是经典 EKF，会有：

Pk+1∣k=F∗kP∗k∣kFk⊤+GkQkGk⊤ \mathbf{P}_{k+1|k} = \mathbf{F}*k \mathbf{P}*{k|k}\mathbf{F}_k^\top + \mathbf{G}_k\mathbf{Q}_k\mathbf{G}_k^\top Pk+1∣k=F∗kP∗k∣kFk⊤+GkQkGk⊤

但在这段代码里并没有看到显式：

状态协方差 §
状态转移 Jacobian (F)
噪声 Jacobian (G)
噪声协方差 (Q)

所以你的判断是对的：

这段不是 EKF 风格主循环代码。

更像是：

前端先做 IMU 状态预测
后端在 optimize_and_marg(...) 里维护 Hessian / 先验 / 线性化信息

也就是说，它大概率是 优化式 VIO，不是纯 EKF-VIO。

4.3 关键帧判定准则

代码里关键帧条件是：

NconnectedNconnected+Nunconnected<τ \frac{N_{\text{connected}}} {N_{\text{connected}} + N_{\text{unconnected}}} < \tau Nconnected+NunconnectedNconnected<τ

其中：

(NconnectedN_{\text{connected}}Nconnected)：当前 camera0 中已经属于地图点的观测数
(NunconnectedN_{\text{unconnected}}Nunconnected)：当前 camera0 中新出现的未建图特征数
(\tau = \text{vio_new_kf_keypoints_thresh})

这个量反映的是：

当前帧被已有地图解释的程度

如果这个比例低，说明：

当前视角进入了新区域
或者场景变化较大
或者需要扩大地图覆盖

所以需要插关键帧。

4.4 两视图三角化推导

第一步：像素反投影成归一化视线

像素点 ( u=(u,v)\mathbf{u} = (u,v)u=(u,v) )

通过相机模型反投影成射线方向：

f=π−1(u) \mathbf{f} = \pi^{-1}(\mathbf{u}) f=π−1(u)

代码对应：

cpp 复制代码

calib.intrinsics[...].unproject(...)

得到的是归一化方向 (f0,f1\mathbf{f}_0, \mathbf{f}_1f0,f1)

第二步：建立两相机坐标系关系

设当前相机为 0，历史相机为 1：

X∗1=R∗01X∗0+t∗01 \mathbf{X}*1 = \mathbf{R}*{01}\mathbf{X}*0 + \mathbf{t}*{01} X∗1=R∗01X∗0+t∗01

代码里的：

cpp 复制代码

SE3 T_0_1

就是这个相对位姿。

第三步：同一空间点在两个视线中共线

空间点在相机0中：

X0=λ0f0 \mathbf{X}_0 = \lambda_0 \mathbf{f}_0 X0=λ0f0

在相机1中：

X1=λ1f1 \mathbf{X}_1 = \lambda_1 \mathbf{f}_1 X1=λ1f1

又因为：

X∗1=R∗01X∗0+t∗01 \mathbf{X}*1 = \mathbf{R}*{01}\mathbf{X}*0 + \mathbf{t}*{01} X∗1=R∗01X∗0+t∗01

代入得：

λ1f1\lambda_1 \mathbf{f}_1λ1f1

R01(λ0f∗0)+t∗01 \mathbf{R}_{01}(\lambda_0 \mathbf{f}*0) + \mathbf{t}*{01} R01(λ0f∗0)+t∗01

整理：

λ1f∗1−λ0R∗01f∗0=t∗01 \lambda_1 \mathbf{f}*1 - \lambda_0 \mathbf{R}*{01}\mathbf{f}*0 = \mathbf{t}*{01} λ1f∗1−λ0R∗01f∗0=t∗01

这是一个线性最小二乘问题，可以解 (λ0,λ1\lambda_0,\lambda_1λ0,λ1)。

然后：

X0=λ0f0 \mathbf{X}_0 = \lambda_0 \mathbf{f}_0 X0=λ0f0

就得到点在当前相机0下的三维坐标。

第四步：为什么要检查 baseline

如果 (∣t01∣|\mathbf{t}_{01}|∣t01∣) 太小，那么：

两条视线几乎从同一点发出
深度解对噪声极其敏感
三角化病态

所以代码里要求：

∣t01∣2>dmin⁡2 |\mathbf{t}{01}|^2 > d{\min}^2 ∣t01∣2>dmin2

即：

cpp 复制代码

if (T_0_1.translation().squaredNorm() < min_triang_distance2) continue;

4.5 stereographic + inverse distance 参数化

代码没有直接存 3D 点坐标，而是存：

cpp 复制代码

kpt_pos.direction
kpt_pos.inv_dist

这意味着点被参数化为：

一个方向
一个逆距离

假设点在 host 相机系下表示为：

X=1ρd \mathbf{X} = \frac{1}{\rho}\mathbf{d} X=ρ1d

其中：

(d\mathbf{d}d)：单位方向或由 stereographic 参数恢复的方向
(ρ\rhoρ)：逆距离 inv_dist

为什么这样做：

比直接优化 XYZ 更稳定
远点更好处理
单目/弱基线初值更自然

4.6 后端优化的数学形式

虽然代码没展开，但 optimize_and_marg(...) 通常在做：

min⁡X∑rimu⊤Wimurimu+∑rproj⊤Wprojrproj+∑rprior⊤Wpriorrprior \min_{\mathcal{X}} \sum r_{\text{imu}}^\top W_{\text{imu}} r_{\text{imu}} + \sum r_{\text{proj}}^\top W_{\text{proj}} r_{\text{proj}} + \sum r_{\text{prior}}^\top W_{\text{prior}} r_{\text{prior}} Xmin∑rimu⊤Wimurimu+∑rproj⊤Wprojrproj+∑rprior⊤Wpriorrprior

其中：

1）IMU 残差

约束相邻状态与预积分一致：

rimu=[rR rv rp rbg rba] r_{\text{imu}} = \begin{bmatrix} r_R \ r_v \ r_p \ r_{bg} \ r_{ba} \end{bmatrix} rimu=[rR rv rp rbg rba]

2）视觉重投影残差

对每个路标观测：

rproj=z∗ij−π(T∗ciwXj) r_{\text{proj}} = \mathbf{z}*{ij} - \pi\left( T*{c_i w} X_j \right) rproj=z∗ij−π(T∗ciwXj)

其中：

(zij\mathbf{z}_{ij}zij)：第 (i) 帧对第 (j) 个点的观测
(X_j)：地图点
(π(⋅)\pi(\cdot)π(⋅))：投影模型

3）边缘化先验

把被边缘化掉的历史状态浓缩成一个先验项：

rprior=Hpδx−bp r_{\text{prior}} = H_p \delta x - b_p rprior=Hpδx−bp

这也是为什么你没看到 EKF 的 § 在这里显式滚动。

优化法很多时候维护的是 先验 Hessian / 线性化残差，不是传统协方差矩阵递推。

5. 这段代码的真正逻辑主线

你可以把它记成一句话：

先用 IMU 把状态推到当前图像时刻，再把视觉观测接入地图；如果新内容够多就插关键帧并三角化新点；最后统一做后端优化和边缘化。

6. 你这里最值得注意的点

点1：这不是 EKF 的"measure update"风格

虽然名字叫 measure，但不是：

先 predict
再用卡尔曼增益 update

这种形式。

它更像：

前端推进一帧
后端统一最优化

点2：协方差传播不在这里显式出现

你原注释：

cpp 复制代码

//TODO 没看到协方差的传播

这个判断是很关键的。

说明你不能把这段按"误差状态卡尔曼"那套硬套完。

更合理的理解是：

predictState 只负责名义状态传播
协方差/先验/Hessian 可能在 PoseVelBiasStateWithLin 或 optimize_and_marg 里维护

点3：新路标初始化依赖"运动三角化"

也就是：

当前帧 camera0
历史帧某相机
有足够基线
才初始化新点

所以它不是每个新点都立刻建图，而是要满足几何条件。

【Basalt】 VIO（sqrt_keypoint_vio）主流程measure函数梳理

1. 这个接口的作用

2. 输入输出

输入

1）opt_flow_meas

2）meas

输出

3. 逐段 + 逐行代码注释版

3.1 函数开头

3.2 用 IMU 预积分做状态传播

3.3 保存当前光流结果

3.4 把当前观测接入已有路标，并统计连接情况

3.5 关键帧判定

3.6 如果要插关键帧：初始化新路标

遍历所有未连接点，尝试初始化为 3D 路标

设置三角化最小基线

用当前帧和某个历史观测做两视图三角化

如果新路标建好了，把它所有历史观测也补进数据库

3.7 找丢失的路标

3.8 后端优化和边缘化

3.9 输出状态

3.10 输出可视化数据

3.11 收尾

4. 数学推导

4.1 IMU 状态传播

4.2 为什么你说"没看到协方差传播"

4.3 关键帧判定准则

4.4 两视图三角化推导

第一步：像素反投影成归一化视线

第二步：建立两相机坐标系关系

第三步：同一空间点在两个视线中共线

第四步：为什么要检查 baseline

4.5 stereographic + inverse distance 参数化

4.6 后端优化的数学形式

1）IMU 残差

2）视觉重投影残差

3）边缘化先验

5. 这段代码的真正逻辑主线

6. 你这里最值得注意的点

点1：这不是 EKF 的"measure update"风格

点2：协方差传播不在这里显式出现

点3：新路标初始化依赖"运动三角化"

1）`opt_flow_meas`

2）`meas`