OKVIS 代碼框架


1. okvis_app_synchronous.cpp

在此文件中 okvis 對象為 okvis_estimator,是類 okvis::ThreadedKFVio 的實例化對象。

數據輸入接口是 ThreadedKFVio::addImuMeasurement 和 ThreadedKFVio::addImage。

整個數據輸入的代碼,在 for(...; i < numCameras; ...) 內,可支持多個相機的影像。

此文件是為 EuRoC 數據集寫的入口,所以 IMU 只有 1 個,也沒有為多個 IMU 提供便利。

輸入 IMU 數據的代碼如下,其中 deltaT 為 0,而 start 是第一張影像的時間。此 if 判斷,將輸入的 IMU 時間控制在,[start - 1, +inf],IMU 需要在影像 1s 前啟動初始化。

if (t_imu - start + okvis::Duration(1.0) > deltaT) {
  okvis_estimator.addImuMeasurement(t_imu, acc, gyr);
}

2. ThreadedKFVio::addImuMeasurement 和 ThreadedKFVio::addImage

IMU 數據與相機數據在這兩個函數中進行封裝(okvis::ImuMeasurement 和 okvis::CameraMeasurement),隨后 push 到隊列的尾部,等待其他線程進行處理。

這兩個隊列實例是 imuMeasurementReceived_ 和 cameraMeasurementReceived_。對應的類是 okvis::threadsafe::ThreadSafeQueue,嗯,這個類可以仔細看看,對小白來說很有價值。對數據的處理有兩種方式,blocking 和 non blocking,控制對列繁忙的時候是不是要把數據丟棄。

3. ThreadedKFVio 中的線程

okvis 開了一些線程進行不同部分的計算,可以在 Threaded::startThreads 中看到這些進程的啟動。這個函數中啟動了一些現在沒有用的線程,如 position, gps, manetometer, differential 數據處理的線程,這些數據當前都沒有支持。

主要關注的線程函數有,frameConsumerLoop 和 imuConsumerLoop 數據預處理,matchingLoop 影像匹配,optimizationLoop 后端優化,publisherLoop publish 最終位姿計算結果。

3.1 ThreadedKFVio::imuConsumerLoop

從 imuMeasurementReceived_ 中 pop 出 imu 數據,push 到 imuMeasurements_ 這個 std::deque 中去。隨后在 frameConsumerLoop 中調用函數 ThreadedKFVio::getImuMeasurements,將其取出進行處理。

最后 imuFrameSynchronizer_.gotImuData(data.timeStamp) 向其他等待的線程申明已經把該時刻之前的 imu 數據放入 imuMeasurements_ 中。

如果設置中設定了要 publish imu 積分結果,就進行一次 imu 積分,frontend_.propagation,積分的結果存放在 optimizationResults_ 中。在 ThreadedKFVio.cpp 中搜一下 optimizationResults_ 的出現場景,實際上對其中數據的輸入、輸出只另外出現在了 optimizationLoop 輸入和 publisherLoop 輸出。

3.2 ThreadedKFVio::frameConsumerLoop

這個函數有 numCameras_ 個,之間用函數參數 cameraIndex 區別。作用是提取每一張影像上的 brisk 特征點,並計算描述子。

先從 cameraMeasurementsReceived_ 中 pop 出 camera 數據 frame,使用 frameSynchronizer_.addNewFrame(frame) 中得到一個 okvis::MultiFrame,MultiFrame 是同一時刻所有相機拍攝到影像的集合(當然也不是完全絕對的同一時刻,總是有點偏差,具體細節看 Frame:Synchronizer::addNewFrame)。

現在,multiFrame 代表了這一張影像。因為特征點需要絕對的方向進行描述,所以現在需要使用 imu 數據積分算當前影像的姿態。若是第一幀,imu 沒有啟動,使用 okvis::Estimator::initPoseFromImu 進行啟動。如果不是第一幀,則使用 okvis::ceres::ImuError::propagation 積分得到當前的姿態。而這個積分的起點是 lastOptimized_T_WS_,是 optimizationLoop 得到的上一幀影像優化結果(publish 出去的最終姿態)。

得到當前影像位姿(中間結果,未優化),進行特征點處理。

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

bool push = false;之后的部分,就是一些后續的結尾工作。

frameSynchronizer_.detectionEndedForMultiFrame(multiFrame->id());

為當前 multiFrame 的處理計數 +1,當該計數為 numCameras_ 時,說明這個 multiFrame 處理完成了,下面這個 if 語句為真。

if (frameSynchronizer_.detectionCompletedForAllCameras(multiFrame->id())) {
  push = true;
}

push 為真,西面就可以把當前 multiFrame push 到 keypointMeasurements_ 中,matchingLoop 會從 keypointMeasurements_ 中取數據。

3.3 ThreadedKFVio::matchingLoop

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

與上一個關鍵幀進行匹配,當前幀不同影像之間進行 Stereo Matching。

if (estimator_.addStates(frame, imuData, asKeyframe)) {
  ...
}

estimator_ 是后端優化的實例,這里又將當前幀與上一幀(上一次 matchingLoop 處理的幀)之間的 imu 提取出存儲在 imuData 中,加入到后端中。

3.4 ThreadedKFVio::optimizationLoop

真正的后端處理在這個函數中。分為三個部分:optimization, marginalization, afterOptimization 三個部分。


免責聲明!

本站轉載的文章為個人學習借鑒使用,本站對版權不負任何法律責任。如果侵犯了您的隱私權益,請聯系本站郵箱yoyou2525@163.com刪除。



 
粵ICP備18138465號   © 2018-2025 CODEPRJ.COM