OKVIS（一）初始化流程及代碼結構

OKVIS代碼結構：

okvis_apps: your own app

okvis_ceres: backend main code, estimator, error term; 

okvis_common: interface need to be derived, parameterReader;

okvis_cv: frame and camera;

okvis_frontend: visual detection, matching, triangulation and initialization?;

okvis_kinematics: transformation, quaternion algebra;

okvis_matcher: keypoint matcher, which needs several threads;

okvis_multisensor_processing: synchronizer, thread safe queue for sensor data flow; also start multiple threads, and initialization!

okvis_time, okvis_timing: timing;

okvis_util: etc.

VIO的初始化是一個比較重要的問題，和純視覺SLAM初始化只需要三角化出3D地圖點的深度不同，還需要完成相機IMU外參、陀螺儀零偏、尺度以及重力的估計。但是，OKVIS的初始化流程似乎非常簡單，但是需要對傳感器各項參數有較好的先驗值，例如需要在配置文件中給出一個比較靠譜的IMU零偏prior：

    sigma_bg: 0.01 # gyro bias prior [rad/s]
    sigma_ba: 0.1 # accelerometer bias prior [m/s^2]

根據提供的okvis_app_synchronous.cpp，系統入口在類ThreadedKFVio（該類繼承自VioInterface接口）的構造函數中，在okvis_multisensor_processing目錄下找到該類對應的文件，構造函數中調用init()，接着調用startThreads()，開啟各線程：

void ThreadedKFVio::startThreads() {

  // consumer threads
  for (size_t i = 0; i < numCameras_; ++i) {
    frameConsumerThreads_.emplace_back(&ThreadedKFVio::frameConsumerLoop, this, i);
  }
  for (size_t i = 0; i < numCameraPairs_; ++i) {
    keypointConsumerThreads_.emplace_back(&ThreadedKFVio::matchingLoop, this);
  }
  imuConsumerThread_ = std::thread(&ThreadedKFVio::imuConsumerLoop, this);
  positionConsumerThread_ = std::thread(&ThreadedKFVio::positionConsumerLoop,
                                        this);
  gpsConsumerThread_ = std::thread(&ThreadedKFVio::gpsConsumerLoop, this);
  magnetometerConsumerThread_ = std::thread(
      &ThreadedKFVio::magnetometerConsumerLoop, this);
  differentialConsumerThread_ = std::thread(
      &ThreadedKFVio::differentialConsumerLoop, this);

  // algorithm threads
  visualizationThread_ = std::thread(&ThreadedKFVio::visualizationLoop, this);
  optimizationThread_ = std::thread(&ThreadedKFVio::optimizationLoop, this);
  publisherThread_ = std::thread(&ThreadedKFVio::publisherLoop, this);
}

其中，positionConsumerLoop，gpsConsumerLoop，magnetmeterConsumerLoop，differentialConsumerLoop均未實現（暫不提供GPS，磁力計以及差分氣壓計支持），也就是開了6個線程，分別執行6個函數：

void ThreadedKFVio::frameConsumerLoop(size_t cameraIndex)
void ThreadedKFVio::matchingLoop()
void ThreadedKFVio::imuConsumerLoop()
// backend algorithms
void ThreadedKFVio::visualizationLoop()
void ThreadedKFVio::optimizationLoop()
void ThreadedKFVio::publisherLoop()

然后，在okvis_app_synchronous.cpp中，將IMU和camera的數據使用addImage()和addImuMeasurement()傳入，注意OKVIS中數據流采用了阻塞式（可以通過ThreadKFVio.setBlocking()設定）的線程安全隊列。

1. IMU消費者線程：

在imuConsumerLoop()中主要處理imu的propagation

每次imuMeasurementsReceived_隊列中出現IMU數據，就會propagate一次，如果剛完成BA優化（需要repropagationNeeded_），則將優化后的狀態值作為propagation的初值，否則在上一狀態基礎上完成狀態propagation。

主要對應ImuError::propagation()函數，該函數大概兩百行，主要實現OKVIS論文中的 4.2 IMU Kinematics and bias model。

2. Frame消費者線程

2.1 判斷該幀是否關鍵幀（第一幀是關鍵幀）

2.2 利用IMU預測pose，為特征點匹配提供方向參考

在frameConsumerLoop()中Image和IMU的同步策略是這樣的：

若沒有IMU數據，則不處理；IMU第一幀數據之前的那一幀Image也拋棄，下一幀Image（第一幀Frame）才進行特征檢測處理。同時第一幀之前的IMU數據會用來計算pose（該函數返回值永遠是true，因此initPose是否准確完全依賴IMU給出的讀數）：

bool success = okvis::Estimator::initPoseFromImu(imuData, T_WS);

第一幀之后的IMU數據進行propagation（注意multiframe在單目情形下就是frame），注意到這里propagation的covariance和jacobian均為0，僅僅用於預測，對特征點檢測提供先驗的T_WC：

okvis::ceres::ImuError::propagation(imuData, parameters_.imu, T_WS, speedAndBiases, lastTimestamp, multiFrame->timestamp());

2.3 Harris角點檢測+BRISK描述子計算

接下來對frame特征檢測（Harris）和描述子（BRISK）計算（這里的T_WC由前一步的propagation提供，主要為了獲取重力方向，提高描述子匹配魯棒性）：

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

將檢測到的keyPoint都push到隊列中，提供給matchingLoop()線程使用：

keypointMeasurements_.PushBlockingIfFull(multiFrame, 1)

3. Matching線程

該線程需要Frame線程提供的keyPointMeasrements_（阻塞隊列）。

在matching之前，通過frame和imuData的信息，將當前狀態添加到后端估計中去；這里的imuData包含上一幀和當前幀時間戳±20ms范圍內的IMU，因此，在frame附近的IMU數據，是會重復使用一次的。OKVIS的算法可以解決該問題（TODO）。

estimator_.addStates(frame, imuData, asKeyframe)

至此，可以獲取通過上一幀和IMU數據計算出的系統狀態（T_WS和speedAndBias）。

該線程最主要的函數是：

frontend_.dataAssociationAndInitialization(estimator_, T_WS, parameters_, map_, frame, &asKeyframe);

完成

• 特征點匹配；
• 3D點初始化；
• 外點剔除
• RANSAC
• 關鍵幀選擇

在rotationOnly的運動時，使用2D-2D跟蹤，使用IMU給出軌跡；有平移運動可以三角化出3D點時，通過3D-2D匹配計算出pose；這里均使用了Opengv中算法

 

參考：

1. OKVIS代碼框架