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：

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

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

 1 void ThreadedKFVio::startThreads() {
 2 
 3   // consumer threads
 4   for (size_t i = 0; i < numCameras_; ++i) {
 5     frameConsumerThreads_.emplace_back(&ThreadedKFVio::frameConsumerLoop, this, i);
 6   }
 7   for (size_t i = 0; i < numCameraPairs_; ++i) {
 8     keypointConsumerThreads_.emplace_back(&ThreadedKFVio::matchingLoop, this);
 9   }
10   imuConsumerThread_ = std::thread(&ThreadedKFVio::imuConsumerLoop, this);
11   positionConsumerThread_ = std::thread(&ThreadedKFVio::positionConsumerLoop,
12                                         this);
13   gpsConsumerThread_ = std::thread(&ThreadedKFVio::gpsConsumerLoop, this);
14   magnetometerConsumerThread_ = std::thread(
15       &ThreadedKFVio::magnetometerConsumerLoop, this);
16   differentialConsumerThread_ = std::thread(
17       &ThreadedKFVio::differentialConsumerLoop, this);
18 
19   // algorithm threads
20   visualizationThread_ = std::thread(&ThreadedKFVio::visualizationLoop, this);
21   optimizationThread_ = std::thread(&ThreadedKFVio::optimizationLoop, this);
22   publisherThread_ = std::thread(&ThreadedKFVio::publisherLoop, this);
23 }

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

1 void ThreadedKFVio::frameConsumerLoop(size_t cameraIndex)
2 void ThreadedKFVio::matchingLoop()
3 void ThreadedKFVio::imuConsumerLoop()
4 // backend algorithms
5 void ThreadedKFVio::visualizationLoop()
6 void ThreadedKFVio::optimizationLoop()
7 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代碼框架