1、主函数入口:
/test中的run_euroc.cpp的main函数:输入数据路径以及config文件路径,开启线程(thd_BackEnd、thd_PubImuData、thd_PubImageData)
2、thd_PubImuData获取IMU数据
调用System::PubImuData将IMU数据保存在队列imu_buf中
3、thd_PubImageData获取image数据
调用System::PubImageData提取特征点,然后保存到feature_buf中
3.1、System::PubImageData
主要是调用FeatureTracker::readImage对图像进行直方图均衡化cv::createCLAHE、光流跟踪cv::calcOpticalFlowPyrLK、新特征的提取 cv::goodFeaturesToTrack,再对特征点进行去畸变FeatureTracker::undistortedPoints;
4、后端线程std::thread thd_BackEnd(&System::ProcessBackEnd, pSystem):
调用System::ProcessBackEnd:图像数据和对应的IMU数据形成measurements,遍历measurements,分别调用estimator.processIMU和estimator.processImag对IMU数据和图像数据进行处理
4.1、estimator.processIMU
调用new IntegrationBase对IMU数据进行预积分
4.2、Estimator::processImage
判断视差,确定marg策略(两种之一)
如果外参数不知,进行相机imu外参数初始化initial_ex_rotation.CalibrationExRotation,接着进行初始化操作initialStructure,初始化完成之后,进行非线性优化solveOdometry以及滑动窗口操作Estimator::slideWindow
4.2.1Estimator::solveOdometry非线性优化
先进性特征点三角化FeatureManager::triangulate,然后进入后端优化Estimator::backendOptimization
4.2.1.1、Estimator::backendOptimization后端优化操作