整理部分代码中公式
OKVIS 中的 propagation 代码公式版
http://blog.csdn.net/fuxingyin/article/details/53449465
OKVIS 中的 propagation 公式版
http://blog.csdn.net/fuxingyin/article/details/53449502
OKVIS 计算 liftJacobian
http://blog.csdn.net/fuxingyin/article/details/53449401
OKVIS IMU 误差公式代码版本
http://blog.csdn.net/fuxingyin/article/details/53449209
OKVIS IMU 误差公式版本
http://blog.csdn.net/fuxingyin/article/details/53449365
OKVIS Reprojection Error
http://blog.csdn.net/fuxingyin/article/details/53455200
OKVIS speedAndBias error
http://blog.csdn.net/fuxingyin/article/details/53453225
OKVIS RelativePoseError
http://blog.csdn.net/fuxingyin/article/details/53455306
OKVIS poseError
http://blog.csdn.net/fuxingyin/article/details/53455412
公式较多,难免有错误,错误的地方请指教。
多线程初始化
okvis_app_synchronous.cpp 中 main 函数,main 函数中初始化 okvis_estimator,okvis_estimator 初始时构造了几个线程:
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
几个线程并行执行,做位姿估计。
imuConsumerLoop
将 imu data push 进 imuMeasurements_ 给 frameConsumerLoop 处理。IMU data 每获取一次,imuConsumerLoop 就 propagation 一次,如果是刚优化完,则 propagation 的初值设为刚优化完的结果,如果是不是刚优化完,则初值是上一次 propagation 结果。propagation 的结果根据参数可以给 publisherLoop,publisherLoop 负责绘制系统状态的黑色小窗口图像。
frameConsumerLoop
每个相机开启一个 frameConsumerLoop 线程,frameConsumerLoop 对当前的输入图像做特征提取,注意到有个 propagation,在特征提取前会根据 IMU 上一帧状态和当前 IMU 测量预测当前帧状态,当前帧的状态会参与到特征提取中(特征提取是带方向的,见 Frame::describe(const Eigen::Vector3d & extractionDirection) 函数)。特征检测的结果给 matchingLoop。
- 1
- 2
- 3
- 4
- 5
- 6
- 7
matchingLoop
因为这里是双目相机,只会开启一个 matchingLoop 线程(对于多目相机每两个相机开启一个线程),注意到函数里代码有个 addStates,addStates 会向优化库 ceres 中添加待优化的参数,在参数添加前也会有个 propagation 操作,propagation 给添加的参数赋初始值。addStates 还会添加 IMU 误差。特征匹配的结果给 optimizationLoop。
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
特征匹配在 dataAssociationAndInitialization 中完成:
- 1
- 2
- 3
- 4
- 5
匹配成功的 3维特征点和 2D 图像特征建立重投影误差,建立重投影误差见函数 ::ceres::ResidualBlockId Estimator::addObservation。
optimizationLoop
负责优化和边缘化,并且把优化和边缘化后的结果根据参数选择可以给 publisherLoop(也可以用 imuConsumerLoop 得到的 IMU 结果),publisherLoop 负责回调 poseViewer 中的 fullStateCallback_ 函数,绘制黑色小窗口图片。把特征匹配结果给 visualizationLoop,visualizationLoop 负责绘制两幅大图像。
visualizationLoop
绘制两幅大图像,两幅大图像实际有四幅图像组成,上面两幅图像是关键帧图像,下面两幅是当前帧图像,中间黑色窗口的图像是 publisherLoop 回调 poseViewer 中的 publishFullStateAsCallback 函数绘制的。
publisherLoop
回调 poseViewer 中的 publishFullStateAsCallback 函数绘制中间黑色窗口图像。
附
两幅大图像和中间小图像的显示在 main 函数中执行: