流形上的预积分

论文：IMU Preintegration on Manifold for Effificient Visual-Inertial Maximum-a-Posteriori Estimation

引言

Recent results in monocular visual-inertial navigation (VIN) have shown that optimization-based approaches outperform filtering methods in terms of accuracy due to their capability to relinearize past states. However, the improvement comes at the cost of increased computational complexity. In this paper, we address this issue by preintegrating inertial measurements between selected keyframes. The preintegration allows us to accurately summarize hundreds of inertial measurements into a single relative motion constraint. Our first contribution is a preintegration theory that properly addresses the manifold structure of the rotation group and carefully deals with uncertainty propagation. The measurements are integrated in a local frame, which eliminates the need to repeat the integration when the linearization point changes while leaving the opportunity for belated bias corrections. The second contribution is to show that the preintegrated IMU model can be seamlessly integrated in a visual-inertial pipeline under the unifying framework of factor graphs. This enables the use of a structureless model for visual measurements, further accelerating the computation. The third contribution is an extensive evaluation of our monocular VIN pipeline: experimental results confirm that our system is very fast and demonstrates superior accuracy with respect to competitive state-of-the-art filtering and optimization algorithms, including off-the-shelf systems such as Google Tango [1].

接上文…

D.预积分IMU因子

预积分测量模型：
ΔR~ij=Ri⊤RjExp⁡(δϕij)Δv~ij=Ri⊤(vj−vi−gΔtij)+δvijΔp~ij=Ri⊤(pj−pi−viΔtij−12gΔtij2)+δpij(31)\begin{aligned} \Delta \tilde{\mathrm{R}}_{i j} &=\mathrm{R}_{i}^{\top} \mathrm{R}_{j} \operatorname{Exp}\left(\delta \phi_{i j}\right) \\ \Delta \tilde{\mathbf{v}}_{i j} &=\mathrm{R}_{i}^{\top}\left(\mathbf{v}_{j}-\mathbf{v}_{i}-\mathbf{g} \Delta t_{i j}\right)+\delta \mathbf{v}_{i j} \\ \Delta \tilde{\mathbf{p}}_{i j} &=\mathrm{R}_{i}^{\top}\left(\mathbf{p}_{j}-\mathbf{p}_{i}-\mathbf{v}_{i} \Delta t_{i j}-\frac{1}{2} \mathbf{g} \Delta t_{i j}^{2}\right)+\delta \mathbf{p}_{i j} \end{aligned} \tag{31} ΔR~ijΔv~ijΔp~ij=Ri⊤RjExp(δϕij)=Ri⊤(vj−vi−gΔtij)+δvij=Ri⊤(pj−pi−viΔtij−21gΔtij2)+δpij(31)
预积分的噪声向量。
[δϕij⊤,δvij⊤,δpij⊤]⊤∼N(09×1,Σij)(35)\left[\delta \boldsymbol{\phi}_{i j}^{\top}, \delta \mathbf{v}_{i j}^{\top}, \delta \mathbf{p}_{i j}^{\top}\right]^{\top} \sim \mathcal{N}\left(\mathbf{0}_{9 \times 1}, \boldsymbol{\Sigma}_{i j}\right) \tag{35} [δϕij⊤,δvij⊤,δpij⊤]⊤∼N(09×1,Σij)(35)

考虑(31)中的预积分测量模型，由于测量噪声为零均值且为一阶高斯(35)，残差 rIij≡[rΔRijT,rΔvijT,rΔpijT]T∈R9\bold{r}_{\mathcal{I}_{ij}} \equiv [ \bold{r}_{\Delta \bold{R}_{ij}}^T , \bold{r}_{\Delta \bold{v}_{ij}}^T , \bold{r}_{\Delta \bold{p}_{ij}}^T ]^T \in \mathbb{R}^9rIij≡[rΔRijT,rΔvijT,rΔpijT]T∈R9 :
rΔRij≐Log((ΔR~ij(b‾ig)Exp⁡(∂ΔR‾ij∂bgδbg))⊤Ri⊤Rj)rΔvij≐Ri⊤(vj−vi−gΔtij)−[Δv~ij(b‾ig,b‾ia)+∂Δv‾ij∂bgδbg+∂Δv‾ij∂baδba]rΔpij≐Ri⊤(pj−pi−viΔtij−12gΔtij2)−[Δp~ij(b‾ig,b‾ia)+∂Δp‾ij∂bgδbg+∂Δp‾ij∂baδba](37)\begin{aligned} \mathbf{r}_{\Delta \mathrm{R}_{i j}} & \doteq \text{Log} \left(\left(\Delta \tilde{\mathbf{R}}_{i j}\left(\overline{\mathbf{b}}_{i}^{g}\right) \operatorname{Exp}\left(\frac{\partial \Delta \overline{\mathrm{R}}_{i j}}{\partial \mathbf{b}^{g}} \delta \mathbf{b}^{g}\right)\right)^{\top} \mathrm{R}_{i}^{\top} \mathrm{R}_{j}\right) \\ \mathbf{r}_{\Delta \mathbf{v}_{i j}} & \doteq \mathrm{R}_{i}^{\top}\left(\mathbf{v}_{j}-\mathbf{v}_{i}-\mathbf{g} \Delta t_{i j}\right) -\left[\Delta \tilde{\mathbf{v}}_{i j}\left(\overline{\mathbf{b}}_{i}^{g}, \overline{\mathbf{b}}_{i}^{a}\right)+\frac{\partial \Delta \overline{\mathbf{v}}_{i j}}{\partial \mathbf{b}^{g}} \delta \mathbf{b}^{g}+\frac{\partial \Delta \overline{\mathbf{v}}_{i j}}{\partial \mathbf{b}^{a}} \delta \mathbf{b}^{a}\right] \\ \mathbf{r}_{\Delta \mathbf{p}_{i j}} & \doteq \mathrm{R}_{i}^{\top}\left(\mathbf{p}_{j}-\mathbf{p}_{i}-\mathbf{v}_{i} \Delta t_{i j}-\frac{1}{2} \mathbf{g} \Delta t_{i j}^{2}\right) -\left[\Delta \tilde{\mathbf{p}}_{i j}\left(\overline{\mathbf{b}}_{i}^{g}, \overline{\mathbf{b}}_{i}^{a}\right)+\frac{\partial \Delta \overline{\mathbf{p}}_{i j}}{\partial \mathbf{b}^{g}} \delta \mathbf{b}^{g}+\frac{\partial \Delta \overline{\mathbf{p}}_{i j}}{\partial \mathbf{b}_{a}} \delta \mathbf{b}^{a}\right] \end{aligned} \tag{37} rΔRijrΔvijrΔpij≐Log((ΔR~ij(big)Exp(∂bg∂ΔRijδbg))⊤Ri⊤Rj)≐Ri⊤(vj−vi−gΔtij)−[Δv~ij(big,bia)+∂bg∂Δvijδbg+∂ba∂Δvijδba]≐Ri⊤(pj−pi−viΔtij−21gΔtij2)−[Δp~ij(big,bia)+∂bg∂Δpijδbg+∂ba∂Δpijδba](37)
其中还包含了式(36)的bias更新。

根据“lift-solve-retract”方法(Section II-C)，在每次GN迭代中，需要使用retraction(15)重新参数化(37)。然后，“求解”步骤需要将结果代价线性化。为了线性化的目的，计算残差的雅可比矩阵的解析表达式是很方便的，如下面的补充材料所示。

补充材料：预积分因子

从预积分IMU测量的残差表达式(c.f.， Eq.(37))开始:
rΔRij≐log⁡((ΔR~ij(b‾ig)Exp⁡(∂ΔR‾ij∂bgδbg))⊤Ri⊤Rj)rΔvij≐Ri⊤(vj−vi−gΔtij)−[Δv~ij(b‾ig,b‾ia)+∂Δv‾ij∂bgδbg+∂Δv‾ij∂baδba]rΔpij≐Ri⊤(pj−pi−viΔtij−12gΔtij2)−[Δp~ij(b‾ig,b‾ia)+∂Δp‾ij∂bgδbg+∂Δp‾ij∂baδba](A.21)\begin{aligned} \mathbf{r}_{\Delta \mathrm{R}_{i j}} &\doteq \log \left(\left(\Delta \tilde{\mathbf{R}}_{i j}\left(\overline{\mathbf{b}}_{i}^{g}\right) \operatorname{Exp}\left(\frac{\partial \Delta \overline{\mathbf{R}}_{i j}}{\partial \mathbf{b}^{g}} \delta \mathbf{b}^{g}\right)\right)^{\top} \mathrm{R}_{i}^{\top} \mathrm{R}_{j}\right) \\ \mathbf{r}_{\Delta \mathbf{v}_{i j}} &\doteq \mathrm{R}_{i}^{\top}\left(\mathbf{v}_{j}-\mathbf{v}_{i}-\mathbf{g} \Delta t_{i j}\right)-\left[\Delta \tilde{\mathbf{v}}_{i j}\left(\overline{\mathbf{b}}_{i}^{g}, \overline{\mathbf{b}}_{i}^{a}\right)+\frac{\partial \Delta \overline{\mathbf{v}}_{i j}}{\partial \mathbf{b}^{g}} \delta \mathbf{b}^{g}+\frac{\partial \Delta \overline{\mathbf{v}}_{i j}}{\partial \mathbf{b}^{a}} \delta \mathbf{b}^{a}\right] \\ \mathbf{r}_{\Delta \mathbf{p}_{i j}} &\doteq \mathbf{R}_{i}^{\top}\left(\mathbf{p}_{j}-\mathbf{p}_{i}-\mathbf{v}_{i} \Delta t_{i j}-\frac{1}{2} \mathbf{g} \Delta t_{i j}^{2}\right)-\left[\Delta \tilde{\mathbf{p}}_{i j}\left(\overline{\mathbf{b}}_{i}^{g}, \overline{\mathbf{b}}_{i}^{a}\right)+\frac{\partial \Delta \overline{\mathbf{p}}_{i j}}{\partial \mathbf{b}^{g}} \delta \mathbf{b}^{g}+\frac{\partial \Delta \overline{\mathbf{p}}_{i j}}{\partial \mathbf{b}_{a}} \delta \mathbf{b}^{a}\right] \end{aligned} \tag{A.21} rΔRijrΔvijrΔpij≐log((ΔR~ij(big)Exp(∂bg∂ΔRijδbg))⊤Ri⊤Rj)≐Ri⊤(vj−vi−gΔtij)−[Δv~ij(big,bia)+∂bg∂Δvijδbg+∂ba∂Δvijδba]≐Ri⊤(pj−pi−viΔtij−21gΔtij2)−[Δp~ij(big,bia)+∂bg∂Δpijδbg+∂ba∂Δpijδba](A.21)
“Lifting”代价函数包括用下列retraction代替:
pi←pi+Riδpi,Ri←RiExp⁡(δϕi),pj←pj+Rjδpj,Rj←RjExp⁡(δϕj)(A.22)\mathbf{p}_{i} \leftarrow \mathbf{p}_{i}+\mathrm{R}_{i} \delta \mathbf{p}_{i}, \quad \mathrm{R}_{i} \leftarrow \mathrm{R}_{i} \operatorname{Exp}\left(\delta \boldsymbol{\phi}_{i}\right), \quad \mathbf{p}_{j} \leftarrow \mathbf{p}_{j}+\mathrm{R}_{j} \delta \mathbf{p}_{j}, \quad \mathrm{R}_{j} \leftarrow \mathrm{R}_{j} \operatorname{Exp}\left(\delta \boldsymbol{\phi}_{j}\right) \tag{A.22} pi←pi+Riδpi,Ri←RiExp(δϕi),pj←pj+Rjδpj,Rj←RjExp(δϕj)(A.22)
而由于速度和bias已经存在于向量空间中，相应的retraction减少为:
vi←vi+δvi,vj←vj+δvi,δbig←δbig+δ~bgi,δbia←δbia+δ~bai(A.23)\mathbf{v}_{i} \leftarrow \mathbf{v}_{i}+\delta \mathbf{v}_{i}, \quad \mathbf{v}_{j} \leftarrow \mathbf{v}_{j}+\delta \mathbf{v}_{i}, \quad \delta \mathbf{b}_{i}^{g} \leftarrow \delta \mathbf{b}_{i}^{g}+\tilde{\delta} \mathbf{b}_{g_{i}}, \quad \delta \mathbf{b}_{i}^{a} \leftarrow \delta \mathbf{b}_{i}^{a}+\tilde{\delta} \mathbf{b}_{a i} \tag{A.23} vi←vi+δvi,vj←vj+δvi,δbig←δbig+δ~bgi,δbia←δbia+δ~bai(A.23)
Lifting过程使残差成为定义在向量空间上的函数，在向量空间上容易计算雅可比矩阵。

下面推导关于 δϕi,δpi,δvi,δϕj,δpj,δvj,δ~bgi,δ~bai\delta \boldsymbol{\phi}_{i}, \delta \mathbf{p}_{i}, \delta \mathbf{v}_{i}, \delta \boldsymbol{\phi}_{j}, \delta \mathbf{p}_{j}, \delta \mathbf{v}_{j}, \tilde{\delta} \mathbf{b}_{g_{i}}, \tilde{\delta} \mathbf{b}_{a i}δϕi,δpi,δvi,δϕj,δpj,δvj,δ~bgi,δ~bai 的雅可比。

rΔpij\mathbf{r}_{\Delta \mathbf{p}_{i j}}rΔpij 的雅可比

rΔpij(pi+Riδpi)=Ri⊤(pj−pi−Riδpi−viΔtij−12gΔtij2)−[Δp~ij+∂Δp‾ij∂bigδbig+∂Δp‾ij∂biaδbia]=rΔpij(pi)+(−I3×1)δpi(A.24)\mathbf{r}_{\Delta \mathbf{p}_{i j}}\left(\mathbf{p}_{i}+\mathrm{R}_{i} \delta \mathbf{p}_{i}\right)=\mathrm{R}_{i}^{\top}\left(\mathbf{p}_{j}-\mathbf{p}_{i}-\mathrm{R}_{i} \delta \mathbf{p}_{i}-\mathbf{v}_{i} \Delta t_{i j}-\frac{1}{2} \mathbf{g} \Delta t_{i j}^{2}\right)-\left[\Delta \tilde{\mathbf{p}}_{i j}+\frac{\partial \Delta \overline{\mathbf{p}}_{i j}}{\partial \mathbf{b}_{i}^{g}} \delta \mathbf{b}_{i}^{g}+\frac{\partial \Delta \overline{\mathbf{p}}_{i j}}{\partial \mathbf{b}_{i}^{a}} \delta \mathbf{b}_{i}^{a}\right] \\ =\mathbf{r}_{\Delta \mathbf{p}_{i j}}\left(\mathbf{p}_{i}\right)+\left(-\mathbf{I}_{3 \times 1}\right) \delta \mathbf{p}_{i} \tag{A.24} rΔpij(pi+Riδpi)=Ri⊤(pj−pi−Riδpi−viΔtij−21gΔtij2)−[Δp~ij+∂big∂Δpijδbig+∂bia∂Δpijδbia]=rΔpij(pi)+(−I3×1)δpi(A.24)

rΔpij(pj+Rjδpj)=Ri⊤(pj+Rjδpj−pi−viΔtij−12gΔtij2)−[Δp~ij+∂Δp‾ij∂bigδbig+∂Δp‾ij∂biaδbia]=rΔpij(pj)+(Ri⊤Rj)δpj(A.25)\mathbf{r}_{\Delta \mathbf{p}_{i j}}\left(\mathbf{p}_{j}+\mathrm{R}_{j} \delta \mathbf{p}_{j}\right)=\mathrm{R}_{i}^{\top}\left(\mathbf{p}_{j}+\mathrm{R}_{j} \delta \mathbf{p}_{j}-\mathbf{p}_{i}-\mathbf{v}_{i} \Delta t_{i j}-\frac{1}{2} \mathbf{g} \Delta t_{i j}^{2}\right)-\left[\Delta \tilde{\mathbf{p}}_{i j}+\frac{\partial \Delta \overline{\mathbf{p}}_{i j}}{\partial \mathbf{b}_{i}^{g}} \delta \mathbf{b}_{i}^{g}+\frac{\partial \Delta \overline{\mathbf{p}}_{i j}}{\partial \mathbf{b}_{i}^{a}} \delta \mathbf{b}_{i}^{a}\right] \\ =\mathbf{r}_{\Delta \mathbf{p}_{i j}}\left(\mathbf{p}_{j}\right)+\left(\mathrm{R}_{i}^{\top} \mathrm{R}_{j}\right) \delta \mathbf{p}_{j} \tag{A.25} rΔpij(pj+Rjδpj)=Ri⊤(pj+Rjδpj−pi−viΔtij−21gΔtij2)−[Δp~ij+∂big∂Δpijδbig+∂bia∂Δpijδbia]=rΔpij(pj)+(Ri⊤Rj)δpj(A.25)

rΔpij(vi+δvi)=Ri⊤(pj−pi−viΔtij−δviΔtij−12gΔtij2)−[Δp~ij+∂Δp‾ij∂bigδbig+∂Δp‾ij∂biaδbia]=rΔpij(vi)+(−Ri⊤Δtij)δvi(A.26)\mathbf{r}_{\Delta \mathbf{p}_{i j}}\left(\mathbf{v}_{i}+\delta \mathbf{v}_{i}\right)=\mathbf{R}_{i}^{\top}\left(\mathbf{p}_{j}-\mathbf{p}_{i}-\mathbf{v}_{i} \Delta t_{i j}-\delta \mathbf{v}_{i} \Delta t_{i j}-\frac{1}{2} \mathbf{g} \Delta t_{i j}^{2}\right)-\left[\Delta \tilde{\mathbf{p}}_{i j}+\frac{\partial \Delta \overline{\mathbf{p}}_{i j}}{\partial \mathbf{b}_{i}^{g}} \delta \mathbf{b}_{i}^{g}+\frac{\partial \Delta \overline{\mathbf{p}}_{i j}}{\partial \mathbf{b}_{i}^{a}} \delta \mathbf{b}_{i}^{a}\right] \\ =\mathbf{r}_{\Delta \mathbf{p}_{i j}}\left(\mathbf{v}_{i}\right)+\left(-\mathbf{R}_{i}^{\top} \Delta t_{i j}\right) \delta \mathbf{v}_{i} \tag{A.26} rΔpij(vi+δvi)=Ri⊤(pj−pi−viΔtij−δviΔtij−21gΔtij2)−[Δp~ij+∂big∂Δpijδbig+∂bia∂Δpijδbia]=rΔpij(vi)+(−Ri⊤Δtij)δvi(A.26)

rΔpij(RiExp⁡(δϕi))=(RiExp⁡(δϕi))⊤(pj−pi−viΔtij−12gΔtij2)−[Δp~ij+∂Δp‾ij∂bigδbig+∂Δp‾ij∂biaδbia]≃Eq. (4) (I−δϕi∧)Ri⊤(pj−pi−viΔtij−12gΔtij2)−[Δp~ij+∂Δp‾ij∂bigδbig+∂Δp‾ij∂biaδbia]=rΔpij(Ri)+(Ri⊤(pj−pi−viΔtij−12gΔtij2))∧δϕi(A.27)\mathbf{r}_{\Delta \mathbf{p}_{i j}}\left(\mathrm{R}_{i} \operatorname{Exp}\left(\delta \boldsymbol{\phi}_{i}\right)\right)=\left(\mathrm{R}_{i} \operatorname{Exp}\left(\delta \boldsymbol{\phi}_{i}\right)\right)^{\top}\left(\mathbf{p}_{j}-\mathbf{p}_{i}-\mathbf{v}_{i} \Delta t_{i j}-\frac{1}{2} \mathbf{g} \Delta t_{i j}^{2}\right)-\left[\Delta \tilde{\mathbf{p}}_{i j}+\frac{\partial \Delta \overline{\mathbf{p}}_{i j}}{\partial \mathbf{b}_{i}^{g}} \delta \mathbf{b}_{i}^{g}+\frac{\partial \Delta \overline{\mathbf{p}}_{i j}}{\partial \mathbf{b}_{i}^{a}} \delta \mathbf{b}_{i}^{a}\right] \\ \stackrel{\text { Eq. (4) }}{\simeq}\left(\mathbf{I}-\delta \phi_{i}^{\wedge}\right) \mathrm{R}_{i}^{\top}\left(\mathbf{p}_{j}-\mathbf{p}_{i}-\mathbf{v}_{i} \Delta t_{i j}-\frac{1}{2} \mathbf{g} \Delta t_{i j}^{2}\right)-\left[\Delta \tilde{\mathbf{p}}_{i j}+\frac{\partial \Delta \overline{\mathbf{p}}_{i j}}{\partial \mathbf{b}_{i}^{g}} \delta \mathbf{b}_{i}^{g}+\frac{\partial \Delta \overline{\mathbf{p}}_{i j}}{\partial \mathbf{b}_{i}^{a}} \delta \mathbf{b}_{i}^{a}\right] \\ =\mathbf{r}_{\Delta \mathbf{p}_{i j}}\left(\mathrm{R}_{i}\right)+\left(\mathrm{R}_{i}^{\top}\left(\mathbf{p}_{j}-\mathbf{p}_{i}-\mathbf{v}_{i} \Delta t_{i j}-\frac{1}{2} \mathbf{g} \Delta t_{i j}^{2}\right)\right)^{\wedge} \delta \phi_{i} \quad \tag{A.27} rΔpij(RiExp(δϕi))=(RiExp(δϕi))⊤(pj−pi−viΔtij−21gΔtij2)−[Δp~ij+∂big∂Δpijδbig+∂bia∂Δpijδbia]≃ Eq. (4) (I−δϕi∧)Ri⊤(pj−pi−viΔtij−21gΔtij2)−[Δp~ij+∂big∂Δpijδbig+∂bia∂Δpijδbia]=rΔpij(Ri)+(Ri⊤(pj−pi−viΔtij−21gΔtij2))∧δϕi(A.27)

使用 a∧b=−b∧aa^{\wedge} b=-b^{\wedge} aa∧b=−b∧a .

综上所述， rΔpij\mathbf{r}_{\Delta \mathbf{p}_{i j}}rΔpij 的雅可比矩阵为:
KaTeX parse error: Expected 'EOF', got '&' at position 81: …elta \phi_{i}} &̲=\left(\mathrm{…

E.Bias模型

当提出IMU模型(20)时，我们说bias是缓慢的时变量。因此用“布朗运动”来建模，即积分白噪声:
b˙g(t)=ηbg,b˙a(t)=ηba(38)\dot{\mathbf{b}}^{g}(t)=\boldsymbol{\eta}^{b g}, \quad \dot{\mathbf{b}}^{a}(t)=\boldsymbol{\eta}^{b a} \tag{38} b˙g(t)=ηbg,b˙a(t)=ηba(38)
在两个连续的关键帧 iii 和 jjj 之间对时间间隔 [ti,tj][t_i, t_j][ti,tj] 进行积分，得到:
bjg=big+ηbgd,bja=bia+ηbad(39)\mathbf{b}_{j}^{g}=\mathbf{b}_{i}^{g}+\boldsymbol{\eta}^{b g d}, \quad \mathbf{b}_{j}^{a}=\mathbf{b}_{i}^{a}+\boldsymbol{\eta}^{b a d} \tag{39} bjg=big+ηbgd,bja=bia+ηbad(39)
记 big≡bg(ti)\bold{b}_i^g \equiv \bold{b}^g(t_i)big≡bg(ti) ,定义离散噪声 ηbgd\boldsymbol{\eta}^{bgd}ηbgd 和 ηbad\boldsymbol{\eta}^{bad}ηbad ，均值为0，协方差为 Σbgd≡ΔtijCov(ηbg)\bold{\Sigma}^{bgd} \equiv \Delta t_{ij} \text{Cov}(\boldsymbol{\eta}^{bg})Σbgd≡ΔtijCov(ηbg) 和 Σbad≡ΔtijCov(ηba)\bold{\Sigma}^{bad} \equiv \Delta t_{ij} \text{Cov}(\boldsymbol{\eta}^{ba})Σbad≡ΔtijCov(ηba) 。

模型(39)可以很容易地包含在因子图中，作为(19)中所有连续关键帧的进一步相加项:
∥rbij∥2≐∥bjg−big∥Σbgd2+∥bja−bia∥Σbad 2(40)\left\|\mathbf{r}_{\mathbf{b}_{i j}}\right\|^{2} \doteq\left\|\mathbf{b}_{j}^{g}-\mathbf{b}_{i}^{g}\right\|_{\Sigma^{b g d}}^{2}+\left\|\mathbf{b}_{j}^{a}-\mathbf{b}_{i}^{a}\right\|_{\Sigma^{\text {bad }}}^{2} \tag{40} ∥∥rbij∥∥2≐∥∥bjg−big∥∥Σbgd2+∥∥bja−bia∥∥Σbad 2(40)

无结构视觉因子

在本节介绍视觉测量的无结构模型，其主要特点是线性消除路标。注意，在每一次GN迭代中都要重复消去，因此仍然可以保证得到最优的MAP估计。

视觉测量通过以下方式贡献代价(19):
∑i∈Kk∑l∈Ci∥rCil∥Σc2=∑l=1L∑i∈X(l)∥rCil∥ΣC2(41)\sum_{i \in \mathcal{K}_{k}} \sum_{l \in \mathcal{C}_{i}}\left\|\mathbf{r}_{\mathcal{C}_{i l}}\right\|_{\Sigma_{c}}^{2}=\sum_{l=1}^{L} \sum_{i \in \mathcal{X}(l)}\left\|\mathbf{r}_{\mathcal{C}_{i l}}\right\|_{\Sigma_{\mathcal{C}}}^{2} \tag{41} i∈Kk∑l∈Ci∑∥rCil∥Σc2=l=1∑Li∈X(l)∑∥rCil∥ΣC2(41)
在右边，把它改写为每个路标 l=1,⋯,Ll=1,\cdots,Ll=1,⋯,L 的贡献之和。在(41)中， X(l)\mathcal{X} (l)X(l) 表示看到 lll 的关键帧的子集。

单像素测量 zil\bold{z}_{il}zil 残差的一个相当标准的模型是[13]:
rCil=zil−π(Ri,pi,ρl)(42)\mathbf{r}_{\mathcal{C}_{i l}}=\mathbf{z}_{i l}-\pi\left(\mathrm{R}_{i}, \mathbf{p}_{i}, \rho_{l}\right) \tag{42} rCil=zil−π(Ri,pi,ρl)(42)
其中 ρl∈R3\rho_l \in \mathbb{R}^3ρl∈R3 表示第 lll 个路标点的位置， π(.)\pi(.)π(.) 表示标准投影函数，其中包含了已知的IMU-相机外参 TBC\bold{T}_{BC}TBC 。

直接使用(42)需要在优化中包含路标位置 ρl\rho_lρl ，这对计算产生了负面影响。因此，接下来采用了一种无结构的方法，避免了对路标的优化，从而保证了MAP估计的检索。

正如在第II-C节中回顾的，在每次GN迭代中，使用retraction(15)提升代价函数。对于视觉因子，这意味着原始残差(41)变成:
∑l=1L∑i∈X(l)∥zil−πˇ(δϕi,δpi,δρl)∥Σc2(43)\sum_{l=1}^{L} \sum_{i \in \mathcal{X}(l)}\left\|\mathbf{z}_{i l}-\check{\pi}\left(\delta \phi_{i}, \delta \mathbf{p}_{i}, \delta \rho_{l}\right)\right\|_{\boldsymbol{\Sigma}_{c}}^{2} \tag{43} l=1∑Li∈X(l)∑∥zil−πˇ(δϕi,δpi,δρl)∥Σc2(43)
其中 δϕi\delta \boldsymbol{\phi}_{i}δϕi , δpi\delta \bold{p}_{i}δpi , δρl\delta \rho_lδρl 是现在的欧式修正， πˇ(.)\check{\pi}(.)πˇ(.) 是提升后的代价函数。 GN方法中的“求解”步骤是基于残差的线性化:
∑l=1L∑i∈X(l)∥FilδTi+Eilδρl−bil∥2,(44)\sum_{l=1}^{L} \sum_{i \in \mathcal{X}(l)}\left\|\mathbf{F}_{i l} \delta \mathbf{T}_{i}+\mathbf{E}_{i l} \delta \rho_{l}-\mathbf{b}_{i l}\right\|^{2}, \tag{44} l=1∑Li∈X(l)∑∥FilδTi+Eilδρl−bil∥2,(44)
其中 δTi≡[δϕi,δpi]T\delta \bold{T}_i \equiv [\delta \boldsymbol{\phi}_{i},\delta \bold{p}_{i}]^TδTi≡[δϕi,δpi]T 。雅可比矩阵 Fil\bold{F}_{il}Fil , Eil\bold{E}_{il}Eil 和向量 bil\bold{b}_{il}bil （都是用 ΣC1/2\Sigma_{\mathcal{C}}^{1/2}ΣC1/2 进行归一化了）从线性化结果中得到。向量 bil\bold{b}_{il}bil 是线性化点的残差。

将(44)中的第二个和写成矩阵形式，得到:
∑l=1L∥FlδTX(l)+Elδρl−bl∥2(45)\sum_{l=1}^{L}\left\|\mathbf{F}_{l} \delta \mathbf{T}_{\mathcal{X}(l)}+\mathbf{E}_{l} \delta \rho_{l}-\mathbf{b}_{l}\right\|^{2} \tag{45} l=1∑L∥∥FlδTX(l)+Elδρl−bl∥∥2(45)
其中 Fl\bold{F}_{l}Fl , El\bold{E}_{l}El 和 bl\bold{b}_{l}bl 通过堆叠 Fil\bold{F}_{il}Fil , Eil\bold{E}_{il}Eil , bil\bold{b}_{il}bil 得到， i∈X(l)i \in \mathcal{X}(l)i∈X(l) 。可以通过将残差投影到 El\bold{E}_{l}El 的零空间来消去变量 δρl\delta \rho_lδρl :
∑l=1L∥Q(FlδTX(l)−bl)∥2(46)\sum_{l=1}^{L}\left\|\mathbf{Q}\left(\mathbf{F}_{l} \delta \mathbf{T}_{\mathcal{X}(l)}-\mathbf{b}_{l}\right)\right\|^{2} \tag{46} l=1∑L∥∥Q(FlδTX(l)−bl)∥∥2(46)
其中 Q≡I−El(ElTEl)−1ElT\bold{Q} \equiv \bold{I} - \bold{E}_{l}(\bold{E}_{l}^T \bold{E}_{l})^{-1} \bold{E}_{l}^TQ≡I−El(ElTEl)−1ElT 是 El\bold{E}_{l}El 的正交投影，如补充材料所示。使用这种方法，我们将涉及位姿和路标的大量因子(43个)简化为只涉及位姿的 LLL 个因子（公式(46)）。其中路标 lll 对应的因子仅涉及图4连通性模式的状态。

补充材料：舒尔补和零空间投影

从线性化的视觉因子开始，在主文档的Eq.(44)中给出:
∑l=1L∑i∈X(l)∥FilδTi+Eilδρl−bil∥2(A.34)\sum_{l=1}^{L} \sum_{i \in \mathcal{X}(l)}\left\|\mathbf{F}_{i l} \delta \mathbf{T}_{i}+\mathbf{E}_{i l} \delta \rho_{l}-\mathbf{b}_{i l}\right\|^{2} \tag{A.34} l=1∑Li∈X(l)∑∥FilδTi+Eilδρl−bil∥2(A.34)
其中是一个关于 δTi≐[δϕiδpi]⊤∈R6\delta \mathbf{T}_{i} \doteq\left[\delta \boldsymbol{\phi}_{i} \delta \mathbf{p}_{i}\right]^{\top} \in \mathbb{R}^{6}δTi≐[δϕiδpi]⊤∈R6 在关键帧 iii 线性化点的扰动， δρl\delta \rho_{l}δρl 是一个关于路标点 lll 的线性化点的扰动。向量 bil∈R2\mathbf{b}_{i l} \in \mathbb{R}^{2}bil∈R2 是在线性化点的残差。为了简洁起见，我们不进入雅可比矩阵 Fil∈R2×6,Eil∈R2×3\mathbf{F}_{i l} \in \mathbb{R}^{2 \times 6}, \mathbf{E}_{i l} \in \mathbb{R}^{2 \times 3}Fil∈R2×6,Eil∈R2×3 的细节。

现在，正如在主文档中所做的那样，我们用 δTX(l)∈R6n\delta \mathbf{T}_{\mathcal{X}(l)} \in \mathbb{R}^{6 n}δTX(l)∈R6n 表示对观测路标 lll 的 nln_lnl 个相机的扰动 δTi\delta \mathbf{T}_{i}δTi 的叠加向量。用这种表示法(A.34)可以写成矩阵
∑l=1L∥FlδTX(l)+Elδρl−bl∥2(A.35)\sum_{l=1}^{L}\left\|\mathbf{F}_{l} \delta \mathbf{T}_{\mathcal{X}(l)}+\mathbf{E}_{l} \delta \rho_{l}-\mathbf{b}_{l}\right\|^{2} \tag{A.35} l=1∑L∥∥FlδTX(l)+Elδρl−bl∥∥2(A.35)
其中：
Fl≐[⋱02×6…02×602×6Fil…⋮⋮…⋱02×602×6…02×6⋱]∈R2nl×6nl,El≐[…Eil……]∈R2nl×3,bl≐[⋮bil⋮⋮]∈R2nl(A.36)\mathbf{F}_{l} \doteq\left[\begin{array}{cccc} \ddots & \mathbf{0}_{2 \times 6} & \ldots & \mathbf{0}_{2 \times 6} \\ \mathbf{0}_{2 \times 6} & \mathbf{F}_{i l} & \ldots & \vdots \\ \vdots & \ldots & \ddots & \mathbf{0}_{2 \times 6} \\ \mathbf{0}_{2 \times 6} & \ldots & \mathbf{0}_{2 \times 6} & \ddots \end{array}\right] \in \mathbb{R}^{2 n_{l} \times 6 n_{l}}, \quad \mathbf{E}_{l} \doteq\left[\begin{array}{c} \ldots \\ \mathbf{E}_{i l} \\ \ldots \\ \ldots \end{array}\right] \in \mathbb{R}^{2 n_{l} \times 3}, \quad \mathbf{b}_{l} \doteq\left[\begin{array}{c} \vdots \\ \mathbf{b}_{i l} \\ \vdots \\ \vdots \end{array}\right] \in \mathbb{R}^{2 n_{l}} \tag{A.36} Fl≐⎣⎡⋱02×6⋮02×602×6Fil…………⋱02×602×6⋮02×6⋱⎦⎤∈R2nl×6nl,El≐⎣⎡…Eil……⎦⎤∈R2nl×3,bl≐⎣⎡⋮bil⋮⋮⎦⎤∈R2nl(A.36)
由于路标 lll 出现在和(A.35)的单个项中，对于任何给定的位姿扰动 δTX(l)\delta \mathbf{T}_{\mathcal{X}(l)}δTX(l) ，路标扰动 δρl\delta \rho_{l}δρl ，最小化二次代价 ∥FlδTX(l)+Elδρl−bl∥2\left\|\mathbf{F}_{l} \delta \mathbf{T}_{\mathcal{X}(l)}+\mathbf{E}_{l} \delta \rho_{l}-\mathbf{b}_{l}\right\|^{2}∥∥FlδTX(l)+Elδρl−bl∥∥2 是：
δρl=−(El⊤El)−1El⊤(FlδTX(l)−bl)(A.37)\delta \rho_{l}=-\left(\mathbf{E}_{l}^{\boldsymbol{\top}} \mathbf{E}_{l}\right)^{-1} \mathbf{E}_{l}^{\top}\left(\mathbf{F}_{l} \delta \mathbf{T}_{\mathcal{X}(l)}-\mathbf{b}_{l}\right) \tag{A.37} δρl=−(El⊤El)−1El⊤(FlδTX(l)−bl)(A.37)
将(A.37)代回(A.35)，可以消除优化问题中的路标:
∑l=1L∥(I−El(El⊤El)−1El⊤)(FlδTX(l)−bl)∥2(A.38)\sum_{l=1}^{L}\left\|\left(\mathbf{I}-\mathbf{E}_{l}\left(\mathbf{E}_{l}^{\boldsymbol{\top}} \mathbf{E}_{l}\right)^{-1} \mathbf{E}_{l}^{\top}\right)\left(\mathbf{F}_{l} \delta \mathbf{T}_{\mathcal{X}(l)}-\mathbf{b}_{l}\right)\right\|^{2} \tag{A.38} l=1∑L∥∥(I−El(El⊤El)−1El⊤)(FlδTX(l)−bl)∥∥2(A.38)
对应于主文档的公式(46)。无结构因子(A.38)只涉及位姿，并允许执行优化忽略路标位置。

通过以下线性代数的考虑可以进一步改进计算。首先，记 Q≐(I−El(El⊤El)−1El⊤)∈R2nl×2nl\mathbf{Q} \doteq\left(\mathbf{I}-\mathbf{E}_{l}\left(\mathbf{E}_{l}^{\top} \mathbf{E}_{l}\right)^{-1} \mathbf{E}_{l}^{\top}\right) \in \mathbb{R}^{2 n_{l} \times 2 n_{l}}Q≐(I−El(El⊤El)−1El⊤)∈R2nl×2nl 是 El\mathbf{E}_{l}El 的orthogonal projector。粗略地说， Q\mathbf{Q}Q 将R2nl\mathbb{R}^{2 n_{l}}R2nl 中的任何向量投影到矩阵 El\mathbf{E}_{l}El 的零空间。此外， El\mathbf{E}_{l}El 的零空间的基 El⊥∈R2nl×2nl−3\mathbf{E}_{l}^{\perp} \in \mathbb{R}^{2 n_{l} \times 2 n_{l}-3}El⊥∈R2nl×2nl−3 满足下面的关系：
El⊥((El⊥)⊤El⊥)−1(El⊥)⊤=I−El(El⊤El)−1El⊤(A.39)\mathbf{E}_{l}^{\perp}\left(\left(\mathbf{E}_{l}^{\perp}\right)^{\top} \mathbf{E}_{l}^{\perp}\right)^{-1}\left(\mathbf{E}_{l}^{\perp}\right)^{\top}=\mathbf{I}-\mathbf{E}_{l}\left(\mathbf{E}_{l}^{\top} \mathbf{E}_{l}\right)^{-1} \mathbf{E}_{l}^{\top} \tag{A.39} El⊥((El⊥)⊤El⊥)−1(El⊥)⊤=I−El(El⊤El)−1El⊤(A.39)
用SVD分解可以很容易地从 El\mathbf{E}_{l}El 中计算出零空间的基。因为基是单位的，满足 (El⊥)⊤El⊥=I\left(\mathbf{E}_{l}^{\perp}\right)^{\top} \mathbf{E}_{l}^{\perp}=\mathbf{I}(El⊥)⊤El⊥=I 。将(A.39)代入(A.38)，并忆及 El\mathbf{E}_{l}El 为酉矩阵，可得:
∑l=1L∥El⊥(El⊥)⊤(FlδTX(l)−bl)∥2=∑l=1L(El⊥(El⊥)⊤(FlδTX(l)−bl))⊤(El⊥(El⊥)⊤(FlδTX(l)−bl))=∑l=1L(FlδTX(l)−bl)⊤El⊥(El⊥)⊤El⊥(El⊥)⊤(FlδTX(l)−bl)=∑l=1L(FlδTX(l)−bl)⊤El⊥(El⊥)⊤(FlδTX(l)−bl)=∑l=1L∥(El⊥)⊤(FlδTX(l)−bl)∥2(A.40)\begin{aligned} & \sum_{l=1}^{L}\left\|\mathbf{E}_{l}^{\perp}\left(\mathbf{E}_{l}^{\perp}\right)^{\top}\left(\mathbf{F}_{l} \delta \mathbf{T}_{\mathcal{X}(l)}-\mathbf{b}_{l}\right)\right\|^{2}=\sum_{l=1}^{L}\left(\mathbf{E}_{l}^{\perp}\left(\mathbf{E}_{l}^{\perp}\right)^{\top}\left(\mathbf{F}_{l} \delta \mathbf{T}_{\mathcal{X}(l)}-\mathbf{b}_{l}\right)\right)^{\top}\left(\mathbf{E}_{l}^{\perp}\left(\mathbf{E}_{l}^{\perp}\right)^{\top}\left(\mathbf{F}_{l} \delta \mathbf{T}_{\mathcal{X}(l)}-\mathbf{b}_{l}\right)\right) \\ =& \sum_{l=1}^{L}\left(\mathbf{F}_{l} \delta \mathbf{T}_{\mathcal{X}(l)}-\mathbf{b}_{l}\right)^{\top} \mathbf{E}_{l}^{\perp}\left(\mathbf{E}_{l}^{\perp}\right)^{\top} \mathbf{E}_{l}^{\perp}\left(\mathbf{E}_{l}^{\perp}\right)^{\top}\left(\mathbf{F}_{l} \delta \mathbf{T}_{\mathcal{X}(l)}-\mathbf{b}_{l}\right) \\ =& \sum_{l=1}^{L}\left(\mathbf{F}_{l} \delta \mathbf{T}_{\mathcal{X}(l)}-\mathbf{b}_{l}\right)^{\top} \mathbf{E}_{l}^{\perp}\left(\mathbf{E}_{l}^{\perp}\right)^{\top}\left(\mathbf{F}_{l} \delta \mathbf{T}_{\mathcal{X}(l)}-\mathbf{b}_{l}\right)=\sum_{l=1}^{L}\left\|\left(\mathbf{E}_{l}^{\perp}\right)^{\top}\left(\mathbf{F}_{l} \delta \mathbf{T}_{\mathcal{X}(l)}-\mathbf{b}_{l}\right)\right\|^{2} \end{aligned} \tag{A.40} ==l=1∑L∥∥El⊥(El⊥)⊤(FlδTX(l)−bl)∥∥2=l=1∑L(El⊥(El⊥)⊤(FlδTX(l)−bl))⊤(El⊥(El⊥)⊤(FlδTX(l)−bl))l=1∑L(FlδTX(l)−bl)⊤El⊥(El⊥)⊤El⊥(El⊥)⊤(FlδTX(l)−bl)l=1∑L(FlδTX(l)−bl)⊤El⊥(El⊥)⊤(FlδTX(l)−bl)=l=1∑L∥∥(El⊥)⊤(FlδTX(l)−bl)∥∥2(A.40)
这是我们视觉因子的另一种表示(A.38)，从计算的角度来看通常是可取的。在卡尔曼滤波体系结构中[6]使用了类似的零空间投影，而[2]给出了Schur补上的因子图视图。

补充材料：SO(3)的角速度和右雅可比

右雅可比矩阵 Jr(ϕ)\mathrm{J}_{r}(\phi)Jr(ϕ) (也称为body雅可比矩阵[7])将参数向量 ϕ˙\dot{\phi}ϕ˙ 的变化率与body的瞬时角速度 BωWB{ }_{\mathrm{B}} \boldsymbol{\omega}_{\mathrm{WB}}BωWB 联系起来:
BωWB=Jr(ϕ)ϕ˙(A.41){ }_{\mathrm{B}} \boldsymbol{\omega}_{\mathrm{WB}}=\mathrm{J}_{r}(\boldsymbol{\phi}) \dot{\boldsymbol{\phi}} \tag{A.41} BωWB=Jr(ϕ)ϕ˙(A.41)
右雅可比矩阵的闭合表达式在[3]中给出:
Jr(ϕ)=I−1−cos⁡(∥ϕ∥)∥ϕ∥2ϕ∧+∥ϕ∥−sin⁡(∥ϕ∥)∥ϕ3∥(ϕ∧)2(A.42)\mathrm{J}_{r}(\phi)=\mathbf{I}-\frac{1-\cos (\|\phi\|)}{\|\phi\|^{2}} \boldsymbol{\phi}^{\wedge}+\frac{\|\phi\|-\sin (\|\phi\|)}{\left\|\phi^{3}\right\|}\left(\phi^{\wedge}\right)^{2} \tag{A.42} Jr(ϕ)=I−∥ϕ∥21−cos(∥ϕ∥)ϕ∧+∥ϕ3∥∥ϕ∥−sin(∥ϕ∥)(ϕ∧)2(A.42)
注意，当 ϕ=0\phi=\mathbf{0}ϕ=0 时，雅可比矩阵变成单位矩阵。

考虑一个直接余弦矩阵 RWB(ϕ)∈SO(3)\mathrm{R}_{\mathrm{WB}}(\phi) \in \mathrm{SO}(3)RWB(ϕ)∈SO(3) ，它将一个点从物体坐标B旋转到世界坐标W，并通过旋转向量 ϕ\phiϕ 进行参数化。角速度和旋转矩阵的导数的关系是[7]：
RWB⊤R˙WB=BωWB∧(A.43)\mathrm{R}_{\mathrm{WB}}^{\top} \dot{\mathrm{R}}_{\mathrm{WB}}={ }_{\mathrm{B}} \boldsymbol{\omega}_{\mathrm{WB}}^{\wedge} \tag{A.43} RWB⊤R˙WB=BωWB∧(A.43)
因此，用(A.41)我们可以写出旋转矩阵在 ϕ\phiϕ 处的导数:
R˙WB(ϕ)=RWB(ϕ)(Jr(ϕ)ϕ˙)∧(A.44)\dot{\mathrm{R}}_{\mathrm{WB}}(\phi)=\mathrm{R}_{\mathrm{WB}}(\phi)\left(\mathrm{J}_{r}(\phi) \dot{\phi}\right)^{\wedge} \tag{A.44} R˙WB(ϕ)=RWB(ϕ)(Jr(ϕ)ϕ˙)∧(A.44)
给定群SO(3)中元素右侧的乘法扰动 Exp⁡(δψ)\operatorname{Exp}(\delta \psi)Exp(δψ) ，我们可能会问，在切空间 δϕ∈so(3)\delta \phi \in \mathfrak{s o}(3)δϕ∈so(3) 中导致相同复合旋转的等价加性扰动是什么:
Exp⁡(ϕ)Exp⁡(δψ)=Exp⁡(ϕ+δϕ)(A.45)\operatorname{Exp}(\boldsymbol{\phi}) \operatorname{Exp}(\delta \boldsymbol{\psi})=\operatorname{Exp}(\boldsymbol{\phi}+\delta \boldsymbol{\phi}) \tag{A.45} Exp(ϕ)Exp(δψ)=Exp(ϕ+δϕ)(A.45)
计算两边增量的导数，使用(A.44)，假设增量很小，发现:
δψ≈Jr(ϕ)δϕ(A.46)\delta \boldsymbol{\psi} \approx \mathrm{J}_{r}(\phi) \delta \boldsymbol{\phi} \tag{A.46} δψ≈Jr(ϕ)δϕ(A.46)
得到：
Exp⁡(ϕ+δϕ)≈Exp⁡(ϕ)Exp⁡(Jr(ϕ)δϕ)(A.47)\operatorname{Exp}(\phi+\delta \phi) \approx \operatorname{Exp}(\phi) \operatorname{Exp}\left(\mathrm{J}_{r}(\boldsymbol{\phi}) \delta \boldsymbol{\phi}\right) \tag{A.47} Exp(ϕ+δϕ)≈Exp(ϕ)Exp(Jr(ϕ)δϕ)(A.47)
类似的一阶近似也适用于对数:
[log⁡(Exp⁡(ϕ)Exp⁡(δϕ))≈ϕ+Jr−1(ϕ)δϕ(A.48)[\log (\operatorname{Exp}(\phi) \operatorname{Exp}(\delta \phi)) \approx \phi+\mathrm{J}_{r}^{-1}(\boldsymbol{\phi}) \delta \phi \tag{A.48} [log(Exp(ϕ)Exp(δϕ))≈ϕ+Jr−1(ϕ)δϕ(A.48)
这个性质直接遵循Baker-Campbell-Hausdor (BCH)公式，假设 δϕ\delta \phiδϕ 为小[1]。右雅可比矩阵逆的显式表达式在[3]中给出:
Jr−1(ϕ)=I+12ϕ∧+(1∥ϕ∥2+1+cos⁡(∥ϕ∥)2∥ϕ∥sin⁡(∥ϕ∥))(ϕ∧)2(A.49)\mathrm{J}_{r}^{-1}(\phi)=\mathbf{I}+\frac{1}{2} \phi^{\wedge}+\left(\frac{1}{\|\phi\|^{2}}+\frac{1+\cos (\|\phi\|)}{2\|\phi\| \sin (\|\phi\|)}\right)\left(\phi^{\wedge}\right)^{2} \tag{A.49} Jr−1(ϕ)=I+21ϕ∧+(∥ϕ∥21+2∥ϕ∥sin(∥ϕ∥)1+cos(∥ϕ∥))(ϕ∧)2(A.49)