参考链接：OpenVINS https://docs.openvins.com/index.html

4.Measurement Update Derivations

4.1 最小均方误差 （MMSE） 估计
  考虑以下静态状态估计问题：给定 $n$ 维高斯随机向量 $x\backsim \mathcal{N}({\hat{x}}^{㊀},P_{xx}^{㊀})$ 的先验分布（概率密度函数或pdf），和一个新的 $m$ 维具有状态无关的零均值白高斯噪声 $n\backsim \mathcal{N}(0,R)$ 的观测 $z_m=z+n=h(x)+n$，我们想要计算后验PDF的前两个（中心）时刻 $p(x|z_m)$。通常（给定非线性测量模型），我们将后验pdf近似为：$p(x|z_m)\approx \mathcal{N}({\hat{x}}^{\oplus },P_{xx}^{\oplus })$。根据设计，这是MMSE估计问题的（近似）解决方案 Kay 1993。

4.2 条件概率分布
  为此，我们采用贝叶斯规则：
$$p(x|z_m)=\frac{p(x,z_m)}{p(z_m)}$$
  通常，如果不施加简化的假设，则无法分析计算此条件pdf。对于手头的问题，我们首先近似 $p(z_m)\approx \mathcal{N}(\hat{z},P_{zz})$（如果确实如此），然后有以下联合高斯pdf（注意高斯pdf的联合仍是符合高斯的）：
$$p(x,z_m)=\mathcal{N}\left(\begin{array}{c}\left[\begin{array}{c}{\hat{x}}^{㊀}\\ z\end{array}\right],\left[\begin{array}{cc}{P}_{xx}& P_{xz}\\ P_{zx}& P_{zz}\end{array}\right]\end{array}\right)=:\mathcal{N}(\hat{y},P_{yy})$$
将这两个高斯代入第一个方程得到以下条件高斯pdf：

$$p(x|z_m)\approx \frac{\mathcal{N}(\hat{y},P_{yy})}{\mathcal{N}(\hat{z},P_{zz})}$$$$=\frac{1}{\sqrt{(2\pi {)}^n|P_{yy}|/|P_{zz}|}}{e}^{-\frac{1}{2}[(y-\hat{y}{)}^{\top }{P}_{yy}^{-1}(y-\hat{y})-(z_m-\hat{z}{)}^{\top }{P}_{zz}^{-1}(z_m-\hat{z})]}$$$$=:\mathcal{N}({\hat{x}}^{\oplus },P_{xx}^{\oplus })$$
  我们现在推导出条件均值，协方差可以计算如下：首先，我们简化分母项 $|P_{yy}|/|P_{zz}|$ 以找到条件协方差。
$$|P_{yy}|=\left|\begin{array}{c}\left[\begin{array}{cc}{P}_{xx}& P_{xz}\\ P_{zx}& P_{zz}\end{array}\right]\end{array}\right|=\left|\begin{array}{c}{P}_{xx}-P_{xz}{P}_{zz}^{-1}{P}_{zx}\end{array}\right|\left|\begin{array}{c}{P}_{zz}\end{array}\right|$$
  我们假设 $P_{zz}$ 是可逆的，并采用了舒尔补的行列式性质。因此，我们有：
$$\frac{|P_{yy}|}{|P_{zz}|}=\frac{\left|\begin{array}{c}{P}_{xx}-P_{xz}{P}_{zz}^{-1}{P}_{zx}\end{array}\right|}{|P_{zz}|}=\left|\begin{array}{c}{P}_{xx}-P_{xz}{P}_{zz}^{-1}{P}_{zx}\end{array}\right|$$

接下来，通过定义误差状态 $r_x=x-{\hat{x}}^{㊀}$，$r_z=z_m-\hat{z}$，$r_y=y-\hat{y}$ 并使用矩阵内引理，我们重写指数项如下：
$$(y-\hat{y}{)}^{\top }{P}_{yy}^{-1}(y-\hat{y})-(z_{,}-\hat{z}{)}^{\top }{P}_{zz}^{-1}(z_m-\hat{z})$$$$=r_y^{\top }{P}_{yy}^{-1}{r}_y-r_z^{\top }{P}_{zz}^{-1}{r}_z$$$$={\left[\begin{array}{c}{r}_x\\ rz\end{array}\right]}^{\top }{\left[\begin{array}{cc}{P}_{xx}& P_{xz}\\ P_{zx}& P_{zz}\end{array}\right]}^{-1}\left[\begin{array}{c}{r}_x\\ rz\end{array}\right]-r_z^{\top }{P}_{zz}^{-1}{r}_z$$$$=(r_x-P_{xz}{P}_{zz}^{-1}{r}_z{)}^{\top }(P_{xx}-P_{xz}{P}_{zz}^{-1}{P}_{zx}{)}^{-1}(r_x-P_{xz}{P}_{zz}^{-1}{r}_z)$$
  其中，$(p_{zz}^{-1}{)}^{\top }=P_{zz}^{-1}$ 由于协方差矩阵是对称的。到目前为止，我们现在可以构建条件高斯pdf，如下所示：
$$p(x_k|z_m)=\frac{1}{\sqrt{(2\pi {)}^n|P_{xx}-P_{xz}{P}_{zz}^{-1}{P}_{zx}|}}\times exp\left(\begin{array}{c}-\frac{1}{2}[(r_x-P_{xz}{P}_{zz}^{-1}{r}_z{)}^{\top }(P_{xx}-P_{xz}{P}_{zz}^{-1}{P}_{zx}{)}^{-1}(r_x-P_{xz}{P}_{zz}^{-1}{r}_z)]\end{array}\right)$$

这引出我们正在寻找的以下条件均值和协方差：
$${\hat{x}}^{\oplus }={\hat{x}}^{㊀}+P_{xz}{P}_{zz}^{-1}(z_m-\hat{z})$$$$P_{xx}^{\oplus }=P_{xx}^{㊀}-P_{xz}{P}_{zz}^{-1}{P}_{zx}$$
这些是（线性）状态估计的基本方程。

4.3 线性测量更新
  作为一个特例，我们考虑一个简单的线性测量模型来说明线性MMSE估计器：
$$z_{m,k}=H_k{x}_k+n_k$$$${\hat{z}}_k:=\mathbb{E}[z_{m,k}]=\mathbb{E}[H_k{x}_k+n_k]=H_k{\hat{x}}_k^{㊀}$$
有了这个，我们可以推导出协方差和相关矩阵如下：
$$P_{zz}=\mathbb{E}[(z_{m,k}-{\hat{z}}_k)(z_{m,k}-{\hat{z}}_k{)}^{\top }]$$$$=\mathbb{E}[(H_k{x}_k+n_k-H_k{\hat{x}}_k^{㊀})(H_k{x}_k+n_k-H_k{\hat{x}}_k^{㊀}{)}^{\top }]$$$$=H_k{P}_{xx}^{㊀}{H}_k^{\top }+R_k$$
其中 $R_k$ 是离散测量噪声矩阵，$H_k$ 是将状态映射到测量域的测量雅可比矩阵，$P_{xx}^{㊀}$是当前状态协方差。
$$P_{xz}=\mathbb{E}[(x_k-{\hat{x}}_k^{㊀})(z_{m,k}-{\hat{z}}_k{)}^{\top }]$$$$=\mathbb{E}[(x_k-{\hat{x}}_k^{㊀})(H_k{x}_k+n_k-H_k{\hat{x}}_k^{㊀}{)}^{\top }]$$$$=P_{xx}^{㊀}{H}_k^{\top }$$
我们采用了噪声独立于状态的事实。将这些量代入基本方程可得到以下更新方程：
$${\hat{x}}_k^{\oplus }={\hat{x}}_k^{㊀}+P_k^{㊀}{H}_k^{\top }(H_k{P}_k^{㊀}{H}_k^{\top }+R_k{)}^{-1}(z_{m,k}-{\hat{z}}_k)$$$$={\hat{x}}_k^{㊀}+K{r}_z$$
$$P_{xx}^{\oplus }=P_k^{㊀}+P_k^{㊀}{H}_k^{\top }(H_k{P}_k^{㊀}{H}_k^{\top }+R_k{)}^{-1}(P_K^{㊀}{H}_k^{\top }{)}^{\top }$$$$=P_k^{㊀}+P_k^{㊀}{H}_k^{\top }(H_k{P}_k^{㊀}{H}_k^{\top }+R_k{)}^{-1}(H_k{P}_K^{㊀})$$

这些本质上是卡尔曼滤波器（或线性MMSE）更新方程。

5. Feature Triangulation

5.1 3D 笛卡尔三角测量

  我们希望创建一个可求解的线性系统，该系统可以让我们初步估计特征的 3D 笛卡尔位置。为此，我们将看到特征的所有位姿都视为已知量。这些特征可以在我们任意选择的相机坐标系 { A } \{A\} { A} 下进行三角化。假设特征点 $p_f$ 可以在给定的坐标系 { A } \{A\} { A} 下的位姿 $\mathrm{1...}m$ 下被观测到，我们可以从一些相机位姿 $C_i,i=\mathrm{1...}m$ 下得到下列转换。

$${}^{C_i}{p}_f{=}_A^{C_i}R{(}^A{p}_f{-}^A{p}_{C_i})$$$${}^A{p}_f{=}_A^{C_i}{R}^{\top }{}^{C_i}{p}_f{+}^A{p}_{C_i}$$
  在没有噪声的情况下，当前坐标系中的测量值是 方位 ${}^{C_i}b$ 及其深度 ${}^{C_i}z$ .因此，我们有以下从当前帧看到的特征的映射：
$${}^{C_i}{p}_f{=}^{C_i}{z}_f^{C_i}{b}_f{=}^{C_i}{z}_f\left[\begin{array}{c}{u}_n\\ v_n\\ 1\end{array}\right]$$

  我们注意到 $u_n$ 和 $v_n$表示未畸变的归一化图像坐标。该方位可以通过代入上述公式来转换到固定的坐标系中：
$${}^A{p}_f{=}_A^{C_i}{R}^{\top }{}^{C_i}{p}_f{+}^A{p}_{C_i}$$$${=}^{C_i}{z}_f{}^A{b}_{C_i\to f}{+}^A{p}_{C_i}$$

  为了消除估计深度 ${}^{C_i}{z}_f$ 额外自由度的需要，我们定义了以下与方位正交的向量 ${}^A{b}_{C_i\to f}$：
$${}^A{N}_i=\lfloor {}^A{b}_{C_i\to f}\times \rfloor =\left[\begin{array}{ccc}0& {-}^A{b}_{C_i\to f}(3)& {}^A{b}_{C_i\to f}(2)\\ {}^A{b}_{C_i\to f}(3)& 0& {-}^A{b}_{C_i\to f}(1)\\ {-}^A{b}_{C_i\to f}(2)& {}^A{b}_{C_i\to f}(1)& 0\end{array}\right]$$

  三行都垂直于向量 ${}^A{b}_{C_i\to f}$，因此 ${}^A{N}_i^A{b}_{C_i\to f}=0_3$。然后，我们可以将变换方程/约束相乘，形成两个仅与未知方程相关的3自由度方程 ${}^A{p}_f$。
$${}^A{N}_i^A{p}_f{=}^A{N}_i^{C_i}{z_f}^A{b}_{C_i\to f}{+}^A{N}_i^A{p}_{C_i}$$$${=}^A{N}_i^A{p}_{C_i}$$
通过叠加所有测量值，我们可以：
$$\underset{A}{\underbrace{\left[\begin{array}{c}⋮\\ {}^A{N}_i\\ ⋮\end{array}\right]}}{p}_f=\underset{b}{\underbrace{\left[\begin{array}{c}⋮\\ {}^A{N}_i^A{p}_{C_i}\\ ⋮\end{array}\right]}}$$

  由于每个像素测量都提供了两个约束，只要 $m>1$ ，我们将有足够的约束来对特征进行三角测量。在实践中，特征点的视图越多，三角测量效果越好，因此通常希望从至少五个视图中看到一个特征点。我们可以选择每 ${}^A{N}_i$ 的两行来减少行数，但是通过方形系统，我们可以执行以下“技巧”。
$$A^{\top }{A}^A{p}_f=A^{\top }b$$$$(\sum _i{}^A{N}_i^{\top }{}^A{N}_i{)}^A{p}_f=(\sum _i{}^A{N}_i^{\top }{}^A{N}_i{}^A{p}_{C_i})$$

  这是一个 3x3 系统，与原始的 3mx3m 或 2mx2m 系统相比，可以快速解决。我们还检查三角测量特征是否“有效”，并且在相机前方且不太远。上述线性系统和否定系统的 condition number 对错误和极大值数据“敏感”。

5.2 1D 深度三角测量

  我们希望创建一个可求解的线性系统，该系统可以让我们初步估计特征的 1D 深度位置。为此，我们将看到特征的所有姿势与锚架中的方向一起视为已知量。此功能将在我们可以任意选择的某个锚定摄像机帧 { A } \{A\} { A} 中进行三角测量。我们将其定义为锚帧中的归一化图像坐标 ${\left[\begin{array}{ccc}{u}_n& v_n& 1\end{array}\right]}^{\top }$。给定一个锚点姿势 $A$，如果一个特征 $p_f$ 是可以被位姿 $\mathrm{1...}m$ 所观测，那么我们可以从任意相机位置 $C_i,i=\mathrm{1...}m$ 得到如下转换：
$${}^{C_i}{p}_f{=}_A^{C_i}R{(}^A{p}_f{-}^A{p}_{C_i})$$$${}^{A_i}{p}_f{=}_A^{C_i}{R}^{\top }{}^{C_i}{p}_f{+}^A{p}_{C_i}$$$${}^A{z}_f^A{p}_f{=}_A^{C_i}{R}^{\top }{}^{C_i}{p}_f{+}^A{p}_{C_i}$$

  在没有噪声的情况下，当前坐标系中的测量值是方向 ${}^{C_i}b$ 及其深度 ${}^{C_i}z$
$${}^{C_i}{p}_f{=}^{C_i}{z}_f^{C_i}{b}_f{=}^{C_i}{z}_f\left[\begin{array}{c}{u}_n\\ v_n\\ 1\end{array}\right]$$
  我们注意到 $u_n$ 和 $v_n$表示未畸变的归一化图像坐标。该方位可以通过代入上述公式来转换到固定的坐标系中：
$${}^A{z_f}^A{b}_f{=}_A^{C_i}{R}^{\top }{}^{C_i}{z}_f{}^{C_i}{b}_f{+}^A{p}_{C_i}$$$${=}^{C_i}{z}_f{}^A{b}_{C_i\to f}{+}^A{p}_{C_i}$$

  为了消除估计深度 ${}^{C_i}{z}_f$ 额外自由度的需要，我们定义了以下与方位正交的向量 ${}^A{b}_{C_i\to f}$：
$${}^A{N}_i=\lfloor {}^A{b}_{C_i\to f}\times \rfloor$$
  三行都垂直于向量 ${}^A{b}_{C_i\to f}$，因此 ${}^A{N}_i^A{b}_{C_i\to f}=0_3$。然后，我们可以将变换方程/约束相乘，形成两个仅与未知方程相关的3自由度方程 ${}^A{p}_f$。
$${(}^A{N}_i^A{p}_f{)}^A{z}_f{=}^A{N}_i^{C_i}{z_f}^A{b}_{C_i\to f}{+}^A{N}_i^A{p}_{C_i}$$$${=}^A{N}_i^A{p}_{C_i}$$
然后我们可以制定以下系统：
$$(\sum _i{{(}^A{N}_i^A{b}_f{)}^{\top }({}^A{N}_i}^A{b}_f){)}^A{z}_f=(\sum _i{(}^A{N}^A{b}_f{)}_i^{\top }{}^A{N}_i{}^A{b}_i)$$

  这是一个 1x1 系统，可以用单个标量除法快速求解。我们还检查三角测量特征是否“有效”，并且在相机前方且不太远。完整的特征可以通过以下方式重建：
$${}^A{p}_f{=}^A{z}_f^A{b}_f$$

5.3 3D Inverse Non-linear Optimization
  获得三角化特征 3D 位置后，将执行非线性最小二乘法来完善此估计值。为了获得良好的数值稳定性，我们对点特征使用逆深度表示，这有助于收敛。我们发现，在大多数情况下，这个问题在室内环境中的 2-3 次迭代内收敛。特征转换可以写成：
$${}^{C_i}{p}_f{=}_A^{C_i}R{(}^A{p}_f{-}^A{p}_{C_i})$$$${=}^A{z}_f{}_A^{C_i}R\left(\begin{array}{c}\left[\begin{array}{c}{}^A{x}_f{/}^A{z}_f\\ {}^A{y}_f{/}^A{z}_f\\ 1\end{array}\right]-{\frac{1}{{}^A{z}_f}}^A{p}_{C_i}\end{array}\right)$$
   我们定义 $u_A{=}^A{x}_f{/}^A{z}_f$，$v_A{=}^A{y}_f{/}^A{z}_f$，${\rho }_A=1{/}^A{z}_f$ 得到以下测量公式：
$$h(u_A,v_A,{\rho }_A){=}_A^{C_i}R\left(\begin{array}{c}\left[\begin{array}{c}{u}_A\\ v_A\\ 1\end{array}\right]-{\rho }_A{}^A{p}_{C_i}\end{array}\right)$$

   从相机坐标系看到的特征测量可以重新表述为：
$$z=\left[\begin{array}{c}{u}_i\\ v_i\end{array}\right]$$$$=\left[\begin{array}{c}h(u_A,v_A,{\rho }_A)(1)/h(u_A,v_A,{\rho }_A)(3)\\ h(u_A,v_A,{\rho }_A)(2)/h(u_A,v_A,{\rho }_A)(3)\end{array}\right]$$$$=h(u_A,v_A,{\rho }_A)$$

因此，我们可以制定最小二乘法和雅可比:
$$\underset{u_A,v_A,{\rho }_A}{\mathrm{argmin}}||z-h(u_A,v_A,{\rho }_A)|{|}^2$$$$\frac{\partial h(u_A,v_A,{\rho }_A)}{h(u_A,v_A,{\rho }_A)}{=}_A^{C_i}R\left[\begin{array}{c}\left[\begin{array}{cc}1& 0\\ 0& 1\\ 0& 0\end{array}\right]-{}^A{p}_{C_i}\end{array}\right]$$

   最小二乘问题可以用高斯-牛顿或莱文伯格-马夸特算法来解决。

6. Camera Measurement Update

6.1 透视投影（方位）测量模型

   考虑在某个时间 $k$ 从相机图像中检测到 3D 特征，其在图像平面上的测量值 $uv$（即相应的像素坐标）由下式给出：
$$\begin{gathered} z_{m,k}=h(x_k)+n_k \\ =h_d(z_{n,k},\zeta )+n_k \\ =h_d(h_p{(}^{C_k}{p}_f),\zeta )+n_k \\ =h_d(h_p(h_t{(}^G{p}_f,{}_G^{G_k}R{,}^G{p}_{C_k})),\zeta )+n_k \\ =h_d(h_p(h_t((h_r(\lambda ,\cdots )),{}_G^{G_k}R{,}^G{p}_{C_k})),\zeta )+n_k \end{gathered}$$

   其中 $n_k$ 是测量噪声，通常假定为零均值白高斯; $z_{n,k}$ 是归一化未畸变的 $uv$观测值; $\zeta$是相机的内在参数，如焦距和畸变参数;${}^{C_k}{p}_f$是当前相机帧 { C k } \{C_k\} { Ck​}中的特征位置; ${}^G{p}_f$是全局坐标系 { G } \{G\} { G}中的特征位置; { G G k R , G p C k } \{^{G_k}_GR,^Gp_{C_k}\} { GGk​​R,GpCk​​}表示全局坐标系中的当前相机姿势（位置和方向）（或相机外参）;$\lambda$是不同表示（位置除外）的特征参数，例如简单的 xyz 位置或带方位的逆深度。

   在上面的表达式中，我们将测量函数分解为对应于不同操作的多个级联函数，这些函数将状态映射到图像平面上的原始 $uv$ 测量中。应该注意的是，由于我们将执行具有不同特征表示的内参标定以及外参标定，因此上述相机测量模型是通用的。下一节将给出每个函数的高级说明。

测量函数概述

6.2 雅可比计算

   给定上述嵌套函数，我们可以利用链式规则来找到总状态雅可比。由于我们的特征表示函数 $h_r(\cdots )$ 也可能取决于状态，即相机姿态，我们需要仔细考虑其附加导数。考虑以下关于雅可比状态 $x$ 的测量示例：

$$\frac{\partial z_k}{\partial x}=\frac{\partial h_d(\cdot )}{\partial z_{n,k}}\frac{\partial h_p(\cdot )}{{\partial }^{C_k}{p}_f}\frac{\partial h_t(\cdot )}{\partial x}+\frac{\partial h_d(\cdot )}{\partial z_{n,k}}\frac{\partial h_p(\cdot )}{{\partial }^{C_k}{p}_f}\frac{\partial h_t(\cdot )}{{\partial }^G{P}_f}\frac{\partial h_r(\cdot )}{\partial x}$$

   在全局特征表达（请参阅点特征表达部分）中，第二项将为零，而对于相机表达，则需要计算该项。

6.3 畸变函数

径向畸变
   为了校准相机内参，我们需要知道如何将归一化坐标映射到图像平面上的原始像素坐标。我们首先采用 OpenCV 模型中的径向畸变：

$$\left[\begin{array}{c}u\\ v\end{array}\right]:=z_k=h_d(z_{n,k},\zeta )=\left[\begin{array}{c}{f}_x\ast x+c_x\\ f_y\ast y+c_y\end{array}\right]$$ $$\begin{gathered} x=x_n(1+k_1{r}^2+k_2{r}^4)+2p_1{x}_n{y}_n+p_2(r^2+=2x_n^2) \\ y=y_n(1+k_1{r}^2+k_2{r}^4)+2p_2{x}_n{y}_n+p_1(r^2+=2y_n^2) \\ {r}^2=x_n^2+y_n^2 \end{gathered}$$

   其中 $z_{n,k}={\left[\begin{array}{cc}{x}_n& y_n\end{array}\right]}^{\top }$ 是 3D 特征的归一化坐标，$u$ 和 $v$ 是图像平面上的畸变图像坐标。上述畸变模型涉及以下畸变和相机内（焦距和图像中心）参数，可以在线估算：
$$\zeta ={\left[\begin{array}{cccccccc}{f}_x& f_y& c_x& c_y& k_1& k_2& p_1& p_2\end{array}\right]}^{\top }$$

   请注意，我们不会像大多数离线校准方法（如 Kalibr）那样估计高阶（即高于四阶）项。要估计这些内参（包括扭曲参数），需要以下这些参数的雅可比矩阵：
$$\frac{\partial h_d(\cdot )}{\partial \zeta }=\left[\begin{array}{cccccccc}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]$$

类似地，关于归一化坐标的雅可比坐标可以按如下方式获得：
$$\frac{\partial h_d(\cdot )}{\partial z_{n,k}}$$

鱼眼模型
   由于鱼眼镜头或广角镜头在实践中被广泛使用，我们在这里提供了OpenCV鱼眼镜头中这种畸变模型的数学推导。
$$\left[\begin{array}{c}u\\ v\end{array}\right]:=z_k=h_d(z_{n,k},\zeta )=\left[\begin{array}{c}{f}_x\ast x+c_x\\ f_y\ast y+c_y\end{array}\right]$$$$\begin{gathered} x=\frac{x_n}{r}\ast {\theta }_d \\ y=\frac{y_n}{r}\ast {\theta }_d \\ {\theta }_d=\theta (1+k_1{\theta }^2+k_2{\theta }^4+k_3{\theta }^6+k_4{\theta }^8) \\ {r}^2=x_n^2+y_n^2 \\ \theta =atan(r) \end{gathered}$$
   其中 $z_{n,k}={\left[\begin{array}{cc}{x}_n& y_n\end{array}\right]}^{\top }$ 是 3D 特征的归一化坐标，$u$ 和 $v$ 是图像平面上的畸变图像坐标。显然，上述模型中使用了以下畸变内参：
$$\zeta ={\left[\begin{array}{cccccccc}{f}_x& f_y& c_x& c_y& k_1& k_2& k_3& k_4\end{array}\right]}^{\top }$$

与前面的径向畸变情况类似，校准需要以下这些参数的雅可比矩阵：
$$\frac{\partial h_d(\cdot )}{\partial \zeta }=\left[\begin{array}{cccccccc}{x}_n& 0& 1& 0& f_x\ast (\frac{x_n}{r}{\theta }^3)& f_x\ast (\frac{x_n}{r}{\theta }^5))& f_x\ast (\frac{x_n}{r}{\theta }^7))& f_x\ast (\frac{x_n}{r}{\theta }^9))\\ 0& y_n& 0& 1& f_y\ast (\frac{y_n}{r}{\theta }^3))& f_y\ast (\frac{y_n}{r}{\theta }^5))& f_y\ast (\frac{y_n}{r}{\theta }^7))& f_y\ast (\frac{y_n}{r}{\theta }^9))\end{array}\right]$$

类似地，使用微分链式规则，我们可以计算以下关于归一化坐标的雅可比坐标：
$$\frac{\partial h_d(\cdot )}{\partial z_{n,k}}=\frac{\partial uv}{\partial xy}\frac{\partial xy}{\partial x_n{y}_n}+\frac{\partial uv}{\partial xy}\frac{\partial xy}{\partial r}\frac{\partial r}{\partial x_n{y}_n}+\frac{\partial uv}{\partial xy}\frac{\partial xy}{\partial {\theta }_d}\frac{\partial {\theta }_d}{\partial \theta }\frac{\partial \theta }{\partial r}\frac{\partial r}{\partial x_n{y}_n}$$

6.4 Perspective Projection Function

   标准针孔相机模型用于将相机帧中的 3D 点投影到归一化图像平面（具有单位深度）：
$$z_{n,k}=h_p{(}^{C_k}{p}_f)=\left[\begin{array}{c}{}^{C}x/{}^{C}z\\ {}^{C}y/{}^{C}z\end{array}\right]$$$${}^{C_k}{p}_f=\left[\begin{array}{c}{}^{C}x\\ {}^{C}y\\ {}^{C}z\end{array}\right]$$
其雅可比矩阵计算如下：
$$\frac{\partial h_p(\cdot )}{{\partial }^{C_k}{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]$$

6.5 欧几里得变换

   我们采用6DOF刚体欧几里得变换，根据当前全局相机姿势将全局坐标系 { G } \{G\} { G} 中的3D特征位置转换为当前相机坐标系 { C k } \{C_k\} { Ck​} ：
$${}^{C_k}{p}_f=h_t{(}^G{p}_f,{}_G^{C_k}R,{}^G{p}_{C_k})={}_G^{G_k}R({}^G{p}_f-{}^G{p}_{C_k})$$

请注意，在视觉惯性导航系统中，我们通常将IMU而不是相机状态保留在状态向量中。因此，我们需要使用 不变的IMU相机外参 { I C R , C p I } \{ {^C_IR}, {^Cp_I}\} { IC​R,CpI​} 进一步变换上述几何结构，如下所示：
$${}^G{p}_{C_k}={}^G{p}_{I_k}+{}_I^G{R}^I{p}_{C_k}={}^G{p}_{I_k}+{}_I^G{R}^I{p}_C$$$${}_G^{C_k}R={}_I^{C_k}R{}_G^{I_k}R={}_I^{C}R{}_G^{I_k}R$$