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 三个部分。

转载于:https://www.cnblogs.com/JingeTU/p/8540426.html