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

A. Levenberg-Marquardt算法

待估计的模型参数 $\mathbf{x}=[x_1,x_2,\cdots ,x_n{]}^T$

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

$m$：观测值的个数

目标函数 $F(\mathbf{x})=\frac{1}{2}\sum _{i=1}^m(f_i(\mathbf{x}){)}^2$

最小二乘问题可以描述为
x ∗ = a r g m i n x { F ( x ) } \mathbf{x}^* = \underset{\mathbf{x}}{argmin}\{ F(\mathbf{x}) \} x∗=xargmin​{F(x)}
一些用到的变量和公式
$$\begin{gathered} \mathbf{f}(\mathbf{x})=[f_1(\mathbf{x}),\cdots ,f_m(\mathbf{x}){]}^T \\ F(\mathbf{x})=\frac{1}{2}{\mathbf{f}}^T(\mathbf{x})\mathbf{f}(\mathbf{x})=\frac{1}{2}||\mathbf{f}(\mathbf{x})|{|}^2 \\ {\mathbf{f}}^{{}^{\prime }}(\mathbf{x})=\mathbf{J}(\mathbf{x})=\left[\begin{array}{ccc}\frac{\partial f_1(\mathbf{x})}{\partial x_1}& \cdots & \frac{\partial f_1(\mathbf{x})}{\partial x_n}\\ ⋮& \ddots & ⋮\\ \frac{\partial f_m(\mathbf{x})}{\partial x_1}& \cdots & \frac{\partial f_m(\mathbf{x})}{\partial x_n}\end{array}\right] \\ {F}^{{}^{\prime }}(\mathbf{x})={\mathbf{f}}^{{}^{\prime }T}(\mathbf{x})\mathbf{f}(\mathbf{x})={\mathbf{J}}^T(\mathbf{x})\mathbf{f}(\mathbf{x}) \end{gathered}$$

$\mathbf{f}(\mathbf{x})$在$\mathbf{x}$处一阶泰勒展开
$$\begin{gathered} \mathbf{f}(\mathbf{x}+\mathbf{h})\approx \mathbf{f}(\mathbf{x})+\mathbf{J}(\mathbf{x})\mathbf{h} \\ \text{where}\mathbf{h}\text{sufficiently small} \end{gathered}$$
于是有
$$\begin{gathered} \begin{array}{rl}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{array} \\ \text{where}\mathbf{h}\text{sufficiently small} \end{gathered}$$
令
$$L(\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}$$
每一次的迭代的基本思路是，在$\mathbf{x}$处求
h l m = argmin h { 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(\mu ,\mathbf{h})$是惩罚项，是为了使得上式求得的${\mathbf{h}}_{lm}$偏小些，并且可以通过调大$\mu$来获得更小的${\mathbf{h}}_{lm}$，或者调小$\mu$来获得更大的${\mathbf{h}}_{lm}$。常见的$P(\mu ,\mathbf{h})$的形式有：
$$P(\mu ,\mathbf{h})=\frac{1}{2}\mu {\mathbf{h}}^T\mathbf{h}$$

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

$n=8$，$\mathbf{x}$对应的是单应性矩阵里待解的8个未知量：
$$\begin{gathered} z\cdot \left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}{x}_1& x_2& x_3\\ x_4& x_5& x_6\\ x_7& x_8& 1\end{array}\right]\cdot \left[\begin{array}{c}a\\ b\\ 1\end{array}\right] \\ \Rightarrow \{\begin{array}{l}u=\frac{x_{1}a+x_{2}b+x_3}{x_{7}a+x_{8}b+1}\\ v=\frac{x_{4}a+x_{5}b+x_6}{x_{7}a+x_{8}b+1}\end{array} \end{gathered}$$
假设有$s$个点对，那么$m=2\cdot s$，因为一个点对产生两个误差函数，即对于第$i$个点对，产生
$$\begin{gathered} f_{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 \end{gathered}$$
对应的一阶导数是
$$\begin{gathered} f_{2i-1}^{{}^{\prime }}(\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}^{{}^{\prime }}(\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 \end{gathered}$$
这里$P(\mu ,\mathbf{h})$的形式如下所示
$$P(\mu ,\mathbf{h})=\frac{1}{2}\mu {\mathbf{h}}^T\mathbf{Wh}$$

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

令
$$\psi (\mathbf{h})=L(\mathbf{h})+\frac{1}{2}\mu {\mathbf{h}}^T\mathbf{Wh}$$
求解上式的最小值，即令
$${\psi }^{{}^{\prime }}(\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$$
于是可得
$$\begin{gathered} {\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}) \end{gathered}$$

整个算法如下所示：

已知：${\mathbf{x}}_0$, ${\xi }_1=1e^{-15}$，${\xi }_2=1e^{-15}$，$k_{\max }$

解法：

begin

$k=0$

$\mathbf{x}={\mathbf{x}}_0$

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

$\mathbf{W}=diag([a_{11},a_{22},\cdots ,a_{nn}]);\mu =1;\tau =0.75$

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

while (not found) and ($k<k_{\max }$)

$k=k+1$

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

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

$\rho =\frac{F(\mathbf{x})-F({\mathbf{x}}_{\mathrm{new}})}{L(0)-L({\mathbf{h}}_{lm})}$

if $\rho >0.75$

$\mu =\mu /2$

if $\mu <\tau$

$\mu =0$

elseif $\rho <0.25$

$\nu =\min \left\{\max \left\{2\times \left(1-\frac{F({\mathbf{x}}_{\mathrm{new}})-F(\mathbf{x})}{{\mathbf{h}}_{lm}^T\mathbf{g}}\right),2\right\},10\right\}$

if $\mu ==0$

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

$temp=\max \{[b_{11},\cdots ,b_{nn}]\}$

$\mu =1/temp;\tau =1/temp$

$\nu =\nu /2$

$\mu =\mu \cdot \nu$

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

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

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

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

end