【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 featuresif (feature_vec.empty())return;//PRINT_DEBUG("\tDelayed init: feature_vec size: %d\n", feature_vec.size());// Start timingboost::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 timesauto it0 = feature_vec.begin();while (it0 != feature_vec.end()) {// Clean the feature(*it0)->clean_old_measurements(clonetimes);// Count how many measurementsint 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 enoughif (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 timestepsstd::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 posesstd::unordered_map<double, FeatureInitializer::ClonePose> clones_cami;for (const auto &clone_imu : state->_clones_IMU) {// Get current camera poseEigen::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 mapclones_cami.insert({clone_imu.first, FeatureInitializer::ClonePose(R_GtoCi, p_CioinG)});}// Append to our mapclones_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 measurementsauto 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 failsbool 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 featurebool success_refine = true;if (initializer_feat->config().refine_features) {success_refine = initializer_feat->single_gaussnewton(*it1, clones_cam);}// Remove the feature if not a successif (!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 rejectauto 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 formatUpdaterHelper::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 depthauto 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 valueif (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 featureUpdaterHelper::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 featureif (feat_rep == LandmarkRepresentation::Representation::ANCHORED_INVERSE_DEPTH_SINGLE) {// Append the Jacobian in respect to the depth of the featureEigen::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 trueUpdaterHelper::nullspace_project_inplace(H_f, H_xf, res);// Split out the state portion and feature portionH_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 matrixdouble 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 failedrT34 = 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 informationif (!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);}
}