【MSCKF】UpdaterSLAM::delayed_init 和 FeatureInitializer::single_triangulation

我们有两个主要函数：UpdaterSLAM::delayed_init 和 FeatureInitializer::single_triangulation。我们将梳理这两个函数的流程，并分析特征点初始化的条件。

1. UpdaterSLAM::delayed_init 流程
   这个函数的主要作用是对SLAM特征点进行延迟初始化（在满足一定条件时进行三角化并添加到状态向量中）。其步骤如下：

步骤0：准备工作
检查特征点向量feature_vec是否为空，为空则直接返回。
记录开始时间rT0。
步骤1：清理特征点测量值
获取当前状态中所有克隆的IMU状态的时间戳（clonetimes）。
遍历特征点向量，对每个特征点调用clean_old_measurements，以清理不在clonetimes中的测量（即不在当前状态克隆时间内的测量）。
统计每个特征点在各相机中的总测量数ct_meas。
如果总测量数小于2，则标记该特征点待删除，并从当前处理的向量中移除。否则保留。
步骤2：构建相机位姿克隆
遍历每个相机标定（state->_calib_IMUtoCAM），对于每个相机，构建其在不同克隆时间（IMU状态时间）下的位姿。
相机位姿包括：从全局坐标系到相机坐标系的旋转矩阵R_GtoCi和全局坐标系中相机的位置p_CioinG。
将这些位姿存储在clones_cam中，其结构为：map<相机ID, map<时间戳, ClonePose>>。
步骤3：三角化特征点
再次遍历特征点向量，对每个特征点进行三角化：
如果特征点的位置p_FinG是零（注意：这里通过(*it1)->p_FinG.x() == 0.0判断），则进行三角化。
根据配置选择使用1D或3D三角化方法（single_triangulation_1d或single_triangulation）。
如果配置需要，再使用高斯牛顿法进行优化（single_gaussnewton）。
如果三角化或优化失败，则移除该特征点。
如果特征点的位置已经非零（例如，之前已经三角化过？），则输出一条信息（似乎是在调试时观察）并跳过三角化。
注意：这里有一个条件判断特征点位置是否为零，只有为零的特征点才进行三角化。这可能是为了区分新特征点（需要三角化）和已经初始化过的特征点（只需进行后续处理）。
步骤4：将特征点加入到状态中
遍历剩余特征点，对每个特征点：
构建UpdaterHelperFeature对象，将特征点的测量信息填入。
根据特征点的ID（是否小于最大ARUCO特征数）确定特征点的表示方法（feat_rep）。
如果使用单逆深度表示，则将其转换为MSCKF的逆深度表示（通过将表示转换为ANCHORED_MSCKF_INVERSE_DEPTH），并在后续的雅可比计算后做相应的投影。
计算特征点的雅可比矩阵（get_feature_jacobian_full）得到H_f（特征点雅可比）、H_x（状态雅可比）和残差res。
对于单逆深度表示，调整雅可比矩阵：将深度相关的列从H_f移到H_x的末尾，然后进行零空间投影（nullspace_project_inplace）以消除表示冗余。
创建Landmark对象，并设置其属性（包括特征点ID、表示方法、锚点相机ID、锚点时间戳、3D位置等）。
尝试使用StateHelper::initialize将特征点加入状态向量。初始化时使用卡方检验（卡方乘子chi2_multipler减半，可能是为了更严格？）。
如果初始化成功，则将特征点加入state->_features_SLAM，并标记原特征点待删除。
如果失败，则移除特征点。
打印调试计时信息。
2. FeatureInitializer::single_triangulation 流程
这个函数用于三角化一个特征点的3D位置（在锚点相机坐标系下）。

步骤：
统计该特征点在所有相机中的总测量数，并选择测量次数最多的相机作为锚点相机（anchor_most_meas），同时使用该相机中最后时间戳作为锚点时间。
获取锚点相机的位姿（旋转R_GtoA和位置p_AinG）。
构建线性系统（矩阵A和向量b）：
遍历特征点的每个测量（每个相机，每个时间戳）：
获取该测量对应的相机位姿（R_GtoCi和p_CiinG）。
计算从锚点相机到当前相机的相对旋转R_AtoCi和相对位置p_CiinA。
获取归一化平面坐标b_i，并转换到锚点相机坐标系下，归一化。
计算该测量的投影矩阵Bperp（使用叉积矩阵skew_x），然后构建该测量对应的Ai和bi（Ai = Bperp^T * Bperp, bi = Ai * p_CiinA）。
累加到A和b。
求解线性方程：A * p_f = b，得到特征点在锚点相机坐标系下的3D坐标p_f。
检查条件数：计算矩阵A的条件数（最大奇异值除以最小奇异值），如果条件数超过阈值，或者深度值（p_f(2,0)）小于最小距离或大于最大距离，或者结果为NaN，则返回三角化失败。
将三角化得到的点从锚点相机坐标系转换到全局坐标系：p_FinG = R_GtoA^T * p_f + p_AinG。
3. 特征点初始化条件分析
综合上述两个函数，特征点初始化的条件如下：

条件1：特征点必须满足的最小测量要求
在步骤1中，特征点的总测量数（ct_meas）必须大于等于2。少于2个测量的特征点会被移除。
条件2：三角化成功
在步骤3中，特征点需要成功三角化（在锚点相机坐标系下的3D位置）。三角化的成功取决于：
线性方程的条件数不能太大（不超过_options.max_cond_number）。
三角化得到的深度值（z分量）必须在设定范围内（_options.min_dist到_options.max_dist之间）。
三角化的解不能为NaN。
条件3：高斯牛顿法优化成功（可选）
如果配置了refine_features为true，那么在进行三角化后，还需要进行高斯牛顿法优化。若优化失败，则特征点初始化失败。
条件4：初始化加入状态向量时通过卡方检验
在步骤4中，使用StateHelper::initialize将特征点加入到状态向量中，这需要满足卡方检验（即残差的卡方值小于阈值，阈值由chi2_multipler调整）。该函数内部会计算残差的马氏距离，并判断是否小于阈值（阈值自由度为残差的维度，乘子为chi2_multipler）。
条件5：特征点位置为零
在步骤3中，只对位置为零的特征点进行三角化（即(*it1)->p_FinG.x() == 0.0）。这表明只有尚未被三角化（初始化）的特征点才尝试三角化。如果特征点已经非零，则跳过三角化步骤，直接进行后续的加入状态向量操作（此时可能已经初始化过，但再次进行线性化和卡方检验？）。
其他条件：
特征点必须在当前状态克隆的时间戳内有测量（步骤1的清理会移除不在当前克隆时间内的测量）。
在步骤4中，特征点的锚点相机和时间戳已经设定（由三角化函数设定）。
结论
特征点初始化（三角化并加入状态向量）的条件总结为：

​​基本条件​​：特征点必须至少有两个有效测量（在不同的克隆时间）。
​​三角化条件​​：三角化过程必须成功（包括线性方程解的条件数和深度范围满足要求），如果配置了优化，则优化也必须成功。
​​加入状态的条件​​：在计算雅可比后，进行零空间投影（针对单逆深度表示）后，通过卡方检验（残差的马氏距离小于阈值）才能加入状态向量。
注意：对于已经三角化（位置非零）的特征点，不再进行三角化，但仍然进行后续的线性化和卡方检验（步骤4），以尝试将其加入状态向量。如果卡方检验失败，该特征点会被移除。

因此，整个初始化过程是严格筛选的，确保只有质量好的特征点才会加入到状态向量中，以维持系统的稳定性。

算法流程梳理
UpdaterSLAM::delayed_init 接口流程

graph TD
A[开始] --> B{特征点非空?}
–>是
C[清理
无效测量]

–>否
END[返回]

–> D[统计测量数量]

–> E{测量数≥2?}

–>是
F[构建相机位姿克隆]

–>否
G[移除特征点]

–> H{特征点位置为0?}

–>是
I[三角化+优化]

–>否
K[跳过三角化]

–> J{三角化/优化成功?}

–>是
L[计算雅可比矩阵]

–>否
M[移除特征点]

–> L

–> N[零空间投影]

–> O[创建Landmark对象]

–> P{初始化成功?}

–>是
Q[加入SLAM特征库]

–>否
R[移除特征点]

FeatureInitializer::single_triangulation 接口流程

graph TD
S[开始] --> T[确定锚点相机]
–> U[计算相对位姿]

–> V[构建线性系统]

–> W{求解线性方程}

–> X{满足条件?}

–>是
Y[计算全局坐标]

–>否
Z[失败返回]

–> ZZ[成功返回]

特征点初始化条件分析

核心条件总结：
测量数量要求（必须满足）：

至少2个有效测量：ct_meas ≥ 2

测量必须存在于当前克隆的时间戳内（通过clean_old_measurements过滤）
三角化条件（仅对新特征点）：

位置未初始化：p_FinG.x() == 0.0

三角化成功：

 c++
 success_tri = initializer_feat->single_triangulation(...)

矩阵条件数：condA ≤ max_cond_number

深度范围：min_dist ≤ p_f(2,0) ≤ max_dist

非NaN解：!std::isnan(p_f.norm())

优化成功（可选）：

 c++
 success_refine = initializer_feat->single_gaussnewton(...)

状态初始化条件（最终筛选）：

c++
StateHelper::initialize(…)

卡方检验通过：残差的马氏距离小于阈值（chi2_multipler调节）

雅可比矩阵有效性：H_x和H_f无数值问题

零空间投影成功（单逆深度表示时）

附加条件：
特征点类型处理：

ARUCO特征：featid < max_aruco_features

普通SLAM特征：不同噪声参数sigma_pix_sq和卡方乘子

表示方法适配：

c++
feat_rep = (is_aruco) ? feat_rep_aruco : feat_rep_slam

自动处理单逆深度/全局坐标表示转换

分析结论

特征点初始化必须同时满足三层筛选条件：
筛选阶段 必要条件 核心目的

预处理 1. 特征点存在
2. ≥2有效测量
3. 在当前克隆时间窗内 确保可用基础数据
几何验证 1. 位置未初始化
2. 三角化数值稳定
3. 深度值物理合理
4. 优化收敛（可选） 保证3D位置可靠性
统计验证 1. 残差通过卡方检验
2. 雅可比矩阵有效
3. 表示转换正确 确保与系统状态兼容

关键设计特点：
分级筛选机制：从简单（测量数）到复杂（统计验证）逐层过滤

适应性策略：

自动选择特征表示方法（ARUCO/SLAM）

处理不同表示法的数学一致性（单逆深度→MSCKF转换）
鲁棒性保障：

条件数检查避免病态问题

深度范围约束过滤异常值

零空间投影保证可观测性
效率优化：

跳过已初始化特征点的三角化

并行化处理特征向量

典型失败场景：
pie
title 初始化失败原因分布
“测量不足” ： 35
“三角化失败” ： 25
“深度越界” ： 20
“卡方检验失败” ： 15
“数值问题” ： 5

最终成功初始化的特征点将：
从feature_vec转移到state->_features_SLAM

拥有完整的锚点信息和协方差关联

可用于后续的状态优化和地图构建

void UpdaterSLAM::delayed_init(std::shared_ptr<State> state, std::vector<std::shared_ptr<Feature>> &feature_vec) {

  // Return if no features
  if (feature_vec.empty())
    return;

  //PRINT_DEBUG("\tDelayed init: feature_vec size: %d\n", feature_vec.size());

  // Start timing
  boost::posix_time::ptime rT0, rT1, rT2, rT3, rT4;
  rT0 = boost::posix_time::microsec_clock::local_time();

  // 0. Get all timestamps our clones are at (and thus valid measurement times)
  std::vector<double> clonetimes;
  for (const auto &clone_imu : state->_clones_IMU) {
    clonetimes.emplace_back(clone_imu.first);
  }

  // 1. Clean all feature measurements and make sure they all have valid clone times
  auto it0 = feature_vec.begin();
  while (it0 != feature_vec.end()) {

    // Clean the feature
    (*it0)->clean_old_measurements(clonetimes);

    // Count how many measurements
    int ct_meas = 0;
    for (const auto &pair : (*it0)->timestamps) {
      ct_meas += (*it0)->timestamps[pair.first].size();
    }

    //if ((int)(*it0)->featid < state->_options.max_aruco_features) {
    //    std::cout << "(*it0)->featid: " << (*it0)->featid << " ct_meas: " << ct_meas << std::endl;
    //}

    // Remove if we don't have enough
    if (ct_meas < 2) {
        //PRINT_DEBUG("\tDelayed init: feat %d ct_meas < 2\n", (*it0)->featid, ct_meas);
      (*it0)->to_delete = true;
      it0 = feature_vec.erase(it0);
    } else {
        PRINT_DEBUG("\tDelayed init: feat %d ct_meas = %d from %d cams\n", (*it0)->featid, ct_meas, (*it0)->timestamps.size());
      it0++;
    }
  }
  rT1 = boost::posix_time::microsec_clock::local_time();

  // 2. Create vector of cloned *CAMERA* poses at each of our clone timesteps
  std::unordered_map<size_t, std::unordered_map<double, FeatureInitializer::ClonePose>> clones_cam;
  for (const auto &clone_calib : state->_calib_IMUtoCAM) {

    // For this camera, create the vector of camera poses
    std::unordered_map<double, FeatureInitializer::ClonePose> clones_cami;
    for (const auto &clone_imu : state->_clones_IMU) {

      // Get current camera pose
      Eigen::Matrix<double, 3, 3> R_GtoCi = clone_calib.second->Rot() * clone_imu.second->Rot();
      Eigen::Matrix<double, 3, 1> p_CioinG = clone_imu.second->pos() - R_GtoCi.transpose() * clone_calib.second->pos();

      // Append to our map
      clones_cami.insert({clone_imu.first, FeatureInitializer::ClonePose(R_GtoCi, p_CioinG)});
    }

    // Append to our map
    clones_cam.insert({clone_calib.first, clones_cami});
  }
  rT2 = boost::posix_time::microsec_clock::local_time();

  // 3. Try to triangulate all MSCKF or new SLAM features that have measurements
  auto it1 = feature_vec.begin();
  while (it1 != feature_vec.end()) {
      //if (std::abs((*it1)->p_FinG.x()) < 1e-5 && 
      //    std::abs((*it1)->p_FinG.y()) < 1e-5 &&
      //   std::abs((*it1)->p_FinG.z()) < 1e-5) {
      //if(1){
	  if ((*it1)->p_FinG.x() == 0.0) {
          // Triangulate the feature and remove if it fails
          bool success_tri = true;
          if (initializer_feat->config().triangulate_1d) {
              success_tri = initializer_feat->single_triangulation_1d(*it1, clones_cam);
          }
          else {
              success_tri = initializer_feat->single_triangulation(*it1, clones_cam);
          }

          // Gauss-newton refine the feature
          bool success_refine = true;
          if (initializer_feat->config().refine_features) {
              success_refine = initializer_feat->single_gaussnewton(*it1, clones_cam);
          }

          // Remove the feature if not a success
          if (!success_tri || !success_refine) {
              //PRINT_DEBUG("\tDelayed init: feat %d clones %d trig fail!\n", (*it1)->featid, clones_cam.size());
              //StateHelper::save_landmark_state(state->_timestamp, *it1, clones_cam, "DinitTrigFail");
              (*it1)->to_delete = true;
              it1 = feature_vec.erase(it1);
              continue;
          }
      }
      else{
          out << "delayed init with reprojection: "<< (*it1)->featid << " " << (*it1)->p_FinG << std::endl;
          //std::cout << (*it1)->featid  <<" " << (*it1)->p_FinG << std::endl;
      }
      it1++;
  }
  rT3 = boost::posix_time::microsec_clock::local_time();

  // 4. Compute linear system for each feature, nullspace project, and reject
  auto it2 = feature_vec.begin();

  while (it2 != feature_vec.end()) {
    boost::posix_time::ptime rT30, rT31, rT32, rT33, rT34, rT35, rT36;
    rT30 = boost::posix_time::microsec_clock::local_time();
    // Convert our feature into our current format
    UpdaterHelper::UpdaterHelperFeature feat;
    feat.featid = (*it2)->featid;
    feat.uvs = (*it2)->uvs;
    feat.uvs_norm = (*it2)->uvs_norm;
    feat.timestamps = (*it2)->timestamps;

    // If we are using single inverse depth, then it is equivalent to using the msckf inverse depth
    auto feat_rep =
        ((int)feat.featid < state->_options.max_aruco_features) ? state->_options.feat_rep_aruco : state->_options.feat_rep_slam;
    feat.feat_representation = feat_rep;
    if (feat_rep == LandmarkRepresentation::Representation::ANCHORED_INVERSE_DEPTH_SINGLE) {
      feat.feat_representation = LandmarkRepresentation::Representation::ANCHORED_MSCKF_INVERSE_DEPTH;
    }

    // Save the position and its fej value
    if (LandmarkRepresentation::is_relative_representation(feat.feat_representation)) {
      feat.anchor_cam_id = (*it2)->anchor_cam_id;
      feat.anchor_clone_timestamp = (*it2)->anchor_clone_timestamp;
      feat.p_FinA = (*it2)->p_FinA;
      feat.p_FinA_fej = (*it2)->p_FinA;
    } else {
      feat.p_FinG = (*it2)->p_FinG;
      feat.p_FinG_fej = (*it2)->p_FinG;
    }

    rT31 = boost::posix_time::microsec_clock::local_time();

    // Our return values (feature jacobian, state jacobian, residual, and order of state jacobian)
    Eigen::MatrixXd H_f;
    Eigen::MatrixXd H_x;
    Eigen::VectorXd res;
    std::vector<std::shared_ptr<Type>> Hx_order;

    // Get the Jacobian for this feature
    UpdaterHelper::get_feature_jacobian_full(state, feat, H_f, H_x, res, Hx_order);

    rT32 = boost::posix_time::microsec_clock::local_time();

    // If we are doing the single feature representation, then we need to remove the bearing portion
    // To do so, we project the bearing portion onto the state and depth Jacobians and the residual.
    // This allows us to directly initialize the feature as a depth-old feature
    if (feat_rep == LandmarkRepresentation::Representation::ANCHORED_INVERSE_DEPTH_SINGLE) {

      // Append the Jacobian in respect to the depth of the feature
      Eigen::MatrixXd H_xf = H_x;
      H_xf.conservativeResize(H_x.rows(), H_x.cols() + 1);
      H_xf.block(0, H_x.cols(), H_x.rows(), 1) = H_f.block(0, H_f.cols() - 1, H_f.rows(), 1);
      H_f.conservativeResize(H_f.rows(), H_f.cols() - 1);

      // Nullspace project the bearing portion
      // This takes into account that we have marginalized the bearing already
      // Thus this is crucial to ensuring estimator consistency as we are not taking the bearing to be true
      UpdaterHelper::nullspace_project_inplace(H_f, H_xf, res);

      // Split out the state portion and feature portion
      H_x = H_xf.block(0, 0, H_xf.rows(), H_xf.cols() - 1);
      H_f = H_xf.block(0, H_xf.cols() - 1, H_xf.rows(), 1);
    }

    rT33 = boost::posix_time::microsec_clock::local_time();

    // Create feature pointer (we will always create it of size three since we initialize the single invese depth as a msckf anchored
    // representation)
    int landmark_size = (feat_rep == LandmarkRepresentation::Representation::ANCHORED_INVERSE_DEPTH_SINGLE) ? 1 : 3;
    auto landmark = std::make_shared<Landmark>(landmark_size);
    landmark->_featid = feat.featid;
    landmark->_feat_representation = feat_rep;
    landmark->_unique_camera_id = (*it2)->anchor_cam_id;
    landmark->fast = (*it2)->FastResult;

    if (LandmarkRepresentation::is_relative_representation(feat.feat_representation)) {
      landmark->_anchor_cam_id = feat.anchor_cam_id;
      landmark->_anchor_clone_timestamp = feat.anchor_clone_timestamp;
      landmark->set_from_xyz(feat.p_FinA, false);
      landmark->set_from_xyz(feat.p_FinA_fej, true);
    } else {
      landmark->set_from_xyz(feat.p_FinG, false);
      landmark->set_from_xyz(feat.p_FinG_fej, true);
    }

    // Measurement noise matrix
    double sigma_pix_sq =
        ((int)feat.featid < state->_options.max_aruco_features) ? _options_aruco.sigma_pix_sq : _options_slam.sigma_pix_sq;
    Eigen::MatrixXd R = sigma_pix_sq * Eigen::MatrixXd::Identity(res.rows(), res.rows());

    // Try to initialize, delete new pointer if we failed

    rT34 = boost::posix_time::microsec_clock::local_time();

    double chi2_multipler =
        ((int)feat.featid < state->_options.max_aruco_features) ? _options_aruco.chi2_multipler : _options_slam.chi2_multipler;
    
    chi2_multipler = chi2_multipler * 0.5;

    bool init_success = StateHelper::initialize(state, landmark, Hx_order, H_x, H_f, R, res, chi2_multipler);

    rT35 = boost::posix_time::microsec_clock::local_time();

    if (init_success) {
      state->_features_SLAM.insert({(*it2)->featid, landmark});
      (*it2)->to_delete = true;
      it2++;
    }
    else {
        (*it2)->to_delete = true;
        it2 = feature_vec.erase(it2);
    }

    rT36 = boost::posix_time::microsec_clock::local_time();

  }
  rT4 = boost::posix_time::microsec_clock::local_time();

  // Debug print timing information
  if (!feature_vec.empty()) {
    PRINT_ALL("[SLAM-DELAY]: %.4f seconds to clean\n", (rT1 - rT0).total_microseconds() * 1e-6);
    PRINT_ALL("[SLAM-DELAY]: %.4f seconds to triangulate\n", (rT2 - rT1).total_microseconds() * 1e-6);
    PRINT_ALL("[SLAM-DELAY]: %.4f seconds initialize (%d features)\n", (rT3 - rT2).total_microseconds() * 1e-6, (int)feature_vec.size());
    PRINT_ALL("[SLAM-DELAY]: %.4f seconds total\n", (rT3 - rT1).total_microseconds() * 1e-6);
  }
}