【OV】MSCKF与SLAMupdate更新解读

0. 零空间投影的作用和目的

零空间投影（Nullspace Projection）在优化和滤波算法中，特别是在多状态约束优化问题中，代码中的零空间投影，其主要目的如下：

1. 消除冗余信息：通过零空间投影，可以去除雅可比矩阵中与某些约束无关的部分，确保优化过程中只保留有效的信息。
2. 保证数值稳定性：在处理大规模、高维的优化问题时，直接使用原始雅克比矩阵可能导致数值不稳定或病态问题。零空间投影可以帮助改善矩阵条件数，提高数值稳定性。
3. 满足约束条件：在某些情况下，系统可能包含特定的约束条件（例如刚体运动中的旋转和平移约束）。零空间投影可以确保这些约束在优化过程中得到满足，避免违反约束的情况。
   零空间投影的具体作用可能是为了确保在更新过程中，状态估计不会偏离已知的约束条件，从而提高滤波器的准确性和鲁棒性。

小姐：零空间投影的主要目的是：消除冗余信息，保证数值稳定性，满足约束条件，提高整个系统的性能和可靠性，特别是在多状态扩展卡尔曼滤波（MSCKF）等复杂优化场景中。

1. 对比UpdaterSLAM::update、UpdaterSLAM::delayed_init和UpdaterMSCKF::update三个接口之间的差异、联系和各自的功能。

1.1 三个接口的主要作用

• UpdaterSLAM::update:
  • 主要负责更新SLAM状态，处理已经存在的特征点，并通过测量来更新状态估计。
  • 包括特征点的清理、雅可比矩阵和残差的计算，以及状态的更新。
• UpdaterSLAM::delayed_init:
  • 用于延迟初始化新特征点，处理那些尚未被初始化的特征点。
  • 包括清理、三角化特征、计算雅可比矩阵和残差，并最终初始化特征。
• UpdaterMSCKF::update:
  • 专注于MSCKF（Multi-State Constraint Kalman Filter）方法的状态更新，处理IMU和视觉测量的融合。
  • 主要通过对多个状态进行约束来更新状态估计。

1.2. 接口核心算法实现流程

1.2.1 特征点处理

• update:

  • 清理特征点的测量，确保每个特征点有足够的测量。
  • 计算雅可比矩阵和残差来更新状态。

• delayed_init:

  • 清理特征点，确保每个特征点至少有两个测量，适合于三角化。
  • 尝试三角化特征点，并在成功后进行高斯-牛顿优化。

• updaterMSCKF::update:

  • 处理IMU和视觉测量，通过约束融合多个状态。
  • 计算雅可比矩阵并更新状态，通常涉及到多次状态预测和更新步骤。

1.2.2 状态更新

• update:

  • 计算线性系统并更新状态，使用扩展卡尔曼滤波（EKF）方法。
  • 处理所有有效特征点的雅可比矩阵和残差，确保状态一致性。

• delayed_init:

  • 初始化新的特征点，并将其添加到状态中。
  • 使用卡方检验来决定特征点是否成功初始化。

• updaterMSCKF::update:

  • 通过对IMU和视觉测量的约束来更新状态，通常包含多个状态的更新步骤。
  • 计算状态的协方差并进行更新，确保估计的准确性。

1.3. 变量和数据结构

• update:

  • 使用Eigen库来处理矩阵和向量运算，构建大雅可比矩阵和残差向量。
  • 维护一个大的雅可比矩阵（Hx_big）和残差向量（res_big），以便进行状态更新。

• delayed_init:

  • 创建克隆相机姿态的映射，以便进行特征三角化。
  • 使用FeatureInitializer来处理特征的三角化和初始化。

• updaterMSCKF::update:

  • 处理IMU和视觉数据，通常涉及到多个状态的雅可比矩阵和残差的计算。
  • 使用状态协方差矩阵来评估状态的不确定性。

1.4. 特征表示

• 在update中，特征的表示可能会根据不同的条件进行处理（如单逆深度），而在delayed_init中则更专注于特征的初始化和三角化。
• updaterMSCKF::update则主要关注IMU和视觉数据的融合，处理状态约束。

1.5. 区别

• UpdaterSLAM::update和UpdaterSLAM::delayed_init主要关注特征点的管理，而updaterMSCKF::update则专注于IMU和视觉测量的融合。
• update处理的是已有特征的状态更新，而delayed_init则是为新的特征点提供初始化机制。
• updaterMSCKF则是通过多状态约束来更新状态，处理不同类型的测量数据。

2. msckf update MSCKF相机视觉里程计更新

2.1 算法目的

利用多个相机观测到的特征点,通过优化计算得到当前状态的最优估计。它可以在不使用地图的情况下实现单目视觉里程计的功能。算法实现了特征点管理、相机位姿估计、特征点三角化和优化、测量矩阵构建和状态更新等主要步骤。

2.2 主要流程

1. 清理特征点测量数据,去除测量次数少于2次的特征点。
2. 根据当前状态中的IMU位姿和IMU到相机的外参,计算每个时刻相机的位姿。
3. 对每个特征点进行三角化和高斯牛顿优化,去除三角化或优化失败的特征点。
4. 构建包含所有特征点的大雅可比矩阵和残差向量,执行空间投影和Chi2距离检测去除离群点。
5. 执行测量压缩,减小更新矩阵的尺寸。
6. 使用EKF更新状态估计的位姿和其他参数。

2.3 代码实现

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

  // Return if no features
  if (feature_vec.empty())
    return;
  //PRINT_DEBUG("\tMSCKF update: feature_vec size: %d\n", feature_vec.size());

  // Start timing
  boost::posix_time::ptime rT0, rT1, rT2, rT3, rT4, rT5;
  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();
    }

    // Remove if we don't have enough
    if (ct_meas < 2) {
        //PRINT_DEBUG("\tMSCKF update: feat %d ct_meas < 2\n", (*it0)->featid, ct_meas); 
      (*it0)->to_delete = true;
      it0 = feature_vec.erase(it0);
    } else {
        //PRINT_DEBUG("\tMSCKF update: 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});
  }

  // 3. Try to triangulate all MSCKF or new SLAM features that have measurements
  auto it1 = feature_vec.begin();
  while (it1 != feature_vec.end()) {

    // 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("\tMSCKF upadte: feat %d clones %d trig fail!\n", (*it1)->featid, clones_cam.size());
      (*it1)->to_delete = true;
      it1 = feature_vec.erase(it1);
      continue;
    }
    it1++;
  }
  rT2 = boost::posix_time::microsec_clock::local_time();

  // Calculate the max possible measurement size
  size_t max_meas_size = 0;
  for (size_t i = 0; i < feature_vec.size(); i++) {
    for (const auto &pair : feature_vec.at(i)->timestamps) {
      max_meas_size += 2 * feature_vec.at(i)->timestamps[pair.first].size();
    }
  }

  // Calculate max possible state size (i.e. the size of our covariance)
  // NOTE: that when we have the single inverse depth representations, those are only 1dof in size
  size_t max_hx_size = state->max_covariance_size();
  for (auto &landmark : state->_features_SLAM) {
    max_hx_size -= landmark.second->size();
  }

  // Large Jacobian and residual of *all* features for this update
  Eigen::VectorXd res_big = Eigen::VectorXd::Zero(max_meas_size);
  Eigen::MatrixXd Hx_big = Eigen::MatrixXd::Zero(max_meas_size, max_hx_size);
  std::unordered_map<std::shared_ptr<Type>, size_t> Hx_mapping;
  std::vector<std::shared_ptr<Type>> Hx_order_big;
  size_t ct_jacob = 0;
  size_t ct_meas = 0;

  // 4. Compute linear system for each feature, nullspace project, and reject
  auto it2 = feature_vec.begin();
  while (it2 != feature_vec.end()) {
    
    // 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
    feat.feat_representation = state->_options.feat_rep_msckf;
    if (state->_options.feat_rep_msckf == 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;
    }

    // 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);

    // Nullspace project
    UpdaterHelper::nullspace_project_inplace(H_f, H_x, res);

    /// Chi2 distance check
    Eigen::MatrixXd P_marg = StateHelper::get_marginal_covariance(state, Hx_order);
    Eigen::MatrixXd S = H_x * P_marg * H_x.transpose();
    S.diagonal() += _options.sigma_pix_sq * Eigen::VectorXd::Ones(S.rows());
    double chi2 = res.dot(S.llt().solve(res));

    // Get our threshold (we precompute up to 500 but handle the case that it is more)
    double chi2_check;
    if (res.rows() < 500) {
      chi2_check = chi_squared_table[res.rows()];
    } else {
      boost::math::chi_squared chi_squared_dist(res.rows());
      chi2_check = boost::math::quantile(chi_squared_dist, 0.95);
      PRINT_WARNING(YELLOW "chi2_check over the residual limit - %d\n" RESET, (int)res.rows());
    }

    // Check if we should delete or not
    if (chi2 > _options.chi2_multipler * chi2_check) {
        //PRINT_DEBUG("\tMSCKF update: feat %d fail! chi2 fail: %.2f > %.2f * %.2f\n", (*it2)->featid, chi2, _options.chi2_multipler, chi2_check);
      (*it2)->to_delete = true;
      it2 = feature_vec.erase(it2);
      // PRINT_DEBUG("featid = %d\n", feat.featid);
      // PRINT_DEBUG("chi2 = %f > %f\n", chi2, _options.chi2_multipler*chi2_check);
      // std::stringstream ss;
      // ss << "res = " << std::endl << res.transpose() << std::endl;
      // PRINT_DEBUG(ss.str().c_str());
      continue;
    }
    else {
        //PRINT_DEBUG("\tMSCKF update: feat %d pass.\n", (*it2)->featid);
    }

    // We are good!!! Append to our large H vector
    size_t ct_hx = 0;
    for (const auto &var : Hx_order) {

      // Ensure that this variable is in our Jacobian
      if (Hx_mapping.find(var) == Hx_mapping.end()) {
        Hx_mapping.insert({var, ct_jacob});
        Hx_order_big.push_back(var);
        ct_jacob += var->size();
      }

      // Append to our large Jacobian
      Hx_big.block(ct_meas, Hx_mapping[var], H_x.rows(), var->size()) = H_x.block(0, ct_hx, H_x.rows(), var->size());
      ct_hx += var->size();
    }

    // Append our residual and move forward
    res_big.block(ct_meas, 0, res.rows(), 1) = res;
    ct_meas += res.rows();
    it2++;
  }
  rT3 = boost::posix_time::microsec_clock::local_time();

  // We have appended all features to our Hx_big, res_big
  // Delete it so we do not reuse information
  for (size_t f = 0; f < feature_vec.size(); f++) {
    feature_vec[f]->to_delete = true;
  }

  // Return if we don't have anything and resize our matrices
  if (ct_meas < 1) {
    return;
  }
  assert(ct_meas <= max_meas_size);
  assert(ct_jacob <= max_hx_size);
  res_big.conservativeResize(ct_meas, 1);
  Hx_big.conservativeResize(ct_meas, ct_jacob);

  // 5. Perform measurement compression
  UpdaterHelper::measurement_compress_inplace(Hx_big, res_big);
  if (Hx_big.rows() < 1) {
    return;
  }
  rT4 = boost::posix_time::microsec_clock::local_time();

  // Our noise is isotropic, so make it here after our compression
  Eigen::MatrixXd R_big = _options.sigma_pix_sq * Eigen::MatrixXd::Identity(res_big.rows(), res_big.rows());

  // 6. With all good features update the state
  StateHelper::EKFUpdate(state, Hx_order_big, Hx_big, res_big, R_big);
  rT5 = boost::posix_time::microsec_clock::local_time();
}