一步一步学习S-MSCKF(四)观测模型

S-MSCKF中观测更新的时机是在特征点跟踪丢失之后,其过程如下:

  • 检查跟踪丢失的特征点是否进行过三角化,若果没有,则求其进行三角化。
  • 根据上述已知的特征点坐标以及观测,得到状态量之间的约束,进行观测更新。

1 特征点三角化

特征点三角化分两步,第一步是根据对特征点的某两帧观测值对其坐标进行初始估计;第二步是根据对特征点的多帧观测对其坐标进行修正。

1.1 两帧约束求特征点三维坐标

如上示意图,已知特征点\(f\)\(C_1\)中的归一化坐标为\((z_1(0),z_2(1))\),其在\(C_2\)中的归一化坐标为\((z_2(0),z_2(1))\)\(C_2\)\(C_1\)的转换矩阵为\([R,t]\),求特征点\(f\)\(C_1\)系中的深度\(d\)
特征点在\(C_1\)系中的坐标可以表示为\(d[z_1(0),z_1(2),1]^T\),那么其在\(C_2\)中的坐标可以表示为:

猜你喜欢

转载自www.cnblogs.com/liuzhenbo/p/12600756.html