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

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

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

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

1.1.1. 最小二乘

1.1.2. 非线性最小二乘

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

求解如下:
$\begin{aligned} &(\boldsymbol{J}^T\boldsymbol{J}+\mu\boldsymbol{I})\boldsymbol{\Delta X}=-\boldsymbol{J}^T\boldsymbol{f} \\ &\Longrightarrow (\boldsymbol{V \Lambda V^T}+\mu \boldsymbol{I})\boldsymbol{\Delta X}=-\boldsymbol{J}^T\boldsymbol{f} \\ &\Longrightarrow(\boldsymbol{V} (\boldsymbol{\Lambda}+\mu \boldsymbol{I})\boldsymbol{V}^T) \boldsymbol{\Delta X}=-\boldsymbol{J}^T\boldsymbol{f} \\ &\Longrightarrow(\boldsymbol{\Lambda}+\mu \boldsymbol{I})\boldsymbol{V}^T\boldsymbol{\Delta X}=-\boldsymbol{V}^T\boldsymbol{J}^T\boldsymbol{f} \\ &\Longrightarrow(\boldsymbol{\Lambda}+\mu \boldsymbol{I})\boldsymbol{V}^T\boldsymbol{\Delta X}=-\boldsymbol{V}^T F^{'}(x)^T \\ &\Longrightarrow\boldsymbol{V}^T\boldsymbol{\Delta X}=-(\boldsymbol{\Lambda}+\mu \boldsymbol{I})^{-1}\boldsymbol{V}^T F^{'}(x)^T \\ \end{aligned}$​(JTJ+μI)ΔX=−JTf⟹(VΛVT+μI)ΔX=−JTf⟹(V(Λ+μI)VT)ΔX=−JTf⟹(Λ+μI)VTΔX=−VTJTf⟹(Λ+μI)VTΔX=−VTF′(x)T⟹VTΔX=−(Λ+μI)−1VTF′(x)T​

$\begin{aligned} \boldsymbol{V}^T\boldsymbol{\Delta X}=- \begin{bmatrix} \frac{1}{\lambda_1+\mu}&0&0& \cdots \\ 0 & \frac{1}{\lambda_2+\mu} & 0 &\cdots \\ 0 & \cdots & & \frac{1}{\lambda_j+\mu} \end{bmatrix} \boldsymbol{V}^T F^{'}(x)^T \end{aligned}$VTΔX=−⎣⎢⎡​λ1​+μ1​00​0λ2​+μ1​⋯​00​⋯⋯λj​+μ1​​⎦⎥⎤​VTF′(x)T​

$\begin{aligned} \boldsymbol{\Delta X}=- \begin{bmatrix} \boldsymbol{v_1} & \cdots &\boldsymbol{v_j} \end{bmatrix} \begin{bmatrix} \frac{1}{\lambda_1+\mu}&0&0& \cdots \\ 0 & \frac{1}{\lambda_2+\mu} & 0 &\cdots \\ 0 & \cdots & & \frac{1}{\lambda_j+\mu} \end{bmatrix} \begin{bmatrix} \boldsymbol{v_1} \\ \vdots \\ \boldsymbol{v_j} \end{bmatrix} F^{'}(x)^T \end{aligned}$ΔX=−[v1​​⋯​vj​​]⎣⎢⎡​λ1​+μ1​00​0λ2​+μ1​⋯​00​⋯⋯λj​+μ1​​⎦⎥⎤​⎣⎢⎡​v1​⋮vj​​⎦⎥⎤​F′(x)T​

$\begin{aligned} \therefore \boldsymbol{\Delta X}= -\sum_{j=1}^{n}\frac{\boldsymbol{v}_j^T {\boldsymbol{F'}}^T}{\lambda_j+\mu} \boldsymbol{v}_j \end{aligned}$∴ΔX=−j=1∑n​λj​+μvjT​F′T​vj​​

---

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

1.1.2.2. 鲁棒核函数相关推导