一步一步学习S-MSCKF（四）特征点三角化

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$。

特征点$f$在$C_1$系中的坐标可以表示为$d[z_1(0),z_1(2),1{]}^T$，那么其在$C_2$中的坐标可以表示为$Rd[z_1(0),z_1(2),1{]}^T+t$，设$m=R[z_1(0),z_1(2),1{]}^T$，那么特征点在$C_2$中可以表示为：

$$S=dm+t=\left[\begin{array}{c}dm(0)+t(0)\\ dm(1)+t(1)\\ dm(2)+t(2)\end{array}\right]$$

将特征点在$C_2$中的归一化坐标同时乘以$dm(2)+t(2)$，得到$S^{{}^{\prime }}$，那么使$||S-S^{{}^{\prime }}||$最小即可。
$$S-S^{{}^{\prime }}=\left[\begin{array}{c}dm(0)+t(0)-z_2(0)\left[dm(2)+t(2)\right]\\ dm(1)+t(1)-z_2(1)[dm(2)+t(2)]\\ dm(2)+t(2)-[dm(2)+t(2)]\end{array}\right]=\left[\begin{array}{c}[m(0)-z_2(0)m(2)]d-[z_2(0)t(2)-t(0)]\\ [m(1)-z_2(1)m(2)]d-[z_2(1)t(2)-t(1)]\\ 0\end{array}\right]$$
也即是$||Ad-b||$最小即可：
d ^ = min ⁡ d   { [ d A ( 0 ) − b ( 0 ) ] 2 + [ d A ( 1 ) + b ( 1 ) ] 2 } = A ( 0 ) b ( 0 ) + A ( 1 ) b ( 1 ) A ( 0 ) 2 + A ( 1 ) 2 = ( A − 1 A ) − 1 A T b \hat d=\min_{d}\, \{ [dA(0)-b(0)]^2+[dA(1)+b(1)]^2 \}=\frac{A(0)b(0)+A(1)b(1)}{{A(0)}^{2}+{A(1)}^2}=(A^{-1}A)^{-1}A^{T}b d^=dmin​{[dA(0)−b(0)]2+[dA(1)+b(1)]2}=A(0)2+A(1)2A(0)b(0)+A(1)b(1)​=(A−1A)−1ATb
所以得到特征点$f$的三维坐标p的初始估计：
$$p(0)=z_1(0)d,p(1)=z_1(1)d,p(2)=d$$

1.2 多帧约束

因为某个特征点在滑动窗口中并不一定只被两帧观测到，所以我们需要根据特征点在多个相机中的观测来进一步优化特征点的三维坐标。

为了提高数值计算的稳定性，特征点的坐标这里采用逆深度表示。比如特征点在某相机坐标系中的坐标为$[X,Y,Z{]}^T$，那么其逆深度表示为$[\alpha ,\beta ,\rho {]}^T=[\frac{X}{Y},\frac{Y}{Z},\frac{1}{Z}{]}^T$。

设特征点为$f_j$，选取相机系$C_n$参考系，那么$f_j$在相机系$C_i$的坐标为：
$${}^{C_i}{p}_{f_j}=C{(}_{C_n}^{C_i}q){}^{C_n}{p}_{f_j}+{}^{C_i}{p}_{C_n}$$
将逆深度带入得到：
$$\begin{gathered} {}^{C_i}{p}_{f_j}={}^{C_n}{Z}_j\left(C({}_{C_n}^{C_i}q)\left[\begin{array}{c}\frac{{}^{C_n}{X}_j}{{}^{C_n}{Z}_j}\\ \frac{{}^{C_n}{Y}_j}{{}^{C_n}{Z}_j}\\ 1\end{array}\right]+\frac{1}{{}^{C_n}{Z}_j}{}^{C_i}{p}_{C_n}\right)={}^{C_n}{Z}_j\left(C({}_{C_n}^{C_i}q)\left[\begin{array}{c}{\alpha }_j\\ {\beta }_j\\ 1\end{array}\right]+{\rho }_j{}^{C_i}{p}_{C_n}\right) \\ ={}^{C_n}{Z}_j\left[\begin{array}{c}{h}_{i1}({\alpha }_j,{\beta }_j,\rho )\\ h_{i2}({\alpha }_j,{\beta }_j,\rho )\\ h_{i3}({\alpha }_j,{\beta }_j,\rho )\end{array}\right] \end{gathered}$$

那么$f_j$在$C_i$系中的观测为：（忽略内参，这里使用归一化坐标）
$$z_i^{(j)}=\frac{1}{h_{i3}({\alpha }_j,{\beta }_j,\rho )}\left[\begin{array}{c}{h}_{i1}({\alpha }_j,{\beta }_j,\rho )\\ h_{i2}({\alpha }_j,{\beta }_j,\rho )\end{array}\right]+n_i^{(j)}$$
设$f_j$在$C_1,...,C_m$系下的观测值为$z_1^{(j)},...,z_m^{(j)}$，那么我们可以构建一个最小二乘问题：
$$\min \frac{1}{2}\sum _{i=1}^m||e_i({\alpha }_j,{\beta }_j,{\rho }_j)|{|}^2=\min \frac{1}{2}\sum _{i=1}^m||z_i^{(j)}-{\hat{z}}_i^{(j)}|{|}^2$$
其中${\hat{z}}_i^{(j)}$为：
$${\hat{z}}_i^{(j)}=\frac{1}{h_{i3}({\hat{\alpha }}_j,{\hat{\beta }}_j,\hat{\rho })}\left[\begin{array}{c}{h}_{i1}({\hat{\alpha }}_j,{\hat{\beta }}_j,\hat{\rho })\\ h_{i2}({\hat{\alpha }}_j,{\hat{\beta }}_j,\hat{\rho })\end{array}\right]$$
其中误差对优化参数的雅可比矩阵为：
$$\begin{gathered} J_i=\frac{\partial e_i}{\partial ({\hat{\alpha }}_j,{\hat{\beta }}_j,{\hat{\rho }}_j)}=-\frac{\partial {\hat{z}}_i^{(j)}}{\partial h}\frac{\partial h}{\partial ({\hat{\alpha }}_j,{\hat{\beta }}_j,{\hat{\rho }}_j)}=-\frac{\partial {\hat{z}}_i^{(j)}}{\partial h}\left[\begin{array}{ccc}\frac{\partial h}{\partial {\hat{\alpha }}_j}& \frac{\partial h}{\partial {\hat{\beta }}_j}& \frac{\partial h}{\partial {\hat{\rho }}_j}\end{array}\right]= \\ -\left[\begin{array}{ccc}\frac{1}{h_{i1}({\hat{\alpha }}_j,{\hat{\beta }}_j,{\hat{\rho }}_j)}& 0& -\frac{h_{i1}({\hat{\alpha }}_j,{\hat{\beta }}_j,{\hat{\rho }}_j)}{h_{i3}^2({\hat{\alpha }}_j,{\hat{\beta }}_j,{\hat{\rho }}_j)}\\ 0& \frac{1}{h_{i3}({\hat{\alpha }}_j,{\hat{\beta }}_j,{\hat{\rho }}_j)}& -\frac{h_{i2}({\hat{\alpha }}_j,{\hat{\beta }}_j,{\hat{\rho }}_j)}{h_{i3}^2({\hat{\alpha }}_j,{\hat{\beta }}_j,{\hat{\rho }}_j)}\end{array}\right]\left[\begin{array}{ccc}C{(}_{C_n}^{C_i}q)\left(\begin{array}{c}1\\ 0\\ 0\end{array}\right)& C{(}_{C_n}^{C_i}q)\left(\begin{array}{c}0\\ 1\\ 0\end{array}\right)& {}^{C_i}{p}_{C_n}\end{array}\right] \end{gathered}$$
整体Jacobian矩阵和残差向量为：
$$J=\left[\begin{array}{c}{J}_1\\ J_2\\ ⋮\\ J_m\end{array}\right]e=\left[\begin{array}{c}{e}_1\\ e_2\\ ⋮\\ e_m\end{array}\right]$$
利用Levenberg-Marquart方法迭代：
$$(J^{H}J+\lambda I)\delta =J^{H}e$$
$$({\hat{\alpha }}_j,{\hat{\beta }}_j,{\hat{\rho }}_j)=({\hat{\alpha }}_j,{\hat{\beta }}_j,{\hat{\rho }}_j)+\delta$$
最后将$C_n$坐标系下的特征点坐标转换到世界坐标系下：
$${}^G{\hat{p}}_{f_j}=\frac{1}{{\hat{\rho }}_j}{C{(}_G^{C_n}\hat{q})}^T\left[\begin{array}{c}{\hat{\alpha }}_j\\ {\hat{\beta }}_j\\ 1\end{array}\right]+{}^G{\hat{p}}_{C_n}$$

参考文献

(1) A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation
(2) Robust Stereo Visual Inertial Odometry for Fast Autonomous Flight
(3) Indirect Kalman Filter for 3D Attitude Estimation