OKVIS框架之前端

1. 数据流入

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

  /// \brief 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.

  /// \return 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;

  }

  /// \brief 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.

  /// \return 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有点像前端和后端的桥梁，在这里准备后端优化所需要的数据。

巴特西