-
初始化
- 摄像机参数、
- 噪声方差(图像噪声、IMU噪声、IMU的bias)、
- 初始的IMU协方差、
- IMU和摄像机的外参数、
- IMU和摄像机的时间偏移量*
- MSCKF参数:
+ 状态向量里滑动窗口大小的范围、
+ 空间点三角化误差阈值、
+ 是否做零空间矩阵构造 和 QR分解 - 构造MSCKF状态向量
- 摄像机参数、
-
IMU数据,估计新的MSCKF状态变量和对应的协方差矩阵
-
图像数据处理
- MSCKF状态向量 中 增加当前帧的摄像机位姿;
若位姿数大于滑动窗口大小的范围,
去除状态变量中最早的视图对应的摄像机位姿. - 提取图像特征并匹配,去除外点.
- 处理所有提取的特征。
判断当前特征是否是之前视图中已经观察到的特征- 如果当前帧还可以观测到该特征,则加入该特征的track列表
- 如果当前帧观测不到该特征(Out_of_View),将该特征的track加入到featureTracksToResidualize,用于更新MSCKF的状态变量.
- *给该特征分配新的featureID,并加入到当前视图可观测特征的集合
- 循环遍历featureTracksToResidualize中的track,用于更新MSCKF的状态变量*
- 计算每个track对应的三维空间点坐标 (利用第一幅视图和最后一幅视图计算两视图三角化,使用逆深度参数化和高斯牛顿优化求解),若三角化误差小于设置的阈值,则加入map集合.
- 计算视觉观测(即图像特征)的估计残差,并计算图像特征的雅克比矩阵.
- 计算图像特征雅克比矩阵的左零空间矩阵和QR分解,构造新的雅克比矩阵.
- 计算新的MSCKF状态向量的协方差矩阵.
- 计算Kalman增益.
- 状态矫正.
- 计算新的协方差矩阵.
- 状态变量管理
- 查找所有无feature track可见的视图集合deleteIdx.
- 将deleteIdx中的视图对应的MSCKF中的状态去除掉.
- 绘制运动轨迹.
- MSCKF状态向量 中 增加当前帧的摄像机位姿;
MSCKF算法流程框架
最新推荐文章于 2024-05-22 09:36:17 发布