地图约束

与状态估计中视觉观测不同的是，在这里我们的$landmark$特征并不是由我们的里程计前端特征三角化后得到的，而是将maplab线程发布的特征点和$landmark$匹配结果作为观测。

观测模型：

$$\begin{array}{rl}{\mathbf{z}}_{m,k}& =\mathbf{h}({\mathbf{x}}_k)+{\mathbf{n}}_k\\ & ={\mathbf{h}}_d({\mathbf{z}}_{n,k},\mathit{\zeta })+{\mathbf{n}}_k\\ & ={\mathbf{h}}_d({\mathbf{h}}_p{(}^{C_k}{\mathbf{p}}_f),\mathit{\zeta })+{\mathbf{n}}_k\\ & ={\mathbf{h}}_d({\mathbf{h}}_p({\mathbf{h}}_t{(}^W{\mathbf{p}}_f{,}_W^{C_k}\mathbf{R},{}^W{\mathbf{p}}_{C_k})),\mathit{\zeta })+{\mathbf{n}}_k\\ & ={\mathbf{h}}_d({\mathbf{h}}_p({\mathbf{h}}_t({\mathbf{h}}_r({\mathbf{}}^{{\mathit{M}}_{\mathit{k}}}{\mathbf{p}}_{\mathit{f}}{,}_M^W\mathbf{R},{}^W{\mathbf{p}}_M){,}_W^{C_k}\mathbf{R},{}^W{\mathbf{p}}_{C_k})),\mathit{\zeta })+{\mathbf{n}}_k\end{array}$$

${\mathbf{z}}_k={\mathbf{h}}_d({\mathbf{z}}_{n,k},\mathit{\zeta })$：将归一化$landmark$作为输入，并将其映射到畸变的像素坐标中的畸变函数

${\mathbf{z}}_{n,k}={\mathbf{h}}_p{(}^{C_k}{\mathbf{p}}_f)$：将相机坐标系中的$landmark$作为输入，将其转换为归一化的像素坐标的投影函数

${}^{C_k}{\mathbf{p}}_f={\mathbf{h}}_t{(}^W{\mathbf{p}}_f{,}_W^{C_k}\mathbf{R},{}^W{\mathbf{p}}_{C_k})$：将世界坐标系中的$landmark$位置转换为当前相机坐标系中

${}^W{\mathbf{p}}_f={\mathbf{h}}_r({\mathbf{}}^{{\mathit{M}}_{\mathit{k}}}{\mathbf{p}}_{\mathit{f}}{,}_M^W\mathbf{R},{}^W{\mathbf{p}}_M)$：将地图坐标系中的$landmark$换为世界坐标系中

其中，${\mathbf{}}^{{\mathit{M}}_{\mathit{k}}}{\mathbf{p}}_{\mathit{f}}$是地图坐标系下的$landmark$；{${}_M^W\mathbf{R},{}^W{\mathbf{p}}_M$}是地图坐标系{$M$}到里程计全局坐标系{$W$}的坐标系转换。

雅可比矩阵：

通过链式求导法则可以计算出总的雅可比矩阵

$$\begin{array}{rl}\frac{\partial {\mathbf{z}}_k}{\partial \mathbf{x}}& =\frac{\partial {\mathbf{h}}_d(\cdot )}{\partial {\mathbf{z}}_{n,k}}\frac{\partial {\mathbf{h}}_p(\cdot )}{{\partial }^{C_k}{\mathbf{p}}_f}\frac{\partial {\mathbf{h}}_t(\cdot )}{\partial \mathbf{x}}+\frac{\partial {\mathbf{h}}_d(\cdot )}{\partial {\mathbf{z}}_{n,k}}\frac{\partial {\mathbf{h}}_p(\cdot )}{{\partial }^{C_k}{\mathbf{p}}_f}\frac{\partial {\mathbf{h}}_t(\cdot )}{{\partial }^W{\mathbf{p}}_f}\frac{\partial {\mathbf{h}}_r(\cdot )}{\partial \mathbf{x}}\end{array}$$

其中：

$$\begin{array}{r}\frac{\partial {\mathbf{h}}_d(\cdot )}{\partial {\mathbf{z}}_{n,i}}=\left[\begin{array}{cc}{f}_x\ast ((1+k_1{r}^2+k_2{r}^4)+(2k_1{x}_n^2+4k_2{x}_n^2(x_n^2+y_n^2))+2p_1{y}_n+(2p_2{x}_n+4p_2{x}_n))& f_x\ast (2k_1{x}_n{y}_n+4k_2{x}_n{y}_n(x_n^2+y_n^2)+2p_1{x}_n+2p_2{y}_n)\\ f_y\ast (2k_1{x}_n{y}_n+4k_2{x}_n{y}_n(x_n^2+y_n^2)+2p_1{x}_n+2p_2{y}_n)& f_y\ast ((1+k_1{r}^2+k_2{r}^4)+(2k_1{y}_n^2+4k_2{y}_n^2(x_n^2+y_n^2))+\begin{array}{r}(2p_1{y}_n+4p_1{y}_n)+2p_2{x}_n)\end{array}\end{array}\right]\end{array}$$$$\begin{array}{rl}\frac{\partial {\mathbf{h}}_p(\cdot )}{{\partial }^{C_k}{\mathbf{p}}_f}& =\left[\begin{array}{ccc}\frac{1}{C_z}& 0& \frac{{-}^{C}x}{{(}^{C}z{)}^2}\\ 0& \frac{1}{C_z}& \frac{{-}^{C}y}{{(}^{C}z{)}^2}\end{array}\right]\\ \frac{\partial {\mathbf{h}}_t(\cdot )}{\partial \mathbf{x}}& =\left[\begin{array}{ccc}{}_I^C\mathbf{R}{|}_W^{I_k}\mathbf{R}({}^W{\mathbf{p}}_f-{}^W{\mathbf{p}}_{I_k}){|}^{\times }& {-}_I^C{\mathbf{R}}_W^{I_k}\mathbf{R}& {}_I^C{\mathbf{R}}_W^{I_k}\mathbf{R}\end{array}\right]\\ \frac{\partial {\mathbf{h}}_t(\cdot )}{{\partial }^W{\mathbf{p}}_f}& ={}_I^C{\mathbf{R}}_W^{I_k}\mathbf{R}\\ \frac{\partial {\mathbf{h}}_r(\cdot )}{\partial \mathbf{x}}& ={}_M^W\mathbf{R}\end{array}$$

相机内参雅可比矩阵：

$$\frac{\partial {\mathbf{h}}_d(\cdot )}{\partial \mathit{\zeta }}=\left[\begin{array}{ccccccc}x& 0& 1& 0& f_x\ast (x_n{r}^2)& f_x\ast (x_n{r}^4)& f_x\ast (2x_n{y}_n)f_x\ast (r^2+2x_n^2)\\ 0& y& 0& 1& f_y\ast (y_n{r}^2)& f_y\ast (y_n{r}^4)& f_y\ast (r^2+2y_n^2)f_y\ast (2x_n{y}_n)\end{array}\right]$$

相机外参雅可比矩阵：

$$\begin{array}{rl}\frac{\partial {\mathbf{h}}_t(\cdot )}{{\partial }_I^C\mathbf{R}}& =|{}_I^C{\mathbf{R}}_W^{I_k}\mathbf{R}({}^W{\mathbf{p}}_f-{}^W{\mathbf{p}}_{I_k}){|}^{\times }\\ \frac{\partial {\mathbf{h}}_t(\cdot )}{{\partial }^C{\mathbf{p}}_I}& ={\mathbf{I}}_{3\times 3}\end{array}$$

地图外参雅可比矩阵：

$$\begin{array}{rl}& \frac{\partial \mathbf{f}(\cdot )}{{\partial }_M^W\mathbf{R}}={|}_M^W{\mathbf{R}}^{M_a}{\mathbf{p}}_f{|}^{\times }\\ & \frac{\partial \mathbf{f}(\cdot )}{{\partial }^W{\mathbf{p}}_M}={\mathbf{I}}_{3\times 3}\end{array}$$