VIO子系统将渲染整个地图的纹理，并通过最小化光度误差来估计系统的状态。更具体地说，我们将一定数量的点（即跟踪点）投影到当前图像中。然后，通过最小化这些点的光度误差ESIKF系统状态在框架内迭代估计。

假设我们在上一帧的图像帧 ${\mathbf{I}}_{k?1}$ 中追踪了 $m$ 这个地图点 $m$ 个地图点记为 P = { P 1 , … , P m } \mathcal{P}=\left\{\mathbf{P}_{1}, \ldots, \mathbf{P}_{m}\right\} P={ P1,…,Pm}，它们在图像帧中 I k ? 1 \mathbf{I}_{k-1} Ik−1​ 中的投影坐标记为 { ρ 1 k − 1 , … , ρ m k − 1 } \left\{\boldsymbol{\rho}_{1_{k-1}}, \ldots, \boldsymbol{\rho}_{m_{k-1}}\right\} { ρ1k−1​​,…,ρmk−1​​}，使用 Lucas-Kanade光流法 可以找出这些地图点在当前图像帧 I k \mathbf{I}_{k} Ik​ 的投影坐标，记为 { ρ 1 k , … , ρ m k } \left\{\boldsymbol{\rho}_{1_{k}}, \ldots, \boldsymbol{\rho}_{m_{k}}\right\} { ρ1k​​,…,ρmk​​}。然后我们通过 ESIKF 来计算和优化这些地图点的重投影误差，从而估计出系统的状态。

如上图所示，以第 $s$ 个点 ${\mathbf{P}}_s={\left[{}^G{\mathbf{p}}_s^T,{\mathbf{c}}_s^T\right]}^T\in \mathcal{P}$ 为例，其中前面的3维向量 ${}^G{\mathbf{p}}_s^T=\left[{}^G{{\mathbf{p}}_{\mathbf{s}}}_x,{}^G{{\mathbf{p}}_{\mathbf{s}}}_y,{}^G{{\mathbf{p}}_{\mathbf{s}}}_z\right]$ 表示点 $s$ 在世界坐标系下的三维坐标，后面的 ${\mathbf{c}}_s^T={\left[{{\mathbf{c}}_{\mathbf{s}}}_r,{{\mathbf{c}}_{\mathbf{s}}}_g,{{\mathbf{c}}_{\mathbf{s}}}_b\right]}^T$表示点 $s$ 的RGB颜色。

为了计算点 $s$ 的在图像帧 ${\mathbf{I}}_k$ 中的重投影误差，我们先要将点 $s$ 转换到相机坐标系当中，即：

$$\begin{array}{r}{}^C{\mathbf{p}}_s={\left[{}^C{\mathbf{p}}_{x_s},{}^C{\mathbf{p}}_{y_s},{}^C{\mathbf{p}}_{z_s}\right]}^T={\left({}^G{\check{\mathbf{R}}}_{I_k}\cdot {}^I{\check{\mathbf{R}}}_{C_k}\right)}^T\cdot {}^G{\mathbf{p}}_s-{}^I{\check{\mathbf{R}}}_{C_k}^T\cdot {}^I{\check{\mathbf{p}}}_{C_k}-{\left({}^G{\check{\mathbf{R}}}_{I_k}\cdot {}^I{\check{\mathbf{R}}}_{C_k}\right)}^T\cdot {}^G{\check{\mathbf{p}}}_{I_k}\end{array}\text{\hspace{1em}(1)}$$

这里要注意的是， C k p G ≠ − G p C k {}^{C_k}p_G \neq -{}^Gp_{C_k} Ck​pG​=−