gici-open学习日记(4)：添加观测数据runMeasurementAddin

runMeasurementAddin线程

runMeasurementAddin函数是在process函数创建三个子线程时被调用，作用应该就是向状态估计线程添加观测值，具体的功能实现是通过调用另一个函数putMeasurements来实现的

void MultiSensorEstimating::runMeasurementAddin()
{
  SpinControl spin(1.0e-4);
  while (!quit_thread_ && SpinControl::ok()) {
    putMeasurements();
    spin.sleep();
  }
}

添加观测数据putMeasurements()

首先是从measurement_addin_buffer_容器里获得数据

  if (measurement_addin_buffer_.size() == 0) {  // TODO 确定measurement_addin_buffer_被推入数据的时间
    mutex_addin_.unlock(); return;
  }
  EstimatorDataCluster data = measurement_addin_buffer_.front();
  measurement_addin_buffer_.pop_front();

然后是向里面加IMU数据，这里好像是用IMU来进行时间的传递的，因为IMU的采集频率通常都是最高的

  // time-propagation sensors
  if (estimatorDataIsImu(data)) {
    handleTimePropagationSensors(data);
  }

然后加前端的数据，应该就是图像吧

  // sensors that needs frontends
  else if (estimatorDataNeedFrontend(data)) {
    handleFrontendSensors(data);
  }

如果是GNSS基准站的话，就不做时间校准直接加进来(应该是以基准站时间为时间基准)

  // GNSS reference station data (no need to align time)
  else if (data.gnss && data.gnss_role == GnssRole::Reference) {
    mutex_input_.lock();
    measurements_.push_back(data);
    mutex_input_.unlock();
  }

最后把剩下的数据(不作为时间传播的基准)也加进来

  else {
    handleNonTimePropagationSensors(data);
  }

handleTimePropagationSensors()添加数据

调用EstimatorBase类型的成员函数addMeasurement来添加数据

• 注释理解：注释这里有一句“only support IMU”，我认为这是因为IMU的时间频率一般都是最高的，所以要以IMU来判断，只要有IMU数据就会进行添加。addMeasurement函数的内容之后再具体记录，主要就是把IMU、GNSS和视觉分别加进去了，不过里面还涉及到了一些初始化的内容

if (estimator_) estimator_->addMeasurement(data);

然后将当前数据的时间戳直接设为时间

latest_imu_timestamp_ = data.timestamp;

然后把时间戳加入到output_timestamps_之中

  if (backend_firstly_updated_ && output_align_tag_ == data.tag) {
    if (checkDownsampling(data.tag)) {
      mutex_output_.lock();
      output_timestamps_.push_back(data.timestamp);
      mutex_output_.unlock();
    }
  }

handleFrontendSensors()添加数据

上一个函数的判断一句是有没有IMU数据，这里的判断依据就是有没有图像

CHECK(estimatorDataNeedFrontend(data));

// estimatorDataNeedFrontend()函数
  inline bool estimatorDataNeedFrontend(const EstimatorDataCluster& data) {
    return (data.image != nullptr);
  }

然后这里的添加也比上一个简单很多，只是单纯的把数据添加到了前端解算关联的容器中(因为前端具体的解算再另一个函数，而handleTimePropagationSensors()还完成了GINS初始化等工作)

image_frontend_measurements_.push_back(data);

handleNonTimePropagationSensors()添加数据

• 进入的条件：再来梳理一下前面的几种添加情况，第一种是有IMU数据，这个时候就是直接以IMU时间戳作为时间信息加入的数据，第二种是有图像，所以也是直接把数据加到了前端要用到的容器里，用来进行前端的解算，第三种情况就是是GNSS这时候是基准站，解算的时候会把基准站信息作为基准，所以我们也是不需要做时间校准，直接把观测值加了进来
• 最后，才有可能进入到这个函数，也就是这个时候的数据没有IMU，没有图像，还不是基准站，那么也就可以理解这里的函数起名的逻辑了

函数一上来判断的依据是根据配置文件中的enable input align进行的

然后判断是否需要校准，代码里的设置是只有GNSS/IMU/Camera三种传感器组合的时候需要进行校准，不需要的话就把数据直接加进去

if (!needTimeAlign(type_)) {
  measurement_align_buffer_.push_back(data);
}

没有数据或者当前数据很新的时候也会加入容器

else if (measurement_align_buffer_.size() == 0 ||                     // 没有数据
    data.timestamp >= measurement_align_buffer_.back().timestamp) {   // 数据更新
  measurement_align_buffer_.push_back(data);
}

如果比最老的时间还老的话，根据阈值判断，满足阈值要求就加进去

// 前面定义的一个变量，相当于阈值const double buffer_time = 2.0 * input_align_latency_;
else if (data.timestamp <= measurement_align_buffer_.front().timestamp) {
  if (data.timestamp < measurement_align_buffer_.back().timestamp - buffer_time) {   // 比时间差的阈值要大
    LOG(WARNING) << "Throughing data at timestamp " << std::fixed << data.timestamp 
      << " because its latency is too large!";
  }
  else {
    measurement_align_buffer_.push_front(data);     // 满足的话就加进去
  }
}

都不满足的话，就找到合适的位置插进去

for (auto it = measurement_align_buffer_.begin(); 
      it != measurement_align_buffer_.end(); it++) {
  if (data.timestamp >= it->timestamp) continue;
  measurement_align_buffer_.insert(it, data);
  break;
}

如果选择不校准的话，其实就是乱序插进去了

  // Non-align mode
  else {
    measurement_align_buffer_.push_back(data);
  }

接下来就是判断要不要向measurements_里加入数据了
对measurement_align_buffer_里的数据进行遍历，首先如果时间太短，就直接break

    // we delay the data for input_align_latency_ to wait incoming data for realigning.
    if (measurement_align_buffer_.back().timestamp - 
        measurement_align_buffer_.front().timestamp < input_align_latency_) break;

在估计的类型包括IMU但当前的时间戳比最新的时间戳还新的时候，相当于没有一个校准的基准，这个时候就直接跳出了

    // we always add IMU measurement to estimator at a given timestamp before we 
    // add other sensor measurements.
    EstimatorDataCluster& measurement = *it;              // 把这个数据拿出来
    if (estimatorTypeContains(SensorType::IMU, type_) &&  // 如果选择的解算传感器的类型包括IMU
        measurement.timestamp > latest_imu_timestamp_) {  // 当前的时间戳如果比最新的参考还新
      it++; continue;
    }

满足条件后就把数据加进去

measurements_.push_back(measurement);

接下来的判断条件也是根据配置文件中的值，enable_backend_data_sparsify进行的，主要是来根据后端是否还在等待处理，判断是否要进行数据的稀疏化

• 所以下面的代码就是根据pending_num_threshold_进行冗余数据的剔除，然后关键帧的话还是保留

    // check pending, sparcify if needed
    if (enable_backend_data_sparsify_)
    {
      if (measurements_.size() > pending_num_threshold_) {
        pending_sparsify_num_++;    // 等待稀疏的个数+1
      }
      else if (pending_sparsify_num_ > 0) pending_sparsify_num_--;  // 比阈值小但是有等待稀疏的话，就减少一个
      if (pending_sparsify_num_) {  // 这里执行具体的稀疏数据操作
        LOG(WARNING) << "Backend pending! Sparsifying measurements with counter " 
                    << pending_sparsify_num_ << ".";
        for (int i = 0; i < pending_sparsify_num_; i++) {
          // some measurements we cannot erase
          if (measurements_.front().frame_bundle && 
              measurements_.front().frame_bundle->isKeyframe()) break;  // 关键帧留一下
          // erase front measurement
          if (measurements_.size() > 1) measurements_.pop_front();      // 不然就从前面剔除
        }
      }
    }

• 看上去好像删除了很多的样子，但看了下后边的代码结合进到这里的情况，后边在processEstimator函数中也会不断地measurements_.pop_front()，所以应该也不会删除特别多。

补充1：addMeasurement函数

如果没有坐标结果，也没有设置重力向量的话，就直接返回

if (coordinate_ == nullptr || !gravity_setted_) return false;

接下来是判断GINS的初始化器的状态，如果还未初始化，就设置相关的变量，并且进行初始化操作并得到结果

  if (!gnss_imu_initializer_->finished()) {
    if (gnss_imu_initializer_->getCoordinate() == nullptr) {
      gnss_imu_initializer_->setCoordinate(coordinate_);        // GNSS/IMU的初始坐标
      initializer_sub_estimator_->setCoordinate(coordinate_);   // RTK的初始坐标
      gnss_imu_initializer_->setGravity(imu_base_options_.imu_parameters.g);
    }
    if (gnss_imu_initializer_->addMeasurement(measurement)) {
      gnss_imu_initializer_->estimate();  // TODO 初始化
      // set result to estimator
      setInitializationResult(gnss_imu_initializer_);
    }
    return false;	// 这里直接返回false，说明加入的measurement必须是初始化之后的
  }

接下来分别加入IMU、GNSS和图像，加入IMU的时候直接调用addImuMeasurement函数就可以了

  // Add IMU
  if (measurement.imu && measurement.imu_role == ImuRole::Major) {
    addImuMeasurement(*measurement.imu);
  }

接下来加入GNSS，这里对模糊度解算的协方差进行了计算

  if (measurement.gnss) {
    // feed to covariance estimator
    if (!ambiguity_covariance_coordinate_setted_ && coordinate_) {
      ambiguity_covariance_estimator_->setCoordinate(coordinate_);
      ambiguity_covariance_coordinate_setted_ = true;
    }
    if (coordinate_ && ambiguity_covariance_estimator_->addMeasurement(measurement)) {
      ambiguity_covariance_estimator_->estimate();
    }
    // feed to local
    GnssMeasurement rov, ref;
    meausrement_align_.add(measurement);
    if (meausrement_align_.get(rtk_options_.max_age, rov, ref)) {
      return addGnssMeasurementAndState(rov, ref);
    }
  }

最后就是向里面加入图像了

  if (measurement.frame_bundle) {
    if (!visual_initialized_) return visualInitialization(measurement.frame_bundle);
    return addImageMeasurementAndState(measurement.frame_bundle);
  }

TODO：数据的传递

关于这里，我一开始一直没看明白数据具体是怎么传递的，所以我每个变量仔细地查找引用，后来才有了点头绪
关于measurement这个变量的成员变量，其具体的数据，IMU或者GNSS或者图像都是在构造函数的地方就已经决定好了

  EstimatorDataCluster(const GnssMeasurement& data) : 
    gnss(std::make_shared<GnssMeasurement>(data)), 
    gnss_role(data.role), tag(data.tag), timestamp(data.timestamp) {}

  EstimatorDataCluster(
    const ImuMeasurement& data, const ImuRole& role, const std::string& tag) :
    imu(std::make_shared<ImuMeasurement>(data)),
    imu_role(role), tag(tag), timestamp(data.timestamp) {}

  EstimatorDataCluster(const std::shared_ptr<cv::Mat>& data, const CameraRole& role, 
                       const std::string& tag, const double time) :
    image(data),
    image_role(role), tag(tag), timestamp(time) {}

  EstimatorDataCluster(
    const FrameBundlePtr& data, const std::string& tag) : 
    frame_bundle(data), timestamp(data->getMinTimestampSeconds()),
    tag(tag) {}

  EstimatorDataCluster(
    const Solution& data, const SolutionRole& role, const std::string& tag) :
    solution(std::make_shared<Solution>(data)),
    solution_role(role), timestamp(data.timestamp), tag(tag) {}

但measurement被具体push_back的地方，又是在estimatorDataCallback这个函数里，关于这个函数的注释我觉得是有点疑惑的，注释给的是“// GNSS data callback”

void MultiSensorEstimating::estimatorDataCallback(EstimatorDataCluster& data)
{
  ...
  measurement_addin_buffer_.push_back(data);
  ...
}

这个函数的引用又是在DataIntegrationBase这个类的构造函数中，这个类呢，又是在NodeHandle的构造函数调用bindStreamerToFormatorToEstimator函数时被调用的

    // initialize data integration handles
    std::vector<std::shared_ptr<DataIntegrationBase>> data_integrations;  // TODO 数据的储存
    data_integrations.push_back(std::make_shared<GnssDataIntegration>(
      estimating, gnss_streamings, gnss_tags, gnss_roles));
    data_integrations.push_back(std::make_shared<ImuDataIntegration>(
      estimating, imu_streamings, imu_tags, imu_roles));
    data_integrations.push_back(std::make_shared<ImageDataIntegration>(
      estimating, image_streamings, image_tags, image_roles));
    data_integrations.push_back(std::make_shared<SolutionDataIntegration>(
      estimating, solution_streamings, solution_tags, solution_roles));
    data_integrations_.back() = data_integrations;

我也不知道理得对不对，这里的数据传递着实有些复杂