OpenVINS代码解析-VioManager

书接上回，上一篇博客主要分析了ROS2Visualizer中接口的逻辑，接下来要进入正题，对VioManager进行分析

---

主要成员变量

• 系统状态变量

/// Our master state object :D
std::shared_ptr<State> state;

• 状态传播

/// Propagator of our state
std::shared_ptr<Propagator> propagator;

• 特征追踪，也可以追踪ARUCO

/// Our sparse feature tracker (klt or descriptor)
std::shared_ptr<ov_core::TrackBase> trackFEATS;
/// Our aruoc tracker
std::shared_ptr<ov_core::TrackBase> trackARUCO;

• 系统初始化

/// State initializer
std::shared_ptr<ov_init::InertialInitializer> initializer;

• 系统状态更新，分别为MSCKF更新，SLAM更新，ZUPT更新

/// Our MSCKF feature updater
std::shared_ptr<UpdaterMSCKF> updaterMSCKF;
/// Our SLAM/ARUCO feature updater
std::shared_ptr<UpdaterSLAM> updaterSLAM;
/// Our zero velocity tracker
std::shared_ptr<UpdaterZeroVelocity> updaterZUPT;

接下来，我们主要看在ROS2Visualizer中调用的feed_measurement_imu()和feed_measurement_camera()分别做了什么？

feed_measurement_imu()

判断是否完成初始化，并更新oldest_time

  double oldest_time = state->margtimestep();
  if (oldest_time > state->_timestamp) {
    oldest_time = -1;
  }
  if (!is_initialized_vio) {
    oldest_time = message.timestamp - params.init_options.init_window_time +
                  state->_calib_dt_CAMtoIMU->value()(0) - 0.10;
  }

将数据喂给propagator，用于后续的状态传播

  propagator->feed_imu(message, oldest_time);

如果系统未进行初始化，则将数据喂给initializer

  // Push back to our initializer
  if (!is_initialized_vio) {
    initializer->feed_imu(message, oldest_time);
  }

同时ZUPT更新也需要根据IMU数据判断车辆是否静止

  // Push back to the zero velocity updater if it is enabled
  // No need to push back if we are just doing the zv-update at the begining and
  // we have moved
  if (is_initialized_vio && updaterZUPT != nullptr &&
      (!params.zupt_only_at_beginning || !has_moved_since_zupt)) {
    updaterZUPT->feed_imu(message, oldest_time);
  }

feed_measurement_camera()

该函数实际调用的是 track_image_and_update()函数，接下来我们对该函数进行分析
首先针对输入的图像构建金字塔

  // Downsample if we are downsampling
  ov_core::CameraData message = message_const;
  for (size_t i = 0; i < message.sensor_ids.size() && params.downsample_cameras;
       i++) {
    cv::Mat img = message.images.at(i);
    cv::Mat mask = message.masks.at(i);
    cv::Mat img_temp, mask_temp;
    cv::pyrDown(img, img_temp, cv::Size(img.cols / 2.0, img.rows / 2.0));
    message.images.at(i) = img_temp;
    cv::pyrDown(mask, mask_temp, cv::Size(mask.cols / 2.0, mask.rows / 2.0));
    message.masks.at(i) = mask_temp;
  }

接下来对处理过后的图像进行特征追踪

trackFEATS->feed_new_camera(message);

其次，尝试进行ZUPT更新，如果更新成功，则清理老的IMU数据

  if (is_initialized_vio && updaterZUPT != nullptr &&
      (!params.zupt_only_at_beginning || !has_moved_since_zupt)) {
    // If the same state time, use the previous timestep decision
    if (state->_timestamp != message.timestamp) {
      did_zupt_update = updaterZUPT->try_update(state, message.timestamp);
    }
    if (did_zupt_update) {
      assert(state->_timestamp == message.timestamp);
      propagator->clean_old_imu_measurements(
          message.timestamp + state->_calib_dt_CAMtoIMU->value()(0) - 0.10);
      updaterZUPT->clean_old_imu_measurements(
          message.timestamp + state->_calib_dt_CAMtoIMU->value()(0) - 0.10);
      return;
    }
  }

如果系统未进行初始化，则尝试进行初始化

  if (!is_initialized_vio) {
    is_initialized_vio = try_to_initialize(message);
    if (!is_initialized_vio) {
      double time_track = (rT2 - rT1).total_microseconds() * 1e-6;
      PRINT_DEBUG(BLUE "[TIME]: %.4f seconds for tracking\n" RESET, time_track);
      return;
    }
  }

最后针对输入的图像进行传播与更新

  // Call on our propagate and update function
  do_feature_propagate_update(message);

上面主要用了两个函数： try_to_initialize() 和 do_feature_propagate_update()，初始化部分我们后面再做详细介绍，这里主要针对do_feature_propagate_update() 进行详细介绍。

do_feature_propagate_update()

在新的一帧图像到来时，首先将系统的状态通过IMU的数据递推到当前的相机时刻。

  if (state->_timestamp != message.timestamp) {
    propagator->propagate_and_clone(state, message.timestamp);
  }

如果没有足够的IMU数据也无法递推，同时如果IMU的数据不足，也无法递推到当前时刻

  if ((int)state->_clones_IMU.size() <
      std::min(state->_options.max_clone_size, 5)) {
    PRINT_DEBUG("waiting for enough clone states (%d of %d)....\n",
                (int)state->_clones_IMU.size(),
                std::min(state->_options.max_clone_size, 5));
    return;
  }
  // Return if we where unable to propagate
  if (state->_timestamp != message.timestamp) {
    PRINT_WARNING(RED
                  "[PROP]: Propagator unable to propagate the state forward in "
                  "time!\n" RESET);
    PRINT_WARNING(
        RED "[PROP]: It has been %.3f since last time we propagated\n" RESET,
        message.timestamp - state->_timestamp);
    return;
  }

接下来需要从trackFEATS获取特征对系统状态进行观测更新，特征更新的策略如下

  std::vector<std::shared_ptr<Feature>> feats_lost, feats_marg, feats_slam;

feats_lost： 获得在state->_timestamp时间之前已经在追踪的所有特征点。

  feats_lost =
      trackFEATS->get_feature_database()->features_not_containing_newer(
          state->_timestamp, false, true);

***feats_marg:***首先初始化为被跟踪上的特征点。后续用作EKF更新，但并未加入状态变量。
feats_slam： 初始化为Aruco的特征点，后续加入状态变量进行EKF更新。

  if ((int)state->_clones_IMU.size() > state->_options.max_clone_size ||
      (int)state->_clones_IMU.size() > 5) {
    feats_marg = trackFEATS->get_feature_database()->features_containing(
        state->margtimestep(), false, true);
    if (trackARUCO != nullptr &&
        message.timestamp - startup_time >= params.dt_slam_delay) {
      feats_slam = trackARUCO->get_feature_database()->features_containing(
          state->margtimestep(), false, true);
    }
  }

其次，针对feats_marg中的特征点根据追踪到的次数，划分为长期特征feats_maxtracks和短期特征：

  // Find tracks that have reached max length, these can be made into SLAM
  // features
  std::vector<std::shared_ptr<Feature>> feats_maxtracks;
  auto it2 = feats_marg.begin();
  while (it2 != feats_marg.end()) {
    // See if any of our camera's reached max track
    bool reached_max = false;
    for (const auto &cams : (*it2)->timestamps) {
      if ((int)cams.second.size() > state->_options.max_clone_size) {
        reached_max = true;
        break;
      }
    }
    // If max track, then add it to our possible slam feature list
    if (reached_max) {
      feats_maxtracks.push_back(*it2);
      it2 = feats_marg.erase(it2);
    } else {
      it2++;
    }
  }

接下来，将跟踪到的长期特征点feats_maxtracks添加到feats_slam中，因为feats_slam会被加入到状态变量中,所以必须严格控制防止状态爆炸。

  if (state->_options.max_slam_features > 0 &&
      message.timestamp - startup_time >= params.dt_slam_delay &&
      (int)state->_features_SLAM.size() <
          state->_options.max_slam_features + curr_aruco_tags) {
    // Get the total amount to add, then the max amount that we can add given
    // our marginalize feature array
    int amount_to_add = (state->_options.max_slam_features + curr_aruco_tags) -
                        (int)state->_features_SLAM.size();
    int valid_amount = (amount_to_add > (int)feats_maxtracks.size())
                           ? (int)feats_maxtracks.size()
                           : amount_to_add;
    // If we have at least 1 that we can add, lets add it!
    // Note: we remove them from the feat_marg array since we don't want to
    // reuse information...
    if (valid_amount > 0) {
      feats_slam.insert(feats_slam.end(), feats_maxtracks.end() - valid_amount,
                        feats_maxtracks.end());
      feats_maxtracks.erase(feats_maxtracks.end() - valid_amount,
                            feats_maxtracks.end());
    }
  }

遍历所有当前状态对应的SLAM特征，如果SLAM特征被当前的trackFEATS追踪到，则加入到feats_slam中，否则将landmark设置为需要边缘化。

  for (std::pair<const size_t, std::shared_ptr<Landmark>> &landmark :
       state->_features_SLAM) {
    if (trackARUCO != nullptr) {
      std::shared_ptr<Feature> feat1 =
          trackARUCO->get_feature_database()->get_feature(
              landmark.second->_featid);
      if (feat1 != nullptr) feats_slam.push_back(feat1);
    }
    std::shared_ptr<Feature> feat2 =
        trackFEATS->get_feature_database()->get_feature(
            landmark.second->_featid);
    if (feat2 != nullptr) feats_slam.push_back(feat2);
    assert(landmark.second->_unique_camera_id != -1);
    bool current_unique_cam =
        std::find(message.sensor_ids.begin(), message.sensor_ids.end(),
                  landmark.second->_unique_camera_id) !=
        message.sensor_ids.end();
    if (feat2 == nullptr && current_unique_cam)
      landmark.second->should_marg = true;
    if (landmark.second->update_fail_count > 1)
      landmark.second->should_marg = true;
  }

接下来进行state->_features_SLAM边缘化处理

  StateHelper::marginalize_slam(state);

对当前的feats_slam进行区分为feats_slam_DELAYED和feats_slam_UPDATE

  std::vector<std::shared_ptr<Feature>> feats_slam_DELAYED, feats_slam_UPDATE;
  for (size_t i = 0; i < feats_slam.size(); i++) {
    if (state->_features_SLAM.find(feats_slam.at(i)->featid) !=
        state->_features_SLAM.end()) {
      feats_slam_UPDATE.push_back(feats_slam.at(i));
      // PRINT_DEBUG("[UPDATE-SLAM]: found old feature %d (%d
      // measurements)\n",(int)feats_slam.at(i)->featid,(int)feats_slam.at(i)->timestamps_left.size());
    } else {
      feats_slam_DELAYED.push_back(feats_slam.at(i));
      // PRINT_DEBUG("[UPDATE-SLAM]: new feature ready %d (%d
      // measurements)\n",(int)feats_slam.at(i)->featid,(int)feats_slam.at(i)->timestamps_left.size());
    }
  }

接下来搜集进行MSCKF更新的featsup_MSCKF，首先将feats_lost初始化为featsup_MSCKF，其次将feats_marg（短期特征）和feats_maxtracks（未SLAM更新的长期特征）加入：

  std::vector<std::shared_ptr<Feature>> featsup_MSCKF = feats_lost;
  featsup_MSCKF.insert(featsup_MSCKF.end(), feats_marg.begin(),
                       feats_marg.end());
  featsup_MSCKF.insert(featsup_MSCKF.end(), feats_maxtracks.begin(),
                       feats_maxtracks.end());

通过上述操作我们获得了featsup_MSCKF和feats_slam可以分别作为MSCKF更新和SLAM更新。

在进行MSCKF更新之前，需要对featsup_MSCKF进行筛选，目前是按照被追踪到的次数进行排序，获取在设置范围内的特征点个数进行EKF更新：

  auto compare_feat = [](const std::shared_ptr<Feature> &a,
                         const std::shared_ptr<Feature> &b) -> bool {
    size_t asize = 0;
    size_t bsize = 0;
    for (const auto &pair : a->timestamps) asize += pair.second.size();
    for (const auto &pair : b->timestamps) bsize += pair.second.size();
    return asize < bsize;
  };
  std::sort(featsup_MSCKF.begin(), featsup_MSCKF.end(), compare_feat);

  // Pass them to our MSCKF updater
  // NOTE: if we have more then the max, we select the "best" ones (i.e. max
  // tracks) for this update NOTE: this should only really be used if you want
  // to track a lot of features, or have limited computational resources
  if ((int)featsup_MSCKF.size() > state->_options.max_msckf_in_update)
    featsup_MSCKF.erase(
        featsup_MSCKF.begin(),
        featsup_MSCKF.end() - state->_options.max_msckf_in_update);
  updaterMSCKF->update(state, featsup_MSCKF);

在进行SLAM EKF更新时首先对已经在状态向量中的部分特征进行EKF更新，其次再对不在状态向量里的特征进行EKF更新

 std::vector<std::shared_ptr<Feature>> feats_slam_UPDATE_TEMP;
  while (!feats_slam_UPDATE.empty()) {
    // Get sub vector of the features we will update with
    std::vector<std::shared_ptr<Feature>> featsup_TEMP;
    featsup_TEMP.insert(
        featsup_TEMP.begin(), feats_slam_UPDATE.begin(),
        feats_slam_UPDATE.begin() + std::min(state->_options.max_slam_in_update,
                                             (int)feats_slam_UPDATE.size()));
    feats_slam_UPDATE.erase(
        feats_slam_UPDATE.begin(),
        feats_slam_UPDATE.begin() + std::min(state->_options.max_slam_in_update,
                                             (int)feats_slam_UPDATE.size()));
    // Do the update
    updaterSLAM->update(state, featsup_TEMP);
    feats_slam_UPDATE_TEMP.insert(feats_slam_UPDATE_TEMP.end(),
                                  featsup_TEMP.begin(), featsup_TEMP.end());
  }
  feats_slam_UPDATE = feats_slam_UPDATE_TEMP;
  rT5 = boost::posix_time::microsec_clock::local_time();
  updaterSLAM->delayed_init(state, feats_slam_DELAYED);
  rT6 = boost::posix_time::microsec_clock::local_time();

---