OKVIS（一）初始化流程及代码结构
OKVIS代码结构：
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代码框架
