VSLAM：：[手写VIO_课堂笔记]第三讲(上)_基于优化的IMU与视觉信息融合(最小二乘问题)

1. 第三讲(上)_基于优化的IMU与视觉信息融合(上)

第三讲(上):
(1)最小二乘问题的求解推导
(2)其中有LM算法的相关推导以及鲁棒核函数的推导

1.1. 最小二乘与非线性优化

1.1.1. 最小二乘

1.1.2. 非线性最小二乘

1.1.2.1. LM算法增量$\Delta X$表达式推导

求解如下:
$$\begin{array}{rl}& ({\mathbf{J}}^T\mathbf{J}+\mu \mathbf{I})\mathbf{\Delta }\mathbf{X}=-{\mathbf{J}}^T\mathbf{f}\\ & ⟹(\mathbf{V}\mathbf{\Lambda }{\mathbf{V}}^{\mathbf{T}}+\mu \mathbf{I})\mathbf{\Delta }\mathbf{X}=-{\mathbf{J}}^T\mathbf{f}\\ & ⟹(\mathbf{V}(\mathbf{\Lambda }+\mu \mathbf{I}){\mathbf{V}}^T)\mathbf{\Delta }\mathbf{X}=-{\mathbf{J}}^T\mathbf{f}\\ & ⟹(\mathbf{\Lambda }+\mu \mathbf{I}){\mathbf{V}}^T\mathbf{\Delta }\mathbf{X}=-{\mathbf{V}}^T{\mathbf{J}}^T\mathbf{f}\\ & ⟹(\mathbf{\Lambda }+\mu \mathbf{I}){\mathbf{V}}^T\mathbf{\Delta }\mathbf{X}=-{\mathbf{V}}^T{F}^{{}^{\prime }}(x{)}^T\\ & ⟹{\mathbf{V}}^T\mathbf{\Delta }\mathbf{X}=-(\mathbf{\Lambda }+\mu \mathbf{I}{)}^{-1}{\mathbf{V}}^T{F}^{{}^{\prime }}(x{)}^T\end{array}$$

$$\begin{array}{r}{\mathbf{V}}^T\mathbf{\Delta }\mathbf{X}=-\left[\begin{array}{cccc}\frac{1}{{\lambda }_1+\mu }& 0& 0& \cdots \\ 0& \frac{1}{{\lambda }_2+\mu }& 0& \cdots \\ 0& \cdots & & \frac{1}{{\lambda }_j+\mu }\end{array}\right]{\mathbf{V}}^T{F}^{{}^{\prime }}(x{)}^T\end{array}$$

$$\begin{array}{r}\mathbf{\Delta }\mathbf{X}=-\left[\begin{array}{ccc}{\mathbf{v}}_1& \cdots & {\mathbf{v}}_{\mathbf{j}}\end{array}\right]\left[\begin{array}{cccc}\frac{1}{{\lambda }_1+\mu }& 0& 0& \cdots \\ 0& \frac{1}{{\lambda }_2+\mu }& 0& \cdots \\ 0& \cdots & & \frac{1}{{\lambda }_j+\mu }\end{array}\right]\left[\begin{array}{c}{\mathbf{v}}_1\\ ⋮\\ {\mathbf{v}}_{\mathbf{j}}\end{array}\right]F^{{}^{\prime }}(x{)}^T\end{array}$$

$$\begin{array}{r}\therefore \mathbf{\Delta }\mathbf{X}=-\sum _{j=1}^n\frac{{\mathbf{v}}_j^T{{\mathbf{F}}^{\prime }}^T}{{\lambda }_j+\mu }{\mathbf{v}}_j\end{array}$$

---

从上图可以看到,由于$\mu$的值在抖动,使得$\Delta X$的步长在来回震荡变化,使得loss值也随着抖动,有一部分的计算是多余的.

1.1.2.2. 鲁棒核函数相关推导