R3LIVE论文学习（二）：VIO子系统

VIO子系统通过最小化帧到帧的PnP重投影误差和最小化帧到全局地图的光度误差来估计系统的状态，并渲染全局地图的纹理。 具体过程如下：

1. 在帧与帧之间，通过Lucas-Kanade光流法跟踪一定数量的地图点，并通过最小化这些点的PnP重投影误差来估计系统状态。系统估计通过ESIKF框架进行迭代求解，与高斯牛顿法（GN）等价。
2. 在帧与全局地图之间，将全局地图中被跟踪的点投影到当前图像帧中，然后最小化这些点的帧与地图之间的光度误差，从而更精确的估计系统状态。同样的，系统估计通过ESIKF框架进行迭代求解。
3. 在帧到全局地图的VIO更新之后，可以得到当前图像帧的精确位姿，然后通过对地图点在当前图像帧上对应的像素和相邻像素的RGB颜色进行线性插值，从而渲染地图点的颜色。

1 帧到帧的VIO

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

1.1 PnP重投影误差

如上图所示，以第 $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\ne -{}^G{p}_{C_k}$，而是 ${}^{C_k}{p}_G=-{}^{C_k}{R}_G{}^G{p}_{C_k}$。

将点 $s$ 的重投影误差记为 $\mathbf{r}\left({\check{\mathbf{x}}}_k,{\mathbf{\rho }}_s^k,{}^G{\mathbf{p}}_s\right)$ ，则有：

$$\mathbf{r}\left({\check{\mathbf{x}}}_k,{\mathbf{\rho }}_{s_k},{}^G{\mathbf{p}}_s\right)={\mathbf{\rho }}_{s_k}-\mathbf{\pi }\left({}^C{\mathbf{p}}_s,{\check{\mathbf{x}}}_k\right)\text{\hspace{1em}(2)}$$

其中，${\check{\mathbf{x}}}_k$ 是每一次ESIKF迭代的当前状态，$\mathbf{\pi }\left({}^C{\mathbf{p}}_s,{\check{\mathbf{x}}}_k\right)\in {\mathbb{R}}^2$ 是相机投影模型，可以通过下式计算：

$$\begin{array}{rl}\mathbf{\pi }\left({}^C{\mathbf{p}}_s,{\check{\mathbf{x}}}_k\right)& ={\left[{\check{f}}_{x_k}\frac{{}^C{\mathbf{p}}_{x_s}}{C_{{\mathbf{p}}_{z_s}}}+{\check{c}}_{x_k},{\check{f}}_{y_k}\frac{{}^C{\mathbf{p}}_{y_s}}{{}^C{\mathbf{p}}_{z_s}}+{\check{c}}_{y_k}\right]}^T+\frac{{\check{t}}_{C_k}}{\Delta t_{k-1,k}}\left({\mathbf{\rho }}_{s_k}-{\mathbf{\rho }}_{s_{k-1}}\right)\end{array}\text{\hspace{1em}(3)}$$

其中， $\Delta t_{k-1,k}$ 是上一帧图像帧 ${\mathbf{I}}_{k-1}$ 和当前帧图像帧 ${\mathbf{I}}_k$ 之间的时间间隔。上式等式右边第一个式子是针孔相机投影模型，第二个式子是在线时间校正因子。

误差公式 (2) 中的测量噪声包含两个部分：一个是 ${}^G{\mathbf{p}}_s$ 中的地图点位置误差，另一个是 ${\mathbf{\rho }}_{s_k}$ 中的像素跟踪误差。

$${}^G{\mathbf{p}}_s={}^G{\mathbf{p}}_s^{\mathrm{gt}}+{\mathbf{n}}_{{\mathbf{p}}_s},{\mathbf{n}}_{{\mathbf{p}}_s}\sim \mathcal{N}\left(0,{\mathbf{\Sigma }}_{{\mathbf{n}}_{{\mathbf{p}}_s}}\right)\text{\hspace{1em}(4)}$$

$${\mathbf{\rho }}_{s_k}={\mathbf{\rho }}_{s_k}^{\mathrm{gt}}+{\mathbf{n}}_{{\mathbf{\rho }}_{s_k}},{\mathbf{n}}_{{\rho }_{s_k}}\sim \mathcal{N}\left(0,{\mathbf{\Sigma }}_{{\mathbf{n}}_{{\rho }_{s_k}}}\right)\text{\hspace{1em}(5)}$$

其中，${}^G{\mathbf{p}}_s^{\mathrm{gt}}$ 和 ${\rho }_{s_k}^{g\mathrm{t}}$ 分别为为 ${}^G{\mathbf{p}}_s$ 和 ${\mathbf{\rho }}_{s_k}$ 的真值。

然后，我们得到零残差真值 $\mathbf{r}\left({\mathbf{x}}_k,{\mathbf{\rho }}_{s_k}^{\mathrm{gt}},{}^G{\mathbf{p}}_s^{\mathrm{gt}}\right)$ 的一阶泰勒展开为：

$$0=\mathbf{r}\left({\mathbf{x}}_k,{\mathbf{\rho }}_{s_k}^{\mathrm{gt}},{}^G{\mathbf{p}}_s^{\mathrm{gt}}\right)\approx \mathbf{r}\left({\check{\mathbf{x}}}_k,{\mathbf{\rho }}_{s_k},{}^G{\mathbf{p}}_s\right)+{\mathbf{H}}_s^r\delta {\check{\mathbf{x}}}_k+{\mathbf{\alpha }}_s\text{\hspace{1em}(6)}$$

其中：${\mathbf{\alpha }}_s\sim \mathcal{N}\left(0,{\mathbf{\Sigma }}_{{\mathbf{\alpha }}_s}\right)$，并且有：

$${\mathbf{H}}_s^r={\frac{\partial {\mathbf{r}}_c\left({\check{\mathbf{x}}}_k⊞\delta {\check{\mathbf{x}}}_k,{\mathbf{\rho }}_{s_k},{}^G{\mathbf{p}}_s\right)}{\partial \delta {\check{\mathbf{x}}}_k}|}_{\delta {\check{\mathbf{x}}}_k=0}\text{\hspace{1em}(7)}$$

$${\mathbf{\Sigma }}_{{\mathbf{\alpha }}_s}={\mathbf{\Sigma }}_{{\mathbf{n}}_{{\rho }_{s_k}}}+{\mathbf{F}}_{{\mathbf{p}}_s}^r{\mathbf{\Sigma }}_{{\mathbf{p}}_s}{\mathbf{F}}_{{\mathbf{p}}_s}^{rT}\text{\hspace{1em}(8)}$$

$${\mathbf{F}}_{{\mathbf{p}}_s}^r=\frac{\partial \mathbf{r}\left({\check{\mathbf{x}}}_k,{\mathbf{\rho }}_{s_k},{}^G{\mathbf{p}}_s\right)}{\partial {\mathbf{p}}_s}\text{\hspace{1em}(9)}$$

1.2 帧到帧的VIO ESIKF更新

公式（6）表示了 ${\mathbf{x}}_k$ 的观测分布，它可以可以与IMU传播的先验分布相结合，从而得到误差状态 $\delta {\check{\mathbf{x}}}_k$ 的最大后验估计（MAP）：

$$\begin{array}{rl}& \underset{\delta {\check{\mathbf{x}}}_k}{\min }\left({\Vert {\check{\mathbf{x}}}_k\boxminus {\hat{\mathbf{x}}}_k+\mathcal{H}\delta {\check{\mathbf{x}}}_k\Vert }_{{\mathbf{\Sigma }}_{\delta {\hat{\mathbf{x}}}_k}}^2+\sum _{s=1}^m{\Vert \mathbf{r}\left({\check{\mathbf{x}}}_k,{\mathbf{\rho }}_{s_k},{}^G{\mathbf{p}}_s\right)+{\mathbf{H}}_s^r\delta {\check{\mathbf{x}}}_k\Vert }_{{\mathbf{\Sigma }}_{{\mathbf{\alpha }}_s}}^2\right)\end{array}\text{\hspace{1em}(10)}$$

其中 $\Vert \mathbf{x}{\Vert }_{\mathbf{\Sigma }}^2={\mathbf{x}}^T{\mathbf{\Sigma }}^{-1}\mathbf{x}$ 表示马氏距离的平方，${\hat{\mathbf{x}}}_k$ 是IMU传播的状态估计，${\Sigma }_{\delta {\hat{\mathbf{x}}}_k}$ 是IMU传播状态的协方差，$\mathcal{H}$ 是从 ${\hat{\mathbf{x}}}_k$ 的切平面投影到 ${\check{\mathbf{x}}}_k$ 的切平面时的状态误差的雅可比矩阵。

其中：

$$\mathbf{H}={\left[{\mathbf{H}}_1^{rT},\dots ,{\mathbf{H}}_m^{rT}\right]}^T\text{\hspace{1em}(11)}$$

$$\mathbf{R}=\mathrm{diag}\left({\mathbf{\Sigma }}_{{\mathbf{\alpha }}_1},\dots ,{\mathbf{\Sigma }}_{{\mathbf{\alpha }}_m}\right)\text{\hspace{1em}(12)}$$

$${\check{\mathbf{z}}}_k={\left[\mathbf{r}\left({\check{\mathbf{x}}}_k,{\mathbf{\rho }}_{1_k},{}^G{\mathbf{p}}_1\right)\dots ,\mathbf{r}\left({\check{\mathbf{x}}}_k,{\mathbf{\rho }}_{m_k},{}^G{\mathbf{p}}_m\right)\right]}^T\text{\hspace{1em}(13)}$$

$$\mathbf{P}=(\mathbf{H}{)}^{-1}{\mathbf{\Sigma }}_{\delta {\hat{\mathbf{x}}}_k}(\mathbf{H}{)}^{-T}\text{\hspace{1em}(14)}$$

Kalman增益可以通过下式计算：

$$\mathbf{K}={\left({\mathbf{H}}^T{\mathbf{R}}^{-1}\mathbf{H}+{\mathbf{P}}^{-1}\right)}^{-1}{\mathbf{H}}^T{\mathbf{R}}^{-1}\text{\hspace{1em}(15)}$$

从而，我们可以通过下式进行误差状态更新：

$${\check{\mathbf{x}}}_k={\check{\mathbf{x}}}_k⊞\left(-\mathbf{K}{\check{\mathbf{z}}}_k-(\mathbf{I}-\mathbf{KH})(\mathcal{H}{)}^{-1}\left({\check{\mathbf{x}}}_k\boxminus {\hat{\mathbf{x}}}_k\right)\right)\text{\hspace{1em}(16)}$$

迭代上式直到收敛（比如状态更新值小于给定的阈值）。注意，该迭代卡尔曼滤波方法与GN高斯牛顿优化方法等价。

2 帧到地图的VIO

2.1 帧到地图光度更新

在帧到帧的VIO更新之后，我们对系统状态 ${\check{\mathbf{x}}}_k$ 有了良好的估计，然后我们通过最小化跟踪点的光度误差来进行帧到地图的VIO更新，从而降低漂移。

如上图所示，以第 $s$ 个点 ${\mathbf{P}}_s={\left[{}^G{\mathbf{p}}_s^T,{\mathbf{c}}_s^T\right]}^T\in \mathcal{P}$ 为例，其帧到地图的光度误差 $\mathbf{o}\left({\check{\mathbf{x}}}_k,{}^G{\mathbf{p}}_s,{\mathbf{C}}_s\right)$ 可以通过下式计算：

$$\mathbf{o}\left({\check{\mathbf{x}}}_k,{}^G{\mathbf{p}}_s,{\mathbf{c}}_s\right)={\mathbf{c}}_s-{\gamma }_s\text{\hspace{1em}(17)}$$

其中，${\mathbf{c}}_s$ 是保存在全局地图中的颜色，${\gamma }_s$ 是当前图像帧 ${\mathbf{I}}_k$ 中观察到的颜色。为了得到观测到的颜色 ${\gamma }_s$ 及其协方差矩阵 ${\Sigma }_{{\mathbf{n}}_{{\gamma }_s}}$，我们预测点 $s$ 在当前图像帧 ${\mathbf{I}}_k$ 上的坐标为 ${\tilde{\rho }}_{s_k}=\pi \left({}^C{\mathbf{p}}_s,{\check{\mathbf{x}}}_k\right)$，然后线性插值计算得到相邻像素的RGB颜色。

我们同样考虑 ${\gamma }_s$ 和 ${\mathbf{c}}_s$ 的测量误差：

$${\gamma }_s={\mathbf{\gamma }}_s^{gt}+{\mathbf{n}}_{{\mathbf{\gamma }}_s},{\mathbf{n}}_{{\mathbf{\gamma }}_s}\sim \mathcal{N}\left(0,{\mathbf{\Sigma }}_{{\mathbf{n}}_{{\gamma }_s}}\right)\text{\hspace{1em}(18)}$$

$$\begin{array}{c}{\mathbf{c}}_s={\mathbf{c}}_s^{gt}+{\mathbf{n}}_{{\mathbf{c}}_s}+{\mathbf{\eta }}_{{\mathbf{c}}_s},\\ {\mathbf{n}}_{{\mathbf{c}}_s}\sim \mathcal{N}\left(0,{\mathbf{\Sigma }}_{{\mathbf{n}}_{{\mathbf{c}}_s}}\right)\\ {\mathbf{\eta }}_{{\mathbf{c}}_s}\sim \mathcal{N}\left(0,{\mathbf{\sigma }}_s^2\cdot \Delta t_{{\mathbf{c}}_s}\right)\end{array}\text{\hspace{1em}(19)}$$

其中，${\gamma }_s^{gt}$ 是 ${\gamma }_s$ 的真值，${\mathbf{c}}_s^{gt}$ 是 ${\mathbf{c}}_s$ 的真值，$\Delta t_{{\mathbf{c}}_s}$ 是当前帧和 ${\mathbf{P}}_s$ 上次渲染时的时间间隔。

在公式（19）中， ${\mathbf{c}}_s$ 的测量噪声由两部分组成：上次渲染的估计误差 ${\mathbf{n}}_{{\mathbf{c}}_s}$ 和脉冲随机游走过程噪声 ${\mathbf{\eta }}_{{\mathbf{c}}_s}$（它解释了环境光照的变化）。

结合公式(17)(18)(19)，我们得到残差零真值 $\mathbf{o}\left({\mathbf{x}}_k,{}^G{\mathbf{p}}_s^{gt},{\mathbf{c}}_s^{gt}\right)$ 的一阶泰勒展开：

$$0=\mathbf{o}\left({\mathbf{x}}_k,{}^G{\mathbf{p}}_s^{gt},{\mathbf{c}}_s^{gt}\right)\approx \mathbf{o}\left({\check{\mathbf{x}}}_k,{}^G{\mathbf{p}}_s,{\mathbf{c}}_s\right)+{\mathbf{H}}_s^o\delta {\check{\mathbf{x}}}_k+{\mathbf{\beta }}_s\text{\hspace{1em}(20)}$$

其中：

$${\mathbf{H}}_s^o={\frac{\partial \mathbf{o}\left({\check{\mathbf{x}}}_k⊞\delta {\check{\mathbf{x}}}_k,{}^G{\mathbf{p}}_s,{\mathbf{c}}_s\right)}{\partial \delta {\check{\mathbf{x}}}_k}|}_{\delta {\check{\mathbf{x}}}_k=0}\text{\hspace{1em}(21)}$$

$${\mathbf{\Sigma }}_{{\mathbf{\beta }}_s}={\mathbf{\Sigma }}_{{\mathbf{n}}_{{\mathbf{c}}_s}}+{\mathbf{\sigma }}_s^2\cdot \Delta t_{{\mathbf{c}}_s}+{\mathbf{\Sigma }}_{{\mathbf{n}}_{{\gamma }_s}}+{\mathbf{F}}_{{\mathbf{p}}_s}^o{\mathbf{\Sigma }}_{{\mathbf{p}}_s}{\mathbf{F}}_{{\mathbf{p}}_s}^{oT}\text{\hspace{1em}(22)}$$

$${\mathbf{F}}_{{\mathbf{p}}_s}^o=\frac{\partial \mathbf{o}\left({\check{\mathbf{x}}}_k,{}^G{\mathbf{p}}_s,{\mathbf{c}}_s\right)}{\partial {}^G{\mathbf{p}}_s}\text{\hspace{1em}(23)}$$

2.2 帧到地图的VIO ESIKF更新

公式（20）构成了 $\delta {\check{\mathbf{x}}}_k$ 的另一个观测分布，它与IMU传播的先验分布相结合，得到了 $\delta {\check{\mathbf{x}}}_k$ 的最大后验(MAP)估计：

$$\begin{array}{r}\underset{\delta {\check{\mathbf{x}}}_k}{\min }\left({\Vert {\check{\mathbf{x}}}_k\boxminus {\hat{\mathbf{x}}}_k+\mathcal{H}\delta {\check{\mathbf{x}}}_k\Vert }_{{\mathbf{\Sigma }}_{\delta {\hat{\mathbf{x}}}_k}^2}+\sum _{s=1}^m{\Vert \mathbf{o}\left({\check{\mathbf{x}}}_k,{}^G{\mathbf{p}}_s,{\mathbf{c}}_s\right)+{\mathbf{H}}_s^o\delta {\check{\mathbf{x}}}_k\Vert }_{{\mathbf{\Sigma }}_{{\mathbf{\beta }}_s}}^2\right)\end{array}\text{\hspace{1em}(24)}$$

其中：

$$\mathbf{H}={\left[{\mathbf{H}}_1^{oT},\dots ,{\mathbf{H}}_m^{o}T\right]}^T\text{\hspace{1em}(25)}$$

$$\mathbf{R}=\mathrm{diag}\left({\mathbf{\Sigma }}_{{\mathbf{\beta }}_1},\dots ,{\mathbf{\Sigma }}_{{\mathbf{\beta }}_m}\right)\text{\hspace{1em}(26)}$$

$${\check{\mathbf{z}}}_k={\left[\mathbf{o}\left({\check{\mathbf{x}}}_k,{}^G{\mathbf{p}}_1,{\mathbf{c}}_1\right)\dots ,\mathbf{o}\left({\check{\mathbf{x}}}_k,{}^G{\mathbf{p}}_m,{\mathbf{c}}_m\right)\right]}^T\text{\hspace{1em}(27)}$$

$$\mathbf{P}=(\mathcal{H}{)}^{-1}{\mathbf{\Sigma }}_{\delta {\hat{\mathbf{x}}}_k}(\mathcal{H}{)}^{-T}\text{\hspace{1em}(28)}$$

然后，我们执行类似于公式（15）和（16）的状态更新：

$${\hat{\mathbf{x}}}_k={\check{\mathbf{x}}}_k,{\mathbf{\Sigma }}_{\delta {\hat{\mathbf{x}}}_k}=(\mathbf{I}-\mathbf{KH}){\mathbf{\Sigma }}_{\delta {\check{\mathbf{x}}}_k}\text{\hspace{1em}(29)}$$

使用 ESIKF 对帧到地图的VIO 迭代更新直到收敛，然后使用收敛的状态估计值去：

1. 渲染地图的纹理
2. 更新当前跟踪点集合 $\mathcal{P}$ ，给下一帧使用
3. 在下一帧LIO或者VIO更新中，作为IMU传播的起点

2.3 渲染全局地图的纹理

在帧到地图的VIO更新之后，我们得到了当前图像帧的精确位姿，然后我们执行渲染功能来更新地图点的颜色。

首先，我们在所有激活的体素中的检索所有点。假设总共有 $n$ 个点，记为 ζ={P1,…,Pn}\zeta=\left\{\mathbf{P}_{1}, \ldots, \mathbf{P}_{n}\right\}ζ={P1​,…,Pn​}。以第 $s$ 个点 ${\mathbf{P}}_s={\left[{}^G{\mathbf{p}}_s^T,{\mathbf{c}}_s^T\right]}^T\in \zeta$ 的颜色更新过程为例。如果点 ${\mathbf{P}}_s$ 落在当前图像帧 ${\mathbf{I}}_k$ 中，我们通过对当前图像帧 ${\mathbf{I}}_k$ 上相邻像素的RGB颜色值进行线性插值，得到其观测颜色 ${\gamma }_s$ 和协方差 ${\mathbf{\Sigma }}_{{\mathbf{n}}_{{\gamma }_s}}$。通过贝叶斯更新，图像上新观测到的点的颜色与记录在地图中的现有颜色值 ${\mathbf{c}}_s$ 进行融合（如上图所示），得到更新后的颜色值和 ${\mathbf{c}}_s$ 的协方差：

$${\mathbf{\Sigma }}_{{\mathbf{n}}_{{\tilde{\mathbf{c}}}_s}}={\left({\left({\mathbf{\Sigma }}_{{\mathbf{n}}_{{\mathbf{c}}_s}}+{\mathbf{\sigma }}_s^2\cdot \Delta t_{{\mathbf{c}}_s}\right)}^{-1}+{\mathbf{\Sigma }}_{{\mathbf{n}}_{{\mathbf{\gamma }}_s}}^{-1}\right)}^{-1}\text{\hspace{1em}(30)}$$

$${\tilde{\mathbf{c}}}_s={\left({\left({\mathbf{\Sigma }}_{{\mathbf{n}}_{{\mathbf{c}}_s}}+{\mathbf{\sigma }}_s^2\cdot \Delta t_{{\mathbf{c}}_s}\right)}^{-1}{\mathbf{c}}_s+{\mathbf{\Sigma }}_{{\mathbf{n}}_{{\gamma }_s}}^{-1}{\gamma }_s\right)}^{-1}{\mathbf{\Sigma }}_{{\mathbf{n}}_{{\tilde{\mathbf{c}}}_s}}\text{\hspace{1em}(31)}$$

$${\mathbf{c}}_s={\tilde{\mathbf{c}}}_s,{\mathbf{\Sigma }}_{{\mathbf{n}}_{{\mathbf{c}}_s}}={\mathbf{\Sigma }}_{{\mathbf{n}}_{{\tilde{\mathbf{c}}}_s}}\text{\hspace{1em}(32)}$$

2.4 VIO子系统的跟踪点更新

在纹理渲染完成后，我们对所跟踪的点集 $\mathcal{P}$ 进行更新：

• 删除点：如果点集 $\mathcal{P}$ 的点通过公式（2）计算出来的PnP重投影误差或者通过公式（17）计算出来的光度误差太大，我们会从跟踪点的集合中删除这些点，我们还会删除重投影之后不属于当前图像帧 ${\mathbf{I}}_k$ 的点。
• 添加点：我们将 $\zeta$ 中的每个点投影到当前图像帧 ${\mathbf{I}}_k$ 上，如果附近没有其它跟踪点（例如，半径50像素内），则将其添加到点集 $\mathcal{P}$ 中。