gici-open学习日记(3)：Estimating线程初始化

线程调用

在NodeHandle的构造函数中，estimators(状态估计)线程的启动是通过调用start()函数实现的

estimatings_[i]->start();

这里的estimatings_是刚初始化完成的变量，这里的estimatings_成员的类型MultiSensorEstimating类是EstimatingBase的一个子类

auto estimating = std::make_shared<MultiSensorEstimating>(nodes, i);    // estimators
      estimatings_.push_back(estimating);

start()函数里面就是新建了一个线程

// Start thread
void EstimatingBase::start()
{
  // Create thread
  quit_thread_ = false;
  thread_.reset(new std::thread(&EstimatingBase::run, this));
}

在EstimatingBase::run()里

// Loop processing
void EstimatingBase::run()
{
  // Spin until quit command or global shutdown called 
  SpinControl spin(1.0e-4);
  while (!quit_thread_ && SpinControl::ok()) {
    // Process funtion in every loop
    process();

    // Publish solution
    if (updateSolution()) {
      std::shared_ptr<DataCluster> out_data = std::make_shared<DataCluster>(solution_);
      for (auto& out_callback : output_data_callbacks_) {
        out_callback(tag_, out_data);
      }
    }

    spin.sleep();
  } 
}

这里的process()和updateSolution()分别实现了数据处理的主要部分和结果更新的主要部分，这里就先简单看一下状态估计的基类EstimatingBase

EstimatingBase类

构造函数

EstimatingBase作为父类，其构造函数也会在MultiSensorEstimating类被调用时调用，其主要功能也是获取到estimator节点一些基本的配置值，没有特别的内容。

checkDownsampling(const std::string& tag)

这个函数是在控制结果输出时被调用，与配置文件的output downsample rate值是对应的。比如我们想让输出结果的频率比我们解算出来的频率低(例如两秒一个，就可以在配置文件设置为2)。然后我们利用成员变量output_downsample_cnt_进行判断，来控制结果的输出频率。

  inline bool checkDownsampling(const std::string& tag) {
    if (++output_downsample_cnt_ < output_downsample_rate_) {
      return false;         // 小于降采样设置的值的话就不输出
    }
    else {
      output_downsample_cnt_ = 0; // 输出了的话就重新置为0，以便下一个循环再次判断
      return true;
    }
    return true;
  }

MultiSensorEstimating类

在NodeHandle的构造函数中，直接声明的对象是MultiSensorEstimating类，所以具体的数据处理操作其实都在这个子类中。它有很多成员变量，按我的理解可以大概分为：线程变量、状态估计变量和选项变量
线程变量就是控制其中更多的子线程用的：

  // Measurement data collection thread handles(数据)
  std::unique_ptr<std::thread> measurement_thread_;

  // Backend thread handles(后端优化)
  std::unique_ptr<std::thread> backend_thread_;

  // Front thread handles(视觉前端)
  std::unique_ptr<std::thread> image_frontend_thread_;

状态估计变量就是之后可能会用到的一些变量，像量测值等等

  // Coordinate
  bool force_initial_global_position_;
  Eigen::Vector3d initial_global_position_;
  std::unique_ptr<SppEstimator> spp_estimator_;

  // Data buffers
  std::deque<EstimatorDataCluster> measurements_;  // non propagate measurements
  int last_backend_pending_num_ = 0;
  // the frontend measurements should be processd by frontend, and the output of frontend will 
  // be inserted into measurements_.
  std::deque<EstimatorDataCluster> image_frontend_measurements_; 
  int last_image_pending_num_ = 0;
  // buffer to temporarily store measurements in case the input stream blocking
  std::deque<EstimatorDataCluster> measurement_addin_buffer_;
  // buffer to align timestamps of different sensor streams
  std::deque<EstimatorDataCluster> measurement_align_buffer_;
  double latest_imu_timestamp_;
  // mutex to lock buffers and processes(用到的一些锁)
  std::mutex mutex_addin_, mutex_input_, mutex_image_input_;
  std::mutex mutex_output_;

至于选项变量就是根据配置文件所设置的解算方法对应的解算设置了。
以及最后一个也是控制输出的变量

bool backend_firstly_updated_ = false;  // 是否是初始更新状态

构造函数

这个构造函数主要也是根据对配置文件的加载初始化了一些解算可能涉及到的相关变量。
并且，这里也根据配置文件的内容，实例化了具体的状态估计器出来，里面有很多种状态，不想看代码的话manual也都列出来了

里面lc就是松组合，tc就是紧组合，然后有一个srr是GNSS/IMU/Camera的半紧组合，还有rrr是三者的紧组合。这里直接去关注我目前最想学习的(应该也是最复杂的)RTK/IMU/Camera tightly integration组合类型。(TODO)之后有时间争取把其他的模式也过一遍
先把对应的解算配置加载进来

    YAML::Node rtk_imu_camera_rrr_node = node["rtk_imu_camera_rrr_options"];
    if (rtk_imu_camera_rrr_node.IsDefined()) {
      option_tools::loadOptions(rtk_imu_camera_rrr_node, rtk_imu_camera_rrr_options_);
    }
    YAML::Node rtk_node = node["rtk_options"];
    if (rtk_node.IsDefined()) {
      option_tools::loadOptions(rtk_node, rtk_options_);
    }
    YAML::Node gnss_imu_init_node = node["gnss_imu_initializer_options"];
    if (gnss_imu_init_node.IsDefined()) {
      option_tools::loadOptions(gnss_imu_init_node, gnss_imu_init_options_);
    }
    YAML::Node ambiguity_node = node["ambiguity_resolution_options"];
    if (ambiguity_node.IsDefined()) {
      option_tools::loadOptions(ambiguity_node, ambiguity_options_);
    }

接下来就是获取传感器之间的外参

• 根据manual来理解，body系是右-前-上(RFU)系，所以这里设置的都是GNSS和相机到body到GNSS和相机的转换，而body到IMU到的外参，就是设置在ImuEstimatorBaseOptions结构体中的Eigen::Vector3d body_to_imu_rotation，然后再根据GNSS与IMU、相机与IMU之间的位姿转换关系，获得body到GNSS和相机的转换

    // rotate estrinsics(旋转外参)
    gnss_imu_init_options_.gnss_extrinsics = ImuEstimatorBase::rotateImuToBody( // body-->GNSS(RFU)
      gnss_imu_init_options_.gnss_extrinsics, imu_base_options_);
    gnss_imu_init_options_.gnss_extrinsics_initial_std = ImuEstimatorBase::rotateImuToBody(
      gnss_imu_init_options_.gnss_extrinsics_initial_std, imu_base_options_);
    CameraBundlePtr camera_bundle = feature_handler_options_.cameras;
    for (size_t i = 0; i < camera_bundle->numCameras(); i++) {
      camera_bundle->set_T_C_B(i, ImuEstimatorBase::rotateImuToBody(  // body-->Camera,配置文件里应该是Camera相对IMU的
        camera_bundle->get_T_C_B(i).inverse(), imu_base_options_).inverse());
    }

接下来就是主要的状态估计变量了，先是视觉用到的特征检测与跟踪

feature_handler_.reset(new FeatureHandler(feature_handler_options_, imu_base_options_));

然后是RTK/INS/Camera的紧组合

    estimator_.reset(new RtkImuCameraRrrEstimator(rtk_imu_camera_rrr_options_, 
      gnss_imu_init_options_, rtk_options_, gnss_base_options_, gnss_loose_base_options_, 
      visual_estimator_base_options_, imu_base_options_, base_options_, ambiguity_options_));

然后是设置视觉估计的相关配置，然后和刚才定义的特征检测和跟踪对应起来

    std::shared_ptr<VisualEstimatorBase> visual_estimator = 
      std::dynamic_pointer_cast<VisualEstimatorBase>(estimator_);
    CHECK_NOTNULL(visual_estimator);
    visual_estimator->setFeatureHandler(feature_handler_);

然后解算类型设置完成后，在构造函数里又初始化了一个SPP估计器

  // For coordinate initialization
  if (estimatorTypeContains(SensorType::GNSS, type_)) {         // 因为GNSS解算所有模式都要用SPP
    spp_estimator_.reset(new SppEstimator(gnss_base_options_)); // 所以这里也定义了一个SPP需要用的估计器
  }

最后又把时间戳置零，估计器就初始化完成了

  // Initial values
  solution_.timestamp = 0.0;

数据处理process()

process()函数里主要是分别针对视觉前端、量测添加(这个线程我也不知道怎么描述比较准确)以及后端优化创建了新的线程

  // Process frontends in a separated thread
  if (image_frontend_thread_ == nullptr && 
      estimatorTypeContains(SensorType::Camera, type_)) {
    image_frontend_thread_.reset(new std::thread(
      &MultiSensorEstimating::runImageFrontend, this));
  }

  // Put measurements from addin buffer to measurement buffer
  if (measurement_thread_ == nullptr) {
    measurement_thread_.reset(new std::thread(
      &MultiSensorEstimating::runMeasurementAddin, this));
  }

  // Process backend in a separated thread
  if (backend_thread_ == nullptr) {
    backend_thread_.reset(new std::thread(
      &MultiSensorEstimating::runBackend, this)); 
  }

之后如果外部有设置终止线程，就把线程都终止了

更新结果updateSolution()

首先确保已经开启了解算以及有需要更新的解算结果

  // Backend not working yet
  if (!backend_firstly_updated_) return false;

  mutex_output_.lock();

  // No need to update 
  if (output_timestamps_.size() == 0) {
    mutex_output_.unlock(); return false;
  }

然后如果解算结果比窗口时间还早的话，就把这一个结果删除

  // Erase timestamps in front of estimator window
  while (output_timestamps_.front() < estimator_->getOldestTimestamp()) {
    LOG(WARNING) << "Erasing output timestamp " << std::fixed 
      << output_timestamps_.front() << " because it is too old!";
    output_timestamps_.pop_front();
  }

接下来根据还剩下的待处理的时间差判断，要是大于1了就提示待处理的太多了(TODO)

  // Check pending
  if (output_timestamps_.back() - output_timestamps_.front() > 1.0) {
    LOG(WARNING) << "Large pending in output control!";
  }

然后拿出output_timestamps_最前面的一个结果，得到这个时间的解算结果，要确保位姿、速度偏差和协方差都能得到

  if (!estimator_->getPoseEstimateAt(timestamp, solution_.pose) || 
      !estimator_->getSpeedAndBiasEstimateAt(timestamp, solution_.speed_and_bias) || 
      (compute_covariance_ && 
      !estimator_->getCovarianceAt(timestamp, solution_.covariance))) {
    return false;
  }

之后得到GNSS的解算结果

• TODO：这里对GNSS解算的判断，是根据是不是紧组合进行分类的么？

  if (estimatorTypeContains(SensorType::GNSS, type_)) {
    if (std::shared_ptr<GnssEstimatorBase> gnss_estimator = 
      std::dynamic_pointer_cast<GnssEstimatorBase>(estimator_)) {         // TODO 紧组合？
      solution_.status = gnss_estimator->getSolutionStatus();             // 解的状态
      solution_.num_satellites = gnss_estimator->getNumberSatellite();    // 卫星数
      solution_.differential_age = gnss_estimator->getDifferentialAge();  // 差分龄期
    }
    else if (std::shared_ptr<GnssLooseEstimatorBase> gnss_estimator = 
      std::dynamic_pointer_cast<GnssLooseEstimatorBase>(estimator_)) {    // TODO 松组合？
      solution_.status = gnss_estimator->getSolutionStatus();
      solution_.num_satellites = gnss_estimator->getNumberSatellite();
      solution_.differential_age = gnss_estimator->getDifferentialAge();
    }
    else {
      LOG(FATAL) << "Unable to cast estimaotr to GNSS estimator!";
    }
  }