测量模型
考虑一个特征点\(f_j\)被位置在\((^{C_i}_Gq,{^{G}p}_{C_i})\)的双目相机观测到。双目的左右相机位姿可以表示为\((^{C_{i,1}}_Gq,^{G}p_{C_{i,1}})\)和\((^{C_{i,2}}_Gq,^{G}p_{C_{i,2}})\)。虽然状态向量仅仅包含左相机的位姿，但是相机左目与右目的位姿可以通过双目相机的外参产生联系。
双目的观测值（归一化坐标）可以表达为：
\[z^{j}_i=
\left[\begin{matrix}
u_{i,1}^{j} \\ v_{i,1}^{j} \\ u_{i,2}^{j} \\ v_{i,2}^{j}
\end{matrix}\right]=
\left[\begin{matrix}
\frac{1}{^{C_{i,1}}Z_j}I_{2\times 2} & 0_{2\times 2} \\
0_{2\times 2} & \frac{1}{^{C_{i,2}}Z_j}I_{2\times 2}
\end{matrix}\right]
\left[\begin{matrix}
^{C_{i,1}X_j} \\ ^{C_{i,1}Y_j} \\ ^{C_{i,2}X_j} \\ ^{C_{i,2}Y_j}
\end{matrix}\right]+n_i^j
\]
左右目的观测值与相机的位姿的关系为：
\[^{C_{i,1}}p_j=\left[\begin{matrix}{^{C_{i,1}}X_j} \\ {^{C_{i,1}}Y_j} \\ {^{C_{i,1}}Z_j}\end{matrix}\right]=C({^{C_{i,1}}_Gq})({^Gp_j}-{^Gp_{C_{i,1}}})
\]
\[^{C_{i,2}}p_j=\left[\begin{matrix}{^{C_{i,2}}X_j} \\ {^{C_{i,2}}Y_j} \\ {^{C_{i,2}}Z_j}\end{matrix}\right]=C({^{C_{i,2}}_Gq})({^Gp_j}-{^Gp_{C_{i,2}}})=
C(^{C_{i,2}}_{C_{i,1}}q)(^{C_{i,1}}p_j-{^{C_{i,1}}p_{C_{i,2}}})
\]
从上面可以发现右目观测量可以用左目位姿来表示。其中\(^Gp_j\)是前面已经计算得到的特征点三维坐标。
对\(z_i^j\)在估计值\(\hat x\)处泰勒展开得到：
\[z_i^j=
\left[\begin{matrix}
\hat u_{i,1}^{j} \\ \hat v_{i,1}^{j} \\ \hat u_{i,2}^{j} \\ \hat v_{i,2}^{j}
\end{matrix}\right]+
\frac{\partial z^j_i}{\partial x_{C_{i,1}}}\tilde x_{C_i}+
\frac{\partial z^j_i}{\partial {^Gp_j}} {^G \tilde p_j}+n_i^j
\]
原文地址：https://www.cnblogs.com/liuzhenbo/p/12614652.html