OKVIS初始化流程及代码结构

Posted 2020-10-11 SLAM-BJTShang

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!

okvis_time, okvis_timing: timing;

okvis_util: etc.

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()