Levenberg-Marquardt算法求解单应性矩阵

A. Levenberg-Marquardt算法

待估计的模型参数 x=[x1,x2,⋯,xn]T\mathbf{x}=[x_1, x_2, \cdots,x_n]^Tx=[x1,x2,⋯,xn]T

误差函数 fi(x)f_i( \mathbf{x})fi(x): 表示第iii个观测值与对应的模型的估计值的差

mmm：观测值的个数

目标函数 F(x)=12∑i=1m(fi(x))2F(\mathbf{x}) = \frac{1}{2} \displaystyle \sum^{m}_{i=1}(f_i(\mathbf{x}))^2F(x)=21i=1∑m(fi(x))2

最小二乘问题可以描述为
x∗=argminx{F(x)}\mathbf{x}^* = \underset{\mathbf{x}}{argmin}\{ F(\mathbf{x}) \} x∗=xargmin{F(x)}
一些用到的变量和公式
f(x)=[f1(x),⋯,fm(x)]TF(x)=12fT(x)f(x)=12∣∣f(x)∣∣2f′(x)=J(x)=[∂f1(x)∂x1⋯∂f1(x)∂xn⋮⋱⋮∂fm(x)∂x1⋯∂fm(x)∂xn]F′(x)=f′(x)f(x)=JT(x)f(x)\mathbf{f}(\mathbf{x})=[f_1(\mathbf{x}), \cdots,f_m(\mathbf{x})]^T \\ F(\mathbf{x}) = \displaystyle \frac{1}{2} \mathbf{f}^T(\mathbf{x}) \mathbf{f}(\mathbf{x}) = \frac{1}{2} ||\mathbf{f}(\mathbf{x})||^2 \\ \mathbf{f}^{'}(\mathbf{x}) = \mathbf{J}(\mathbf{x}) = \displaystyle \begin{bmatrix} \frac{\partial f_1(\mathbf{x})}{\partial x_1} &\cdots &\frac{\partial f_1(\mathbf{x})}{\partial x_n} \\ \vdots &\ddots &\vdots \\ \frac{\partial f_m(\mathbf{x})}{\partial x_1} &\cdots &\frac{\partial f_m(\mathbf{x})}{\partial x_n} \\ \end{bmatrix} \\ F^{'}(\mathbf{x}) = \mathbf{f}^{'}(\mathbf{x})\mathbf{f}(\mathbf{x}) = \mathbf{J}^T(\mathbf{x})\mathbf{f}(\mathbf{x}) \\ f(x)=[f1(x),⋯,fm(x)]TF(x)=21fT(x)f(x)=21∣∣f(x)∣∣2f′(x)=J(x)=⎣⎡∂x1∂f1(x)⋮∂x1∂fm(x)⋯⋱⋯∂xn∂f1(x)⋮∂xn∂fm(x)⎦⎤F′(x)=f′(x)f(x)=JT(x)f(x)

f(x)\mathbf{f}(\mathbf{x})f(x)在x\mathbf{x}x处一阶泰勒展开
f(x+h)≈f(x)+J(x)hwhere hsufficiently small\displaystyle \mathbf{f}(\mathbf{x} + \mathbf{h}) \approx \mathbf{f}(\mathbf{x}) + \mathbf{J}(\mathbf{x}) \mathbf{h}\\ \text{where} \ \mathbf{h}\ \text{sufficiently small} f(x+h)≈f(x)+J(x)hwhere h sufficiently small
于是有
F(x+h)=12fT(x+h)f(x+h)≈12(f(x)+J(x)h)T(f(x)+J(x)h)=F(x)+hTJT(x)f(x)+12hTJT(x)J(x)hwhere hsufficiently small\displaystyle \begin{align} F(\mathbf{x} + \mathbf{h}) &= \frac{1}{2} \mathbf{f}^T(\mathbf{x} + \mathbf{h}) \mathbf{f}(\mathbf{x}+ \mathbf{h}) \\ &\approx \frac{1}{2} \left( \mathbf{f}(\mathbf{x}) + \mathbf{J}(\mathbf{x}) \mathbf{h} \right)^T \left( \mathbf{f}(\mathbf{x}) + \mathbf{J}(\mathbf{x}) \mathbf{h} \right) \\ &= F(\mathbf{x}) + \mathbf{h}^T \mathbf{J}^T(\mathbf{x}) \mathbf{f}(\mathbf{x}) + \frac{1}{2} \mathbf{h}^T \mathbf{J}^T(\mathbf{x}) \mathbf{J}(\mathbf{x}) \mathbf{h} \\ \end{align} \\ \text{where} \ \mathbf{h}\ \text{sufficiently small} F(x+h)=21fT(x+h)f(x+h)≈21(f(x)+J(x)h)T(f(x)+J(x)h)=F(x)+hTJT(x)f(x)+21hTJT(x)J(x)hwhere h sufficiently small
令
L(h)=F(x)+hTJT(x)f(x)+12hTJT(x)J(x)hL(\mathbf{h}) = F(\mathbf{x}) + \mathbf{h}^T \mathbf{J}^T(\mathbf{x}) \mathbf{f}(\mathbf{x}) + \frac{1}{2} \mathbf{h}^T \mathbf{J}^T(\mathbf{x}) \mathbf{J}(\mathbf{x}) \mathbf{h} L(h)=F(x)+hTJT(x)f(x)+21hTJT(x)J(x)h
每一次的迭代的基本思路是，在x\mathbf{x}x处求
hlm=argminh{L(h)+P(μ,h)}\mathbf{h}_{lm} = \underset{\mathbf{h}}{\text{argmin}} \{L(\mathbf{h}) + P(\mu,\mathbf{h}) \} hlm=hargmin{L(h)+P(μ,h)}
其中 P(μ,h)P(\mathbf{\mu,h})P(μ,h)是惩罚项，是为了使得上式求得的hlm\mathbf{h}_{lm}hlm偏小些，并且可以通过调大μ\muμ来获得更小的hlm\mathbf{h}_{lm}hlm，或者调小μ\muμ来获得更大的hlm\mathbf{h}_{lm}hlm。常见的P(μ,h)P(\mu,\mathbf{h})P(μ,h)的形式有：
P(μ,h)=12μhThP(\mu, \mathbf{h}) = \frac{1}{2} \mu \mathbf{h}^T\mathbf{h} P(μ,h)=21μhTh

A.1 求解单应性矩阵的LM算法

n=8n = 8n=8，x\mathbf{x}x对应的是单应性矩阵里待解的8个未知量：
z⋅[uv1]=[x1x2x3x4x5x6x7x81]⋅[ab1]⇒{u=x1a+x2b+x3x7a+x8b+1v=x4a+x5b+x6x7a+x8b+1z \cdot \begin{bmatrix} u \\ v \\ 1 \\ \end{bmatrix} = \begin{bmatrix} x_1 &x_2 &x_3 \\ x_4 &x_5 &x_6 \\ x_7 &x_8 &1 \\ \end{bmatrix} \cdot \begin{bmatrix} a \\ b \\ 1 \\ \end{bmatrix} \\ \Rightarrow \begin{cases} \displaystyle u = \frac{x_1 a + x_2 b + x_3}{x_7 a + x_8 b + 1} \\ \displaystyle v = \frac{x_4 a + x_5 b + x_6}{x_7 a + x_8 b + 1} \\ \end{cases} z⋅⎣⎡uv1⎦⎤=⎣⎡x1x4x7x2x5x8x3x61⎦⎤⋅⎣⎡ab1⎦⎤⇒⎩⎨⎧u=x7a+x8b+1x1a+x2b+x3v=x7a+x8b+1x4a+x5b+x6
假设有sss个点对，那么m=2⋅sm=2 \cdot sm=2⋅s，因为一个点对产生两个误差函数，即对于第iii个点对，产生
f2i−1(x)=x1ai+x2bi+x3x7ai+x8bi+1−uif2i(x)=x4ai+x5bi+x6x7ai+x8bi+1−vif_{2i-1}(\mathbf{x}) = \frac{x_1 a_i + x_2 b_i + x_3}{x_7 a_i + x_8 b_i + 1} - u_i \\ f_{2i}(\mathbf{x}) = \frac{x_4 a_i + x_5 b_i + x_6}{x_7 a_i + x_8 b_i + 1} - v_i \\ f2i−1(x)=x7ai+x8bi+1x1ai+x2bi+x3−uif2i(x)=x7ai+x8bi+1x4ai+x5bi+x6−vi
对应的一阶导数是
f2i−1′(x)=[aix7ai+x8bi+1,bix7ai+x8bi+1,1x7ai+x8bi+1,0,0,0,−ai(x1ai+x2bi+x3)(x7ai+x8bi+1)2,−bi(x1ai+x2bi+x3)(x7ai+x8bi+1)2]Tf2i′(x)=[0,0,0,aix7ai+x8bi+1,bix7ai+x8bi+1,1x7ai+x8bi+1,−ai(x4ai+x5bi+x6)(x7ai+x8bi+1)2,−bi(x4ai+x5bi+x6)(x7ai+x8bi+1)2]Tf^{'}_{2i-1}(\mathbf{x}) = \left[ \frac{a_i}{x_7 a_i + x_8 b_i + 1},\frac{b_i}{x_7 a_i + x_8 b_i + 1},\frac{1}{x_7 a_i + x_8 b_i + 1},0,0,0,-\frac{a_i(x_1 a_i + x_2 b_i + x_3)}{(x_7 a_i + x_8 b_i + 1)^2},-\frac{b_i(x_1 a_i + x_2 b_i + x_3)}{(x_7 a_i + x_8 b_i + 1)^2} \right]^T \\ f^{'}_{2i}(\mathbf{x}) = \left[ 0,0,0,\frac{a_i}{x_7 a_i + x_8 b_i + 1},\frac{b_i}{x_7 a_i + x_8 b_i + 1},\frac{1}{x_7 a_i + x_8 b_i + 1},-\frac{a_i(x_4 a_i + x_5 b_i + x_6)}{(x_7 a_i + x_8 b_i + 1)^2},-\frac{b_i(x_4 a_i + x_5 b_i + x_6)}{(x_7 a_i + x_8 b_i + 1)^2} \right]^T \\ f2i−1′(x)=[x7ai+x8bi+1ai,x7ai+x8bi+1bi,x7ai+x8bi+11,0,0,0,−(x7ai+x8bi+1)2ai(x1ai+x2bi+x3),−(x7ai+x8bi+1)2bi(x1ai+x2bi+x3)]Tf2i′(x)=[0,0,0,x7ai+x8bi+1ai,x7ai+x8bi+1bi,x7ai+x8bi+11,−(x7ai+x8bi+1)2ai(x4ai+x5bi+x6),−(x7ai+x8bi+1)2bi(x4ai+x5bi+x6)]T
这里P(μ,h)P(\mu, \mathbf{h})P(μ,h)的形式如下所示
P(μ,h)=12μhTWhP(\mu, \mathbf{h}) = \frac{1}{2} \mu \mathbf{h}^T \mathbf{W} \mathbf{h} P(μ,h)=21μhTWh

W\mathbf{W}W是一个已知的对角矩阵，而且对角线元素都是大于0的。

令
ψ(h)=L(h)+12μhTWh\psi(\mathbf{h}) = L(\mathbf{h}) + \frac{1}{2} \mu \mathbf{h}^T \mathbf{W} \mathbf{h} ψ(h)=L(h)+21μhTWh
求解上式的最小值，即令
ψ′(h)=JT(x)f(x)+(JT(x)J(x)+μW)h=0\psi^{'}(\mathbf{h}) = \mathbf{J}^T(\mathbf{x}) \mathbf{f}(\mathbf{x}) + \left( \mathbf{J}^T(\mathbf{x}) \mathbf{J}(\mathbf{x}) + \mu \mathbf{W} \right) \mathbf{h} = 0 ψ′(h)=JT(x)f(x)+(JT(x)J(x)+μW)h=0
于是可得
hlm=−(A+μW)−1gA=JT(x)J(x)g=JT(x)f(x)\mathbf{h}_{lm} = - \left( \mathbf{A} + \mu \mathbf{W} \right)^{-1} \mathbf{g} \\ \mathbf{A} = \mathbf{J}^T(\mathbf{x}) \mathbf{J}(\mathbf{x}) \\ \mathbf{g} = \mathbf{J}^T(\mathbf{x}) \mathbf{f}(\mathbf{x}) hlm=−(A+μW)−1gA=JT(x)J(x)g=JT(x)f(x)

整个算法如下所示：

已知：x0\mathbf{x}_0x0, ξ1=1e−15\xi_1=1e^{-15}ξ1=1e−15，ξ2=1e−15\xi_2=1e^{-15}ξ2=1e−15，kmaxk_{\mathrm{max}}kmax

解法：

begin

k=0k = 0k=0

x=x0\mathbf{x} = \mathbf{x}_0x=x0

A=JT(x)J(x);g=JT(x)f(x)\mathbf{A} = \mathbf{J}^T(\mathbf{x}) \mathbf{J}(\mathbf{x});\quad \mathbf{g} = \mathbf{J}^T(\mathbf{x}) \mathbf{f}(\mathbf{x})A=JT(x)J(x);g=JT(x)f(x)

W=diag([a11,a22,⋯,ann]);μ=1;τ=0.75\mathbf{W} = diag([a_{11},a_{22}, \cdots,a_{nn}]); \quad \mu = 1; \quad \tau = 0.75W=diag([a11,a22,⋯,ann]);μ=1;τ=0.75

found = $ \left(||\mathbf{f}(\mathbf{x})|| < \xi_2 \right)$

while (not found) and (k<kmaxk < k_{\mathrm{max}}k<kmax)

k=k+1k = k + 1k=k+1

Solve (A+μW)hlm=−g\left( \mathbf{A} + \mu \mathbf{W} \right) \mathbf{h}_{lm} = -\mathbf{g}(A+μW)hlm=−g

xnew=x+hlm\mathbf{x}_{\mathrm{new}} = \mathbf{x} + \mathbf{h}_{lm}xnew=x+hlm

ρ=F(x)−F(xnew)L(0)−L(hlm)\rho = \displaystyle \frac{F(\mathbf{x}) - F(\mathbf{x}_{\mathrm{new}})}{L(\mathbf{0}) - L(\mathbf{h}_{lm})}ρ=L(0)−L(hlm)F(x)−F(xnew)

if ρ>0.75\rho > 0.75ρ>0.75

μ=μ/2\mu = \mu /2μ=μ/2

if μ<τ\mu < \tauμ<τ

μ=0\mu = 0μ=0

elseif ρ<0.25\rho < 0.25ρ<0.25

ν=min{max{2×(1−F(xnew)−F(x)hlmTg),2},10}\nu = \mathrm{min} \left\{ \mathrm{max} \left\{ 2 \times \left( 1 - \displaystyle \frac{F(\mathbf{x}_{\mathrm{new}}) - F(\mathbf{x})}{\mathbf{h}^T_{lm} \mathbf{g}} \right),2 \right\},10 \right\}ν=min{max{2×(1−hlmTgF(xnew)−F(x)),2},10}

if μ==0\mu == 0μ==0

B=A−1\mathbf{B} = \mathbf{A}^{-1}B=A−1

temp=max{[b11,⋯,bnn]}temp = \mathrm{max}\{ [b_{11},\cdots,b_{nn}] \}temp=max{[b11,⋯,bnn]}

μ=1/temp;τ=1/temp\mu = 1/temp; \quad \tau = 1/tempμ=1/temp;τ=1/temp

ν=ν/2\nu = \nu / 2ν=ν/2

μ=μ⋅ν\mu = \mu \cdot \nuμ=μ⋅ν

if F(xnew)<F(x)F(\mathbf{x}_{\mathrm{new}}) < F(\mathbf{x})F(xnew)<F(x)

x=xnew\mathbf{x} = \mathbf{x}_{\mathrm{new}}x=xnew

A=JT(x)J(x);g=JT(x)f(x)\mathbf{A} = \mathbf{J}^T(\mathbf{x}) \mathbf{J}(\mathbf{x});\quad \mathbf{g} = \mathbf{J}^T(\mathbf{x}) \mathbf{f}(\mathbf{x})A=JT(x)J(x);g=JT(x)f(x)

found = $ \left(||\mathbf{h}_{lm}|| < \xi_1 \ \mathrm{or} \ ||\mathbf{f}(\mathbf{x})|| < \xi_2 \right)$

end