OKVIS框架之前端

1. 数据流入

在okvis_app_sychronous.cpp内，把IMU和图像数据加入到各自的队列里。由ThreadedKFVio负责队列的各种操作。作者对队列加了特殊功能，保证队列是线程安全的。比如：在push时，当超过最大设定值，可以选择是阻塞还是丢掉最老的数据。在pop时也有互斥锁。

  /// rief Push to the queue if the size is less than max_queue_size, else block.
  /// param[in] value New entry in queue.
  /// param[in] max_queue_size Maximum queue size.
  /// 
eturn False if shutdown is requested.
  bool PushBlockingIfFull(const QueueType& value, size_t max_queue_size) {
    while (!shutdown_) {
      pthread_mutex_lock(&mutex_);
      size_t size = queue_.size();
      if (size >= max_queue_size) {
        pthread_cond_wait(&condition_full_, &mutex_);
      }
      if (size >= max_queue_size) {
        pthread_mutex_unlock(&mutex_);
        continue;
      }
      queue_.push(value);
      pthread_cond_signal(&condition_empty_);  // Signal that data is available.
      pthread_mutex_unlock(&mutex_);
      return true;
    }
    return false;
  }

  /// rief Push to the queue. If full, drop the oldest entry.
  /// param[in] value New entry in queue.
  /// param[in] max_queue_size Maximum queue size.
  /// 
eturn True if oldest was dropped because queue was full.
  bool PushNonBlockingDroppingIfFull(const QueueType& value,
                                     size_t max_queue_size) {
    pthread_mutex_lock(&mutex_);
    bool result = false;
    if (queue_.size() >= max_queue_size) {
      queue_.pop();
      result = true;
    }
    queue_.push(value);
    pthread_cond_signal(&condition_empty_);  // Signal that data is available.
    pthread_mutex_unlock(&mutex_);
    return result;
  }

  /**
   * @brief Get the oldest entry still in the queue. Blocking if queue is empty.
   * @param[out] value Oldest entry in queue.
   * @return False if shutdown is requested.
   */
  bool Pop(QueueType* value) {
    return PopBlocking(value);
  }

2. IMU数据处理线程---imuConsumerLoop()

此线程主要是进行IMU积分，获得最新的没有优化过的位姿，以及速度，偏置等信息。每当位姿优化过后，会使repropagationNeeded置为真，则在优化后的参数上进行积分处理。

 if (parameters_.publishing.publishImuPropagatedState) {
        if (!repropagationNeeded_ && imuMeasurements_.size() > 0) {
          start = imuMeasurements_.back().timeStamp;
        } else if (repropagationNeeded_) {
          std::lock_guard<std::mutex> lastStateLock(lastState_mutex_);
          start = lastOptimizedStateTimestamp_;
          T_WS_propagated_ = lastOptimized_T_WS_;
          speedAndBiases_propagated_ = lastOptimizedSpeedAndBiases_;
          repropagationNeeded_ = false;
        } else
          start = okvis::Time(0, 0);
        end = &data.timeStamp;
      }
      imuMeasurements_.push_back(data);
    }  // unlock _imuMeasurements_mutex
    
    std::cout<<"IMU loop"<<data.timeStamp<<std::endl;

    // notify other threads that imu data with timeStamp is here.
    imuFrameSynchronizer_.gotImuData(data.timeStamp);

    if (parameters_.publishing.publishImuPropagatedState) {
      Eigen::Matrix<double, 15, 15> covariance;
      Eigen::Matrix<double, 15, 15> jacobian;

      frontend_.propagation(imuMeasurements_, imu_params_, T_WS_propagated_,
                            speedAndBiases_propagated_, start, *end, &covariance,
                            &jacobian);
      OptimizationResults result;
      result.stamp = *end;
      result.T_WS = T_WS_propagated_;
      result.speedAndBiases = speedAndBiases_propagated_;
      result.omega_S = imuMeasurements_.back().measurement.gyroscopes
          - speedAndBiases_propagated_.segment<3>(3);
      for (size_t i = 0; i < parameters_.nCameraSystem.numCameras(); ++i) {
        result.vector_of_T_SCi.push_back(
            okvis::kinematics::Transformation(
                *parameters_.nCameraSystem.T_SC(i)));
      }
      result.onlyPublishLandmarks = false;
      optimizationResults_.PushNonBlockingDroppingIfFull(result,1);
    }

3. 图像数据处理线程---frameConsumerLoop(size_t cameraIndex)

3.1 生成multiFrame类

每一个相机都会有一个线程处理图像，所以这里需要把两个相机的图像融合到一个数据结构里，也就是multiFrame。

 multiFrame = frameSynchronizer_.addNewFrame(frame);

此函数内部，不做检测，仅仅是融合左右两个图像到一个multiFrame里。

3.2 IMU预积分

如果是第一帧图像对，则不进行预积分。这里预积分是为了获得TWS，然后的出相机位姿，因为对特征点进行描述时，需要方向信息。但是目前左右帧都会进行预积分，然后在后端优化时候，estimator类里也会进行预积分。不明白对于同一对图像为什么不能只进行一次预积分。

3.3 特征点检测以及描述子计算

frontend_.detectAndDescribe(frame->sensorId, multiFrame, T_WC, nullptr);

这里没有用什么特殊的方法，都是opencv内的特征提取方法。

for (size_t i = 0; i < numCameras_; ++i) {
    featureDetectors_.push_back(
        std::shared_ptr<cv::FeatureDetector>(
#ifdef __ARM_NEON__
            new cv::GridAdaptedFeatureDetector( 
            new cv::FastFeatureDetector(briskDetectionThreshold_),
                briskDetectionMaximumKeypoints_, 7, 4 ))); // from config file, except the 7x4...
#else
            new brisk::ScaleSpaceFeatureDetector<brisk::HarrisScoreCalculator>(
                briskDetectionThreshold_, briskDetectionOctaves_, 
                briskDetectionAbsoluteThreshold_,
                briskDetectionMaximumKeypoints_)));
#endif
    descriptorExtractors_.push_back(
        std::shared_ptr<cv::DescriptorExtractor>(
            new brisk::BriskDescriptorExtractor(
                briskDescriptionRotationInvariance_,
                briskDescriptionScaleInvariance_)));
  }

4. 匹配线程---matchingLoop()

在判断左右两幅图像都检测完后，则把数据传给匹配线程。匹配过程略复杂，而且代码使用了很多模板，比较难看懂。在进行图像匹配前，作者先把状态参数，传递给后端，添加各种误差项。这里不太明白为什么要在匹配前进行。

 if (estimator_.addStates(frame, imuData, asKeyframe)) 
      {
        lastAddedStateTimestamp_ = frame->timestamp();
        addStateTimer.stop();
      } else {
        LOG(ERROR) << "Failed to add state! will drop multiframe.";
        addStateTimer.stop();
        continue;
      }

      // -- matching keypoints, initialising landmarks etc.
      okvis::kinematics::Transformation T_WS;
      estimator_.get_T_WS(frame->id(), T_WS);
      matchingTimer.start();
      frontend_.dataAssociationAndInitialization(estimator_, T_WS, parameters_, map_, frame, &asKeyframe);
      matchingTimer.stop();

然后先让current frame 和以前所有的关键帧进行匹配。然后再和lastFrame进行匹配，最后进行左右立体匹配。每种匹配后都会在setBestMatch函数内增加每个点的投影误差。matchingLoop有点像前端和后端的桥梁，在这里准备后端优化所需要的数据。