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^]

根据提供的okvis_app_synchronous.cpp，系统入口在类ThreadedKFVio（该类继承自VioInterface接口）的构造函数中，在okvis_multisensor_processing目录下找到该类对应的文件，构造函数中调用init()，接着调用startThreads()，开启各线程：

 void ThreadedKFVio::startThreads() {

   // consumer threads

   for (size_t i = ; i < numCameras_; ++i) {

     frameConsumerThreads_.emplace_back(&ThreadedKFVio::frameConsumerLoop, this, i);

   }

   for (size_t i = ; 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, )

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代码框架