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