《视觉SLAM十四讲》学习笔记-非线性最小二乘问题

最新推荐文章于 2024-02-15 13:07:45 发布

teddyluo

最新推荐文章于 2024-02-15 13:07:45 发布

阅读量1k

点赞数

分类专栏： slam

slam 专栏收录该内容

20 篇文章 17 订阅

订阅专栏

非线性最小二乘问题：

min x ⃗ 1 2 ‖ f (x ⃗) ‖ 22

$\underset{\vec{x}}{\min}\frac{1}{2}\|f(\vec{x})\|_2^2$
其 迭代解步骤为：
1) 给定初值

x⃗ 0 x → 0 $\vec{x}_0$ ;
2) 对

k k $k$ 次迭代，寻找增量

△ {\vec{x}}_{k}

$\triangle\vec{x}_k$ , 使得

‖f(x⃗ k+△x⃗ k)‖22 ‖ f ( x → k + △ x → k ) ‖ 2 2 $\|f(\vec{x}_k + \triangle\vec{x}_k)\|^2_2$ ；
3) 若

△x⃗ k △ x → k $\triangle\vec{x}_k$ 足够小，则停止迭代；
4) 否则令

x⃗ k+1=x⃗ k+△x⃗ k x → k + 1 = x → k + △ x → k $\vec{x}_{k+1} = \vec{x}_{k} + \triangle\vec{x}_k$ ,返回2).

牛顿法

为方便讨论，将最小问题在 $x$ 附近泰勒展开：

‖ f (\vec{x} + △ \vec{x}) ‖_{2}^{2} \approx ‖ f (\vec{x}) ‖_{2}^{2} + J △ \vec{x} + \frac{1}{2} △ {\vec{x}}^{⊤} H △ \vec{x}

$\|f(\vec{x} + \triangle\vec{x})\|^2_2 \approx \|f(\vec{x})\|^2_2 + \mathbf{J}\triangle\vec{x} + \frac{1}{2}\triangle\vec{x}^\top \mathbf{H}\triangle\vec{x}$
式中

J J $\mathbf{J}$ 为Jacobi矩阵，而

H H $\mathbf{H}$ 为Hessian矩阵。
如果只保留一阶梯度，而增量方向为:

△ x ⃗ * = - J ⊤ (x ⃗)

$\triangle\vec{x}^* = - \mathbf{J}^\top(\vec{x})$
上式若加上一个步长

λ λ $\lambda$ , 则为最速下降算法.

若保留到二阶梯度，则增量方程为：

△ x ⃗ * = arg min ‖ f (x ⃗) ‖ 22 + J (x ⃗) △ x ⃗ + 1 2 △ x ⃗ ⊤ H △ x ⃗

$\triangle\vec{x}^* = \arg\min \|f(\vec{x})\|^2_2 + \mathbf{J}(\vec{x})\triangle\vec{x}+\frac{1}{2}\triangle\vec{x}^\top\mathbf{H}\triangle\vec{x}$
对上式求导并令其为0，则增量的解为：

H △ x ⃗ = - J ⊤

$\mathbf{H}\triangle\vec{x}=-\mathbf{J}^\top$
上试称为 牛顿法。牛顿法需要求解二阶Hessian矩阵，在求解规模较大时非常困难。

Gauss-Newton

Gauss-Newton的思想是将 $f(\vec{x})$ 进行一阶泰勒展开：

f (x ⃗ + △ x ⃗) \approx f (x ⃗) + J (x ⃗) △ x ⃗

$f(\vec{x}+\triangle\vec{x}) \approx f(\vec{x}) + \mathbf{J}(\vec{x}) \triangle\vec{x}$
Gauss-Newton的目标是寻找一个

△x⃗ △ x → $\triangle\vec{x}$ ,使得

‖f(x⃗ +△x⃗ )‖2 ‖ f ( x → + △ x → ) ‖ 2 $\| f(\vec{x} + \triangle\vec{x}) \|^2$ 最小。所以需要解一个线性最小二乘问题：

△ x ⃗ * = arg min △ x ⃗ 1 2 ‖ f (x ⃗ + J △ x ⃗) ‖ 2

$\triangle\vec{x}^* = \arg\underset{\triangle\vec{x}}{\min} \frac{1}{2}\| f(\vec{x} + \mathbf{J}\triangle\vec{x} ) \|^2$
将右边平方项展开并令其导数为0，则可得到：

2 J ⊤ f (x ⃗) + 2 J (x ⃗) ⊤ J (x ⃗) △ x ⃗ = 0 ⃗

$2\mathbf{J}^\top f(\vec{x}) + 2\mathbf{J}(\vec{x})^\top\mathbf{J}(\vec{x})\triangle\vec{x}=\vec{0}$
解得方程组为：

J (x ⃗) ⊤ J (x ⃗) △ x ⃗ = - J (x ⃗) ⊤ f (x ⃗)

$\mathbf{J}(\vec{x})^\top\mathbf{J}(\vec{x})\triangle\vec{x} = - \mathbf{J}(\vec{x})^\top f(\vec{x})$
此为Gauss-Newton方程。左边系数记为

H H $\mathbf{H}$ ,右边记为

g⃗ g → $\vec{g}$ ,变换为：

H △ x ⃗ = g ⃗

$\mathbf{H} \triangle\vec{x} = \vec{g}$
可见 Gauss-Newton用 $\mathbf{J}(\vec{x})^\top\mathbf{J}(\vec{x})$ 作为二阶Hessian矩阵的近似。

Levenberg-Marquadt法

Levenberg-Marquadt方法：给 $\triangle\vec{x}$ 添加信赖区域(trust region)，信赖区域通过下式确定：

ρ = f ( x ⃗ + △ x ⃗ ) - f ( x ⃗ ) J ( x ⃗ ) △ x ⃗

$\rho=\frac{f(\vec{x}+\triangle\vec{x}) -f(\vec{x}) }{\mathbf{J}(\vec{x})\triangle \vec{x}}$
于是第

k k $k$ 次迭代公式变为：

min_{△ {\vec{x}}_{k}} \frac{1}{2} ‖ f ({\vec{x}}_{k}) + J ({\vec{x}}_{k}) △ {\vec{x}}_{k} ‖^{2}, s . t . ‖ D △ {\vec{x}}_{k} ‖^{2} \leq μ

$\underset{\triangle\vec{x}_k}{\min } \frac{1}{2}\| f(\vec{x}_k)+\mathbf{J}(\vec{x}_k)\triangle\vec{x}_k \|^2, ~~s.t.~~\|\mathbf{D}\triangle\vec{x}_k\|^2\leq \mu$
其中

μ μ $\mu$ 为信赖区域的半径，

D D $\mathbf{D}$ 若为单位矩阵

I I $\mathbf{I}$ ,则

△x⃗ △ x → $\triangle\vec{x}$ 将约束在一个球中；实际上常将

D D $\mathbf{D}$ 取成一个非负数对角阵。

为解L-M问题，用Lagrange乘子转化为无约束问题：

min △ x ⃗ k 1 2 ‖ f (x ⃗ k) + J (x k) △ x k ‖ 2 + 1 2 λ ‖ D △ x ⃗ ‖ 2

$\underset{\triangle\vec{x}_k}{\min} \frac{1}{2}\| f(\vec{x}_k) + \mathbf{J}(\mathbf{x}_k)\triangle\mathbf{x}_k \|^2 + \frac{1}{2}\lambda \|\mathbf{D}\triangle\vec{x}\|^2$
类似于Gauss-Newton展开可得：