三维重建中的Levenberg-Marquardt算法

mypro334

本人数学基础薄弱，在学习三维重建的过程中，我遇到了一个棘手的问题：重投影公式是如何从：
x^*=arg\min_{x}\sum_{i=1}^{k}\left \| f_i\left ( x \right )  \right \|  ^2\tag{1} \\
变为： \delta ^*=arg\min_{\delta}\left \| J\left ( x \right )\delta+f  \left ( x \right )\right \| ^2+\lambda \left \|D\left ( x \right )  \delta \right \|^2 \tag{2}\\ 《视觉SLAM十四讲》的6.2.3中也介绍了三维重建中的Levenberg-Marquardt算法，但本人没有看明白。
通过在网上搜索了相关资料和阅读相应教材和文献，我将根据自己的理解解释这个算法。
一、"Multicore bundle adjustment"第二章"Theoretical Background"原文

首先，让我们回到"Multicore bundle adjustment"的章节原文中。

Given a set of measured image feature locations and correspondences, the goal of bundle adjustment is to find 3D point positions and camera parameters that minimize the reprojection error [13]. This optimization problem is usually formulated as a non-linear least squares problem, where the error is the squared L_2 norm of the difference between the observed feature location and the projection of the corresponding 3D point on the image plane of the camera.
Let x  be a vector of parameters and f\left( x \right)=\left[ f_1\left( x \right),...,f_k\left( x \right) \right] be the vector of residuals/reprojection errors for a 3D reconstruction. Then the optimization problem we wish to solve is the non-linear least squares problem:
x^*=arg\min_{x}\sum_{i=1}^{k}\left \| f_i\left ( x \right )  \right \|  ^2 \tag{1}  \\The Levenberg-Marquardt (LM) algorithm [11] is the most popular algorithm for solving non-linear least squares problems, and is the algorithm of choice for bundle adjustment. LM operates by solving a series of regularized linear approximations to the original nonlinear problem. Let J\left( x \right) be the Jacobian of f  \left ( x \right ), then in each iteration LM solves a linear least squares problem of the form
\delta ^*=arg\min_{\delta}\left \| J\left ( x \right )\delta+f  \left ( x \right )\right \| ^2+\lambda \left \|D\left ( x \right )  \delta \right \|^2 \tag{2}\\and updates x\longleftarrow x+\delta ^* \space  if \left \| f\left ( x+\delta ^* \right )  \right \| <\left \| f\left ( x \right )  \right \| . Here, D\left( x \right) is a non-negative diagonal matrix, typically the square root of the diagonal of the matrix J\left( x \right)^\top J\left( x \right) and \lambda  is a nonnegative parameter that controls the strength of regularization. The regularization is needed to ensure a convergent algorithm. LM updates the value of \lambda at each step based on how well the Jacobian J\left( x \right) approximates f  \left ( x \right ) [11].

原文翻译如下：
给定一组测量的图像特征位置和对应关系，光束法平差的目标是找到使重投影误差[13]最小的三维点位置和摄像机参数。该优化问题通常被表述为非线性最小二乘问题，其中误差为观测到的特征位置与对应的3D点在相机成像平面上的投影之差的平方范数。
令x 为参数向量，f\left( x \right)=\left[ f_1\left( x \right),...,f_k\left( x \right) \right]为残差/三维重建的重投影误差向量。那么我们要解决的优化问题就是非线性最小二乘问题:
x^*=arg\min_{x}\sum_{i=1}^{k}\left \| f_i\left ( x \right )  \right \|  ^2 \tag{1}\\
莱文贝格－马夸特（Levenberg-Marquardt, LM）算法[11]是求解非线性最小二乘问题最常用的算法，是束调整的首选算法。LM的工作原理是对原始非线性问题进行一系列正则化线性逼近。令J\left( x \right)为f  \left ( x \right )的雅可比矩阵，则则每次迭代LM都要解决如下形式的线性最小二乘问题：
\delta ^*=arg\min_{\delta}\left \| J\left ( x \right )\delta+f  \left ( x \right )\right \| ^2+\lambda \left \|D\left ( x \right )  \delta \right \|^2 \tag{2}\\并且更新 x\longleftarrow x+\delta ^* \space ，如果 \left \| f\left ( x+\delta ^* \right )  \right \| <\left \| f\left ( x \right )  \right \|。此处， D\left( x \right) 是一个非负对角线矩阵，通常是矩阵J\left( x \right)^\top J\left( x \right)对角线的平方根，\lambda是一个控制正则化强度的非负参数。正则化是保证算法收敛的必要条件。LM根据雅可比矩阵J\left( x \right)与f  \left ( x \right )的近似程度更新\lambda每一步的值。
二、算法脉络

此处关键就是LM算法这个最优化算法，原文在公式2后的方法部分是简单的矩阵运算，各位可以很容易推导，本文将不赘述。要理解LM算法就必须先简单了解最速下降法、牛顿法和高斯-牛顿法。它们四者的关系是：最速下降法是一阶梯度法；牛顿迭代法是二阶梯度法；高斯-牛顿法是在最小二乘问题中对牛顿迭代法的改进；LM算法则是对高斯-牛顿法的改进。
接下来，我将先说明一维函数下的最速下降法到牛顿法的演变过程。然后，说明从一维函数到多维函数组时牛顿法的演变过程。最后说明在多维函数组下的牛顿法到L-M法的演变过程。整个过程将不涉及深入的数学证明如：牛顿法收敛性的证明等。有兴趣的读者可以参考《Numerical optimization》。
2.1 最速下降法（一阶梯度）（一维函数）

最优化问题一般是求目标函数的最小值。对于优化目标函数：
min f\left( x \right)\tag{3}\\
最直观的方法是利用目标函数 f\left( x \right) 的一阶导数为零这一公式求出当f\left( x \right)达到极值时对应的若干组解。
{f}'  \left ( x \right )=0\tag{4}\\
然而当函数f\left( x \right)为复杂的非线性函数时，其关于x的一阶导数是十分复杂的，多数情况公式（4）难以求解，或难以求得全部解，即难以通过公式（4）获得函数f\left( x \right)的全局特性，因此自然无法通过比较所有极值点大小解得目标函数的最小值。
我们可以转换思路，在给定x时，求得f\left( x \right)的各阶导数值是比较容易的（假设其存在），即我们可以很容易获得目标函数f\left( x \right)的局部特性。那么问题就转化为：如何利用目标函数的局部特性求解目标函数的最小值？
最直观的方法就是：给定一个初始值 x_0 ，然后每一次都向使目标函数值更小的方向前进一个\bigtriangleup 增量，直到目标函数值不再下降。对应的算法流程为：

输入：初始值 x_0 ；终止阈值 \varepsilon ；目标函数 f(.)
输出：目标函数较小值  f_{min}
1.   k=0  
2.  寻找增量\Delta_0，使 f\left( x_0+\Delta_0 \right) 达到极小值
3.  while \Delta_k > \varepsilon
4.       x_{k+1}=x_k+\Delta_k
5.       k=k+1
6.      寻找增量\Delta_k，使 f\left( x_k+\Delta_k \right) 达到极小值
7.  end
8.   f_{min}=f\left( x_k \right)

值得注意的是，因为我们只考察了f\left( x \right)的局部特性，没有也几乎不可能对f\left( x \right)的全局特性进行考察。这种方法找到的是f\left( x \right)较小值，不一定是f\left( x \right)的最小值。另外当\bigtriangleup < \varepsilon时（终止阈值通常非常小，例如 \varepsilon=10^{-6}），说明此时\bigtriangleup 只有在微小变化时才能使f\left( x \right)变小，\bigtriangleup 稍微大的改变都会使f\left( x \right)增大，也就是说f\left( x \right)几乎到达了一个极值点附近（类似谷底），因此此时算法可以停止。
如何获得增量\bigtriangleup 就是上述方法的核心关键点，后续的最速下降法、牛顿迭代法等都是围绕这一问题进行讨论。
最速下降法的核心就是利用泰勒公式，将一阶泰勒展开式近似非线性函数f\left( x \right)，即：
f\left ( x_0+\bigtriangleup  \right ) \approx f\left ( x_0 \right ) +{f}'\left ( x_0 \right )\bigtriangleup \tag{5}\\ 其中 \bigtriangleup=x-x_0 。由于x_0 已知，所以 f\left ( x_0 \right )  和 {f}'\left ( x_0 \right )  都是确定数值，目标函数被近似为以\bigtriangleup为变量的一次函数，取\bigtriangleup为一阶梯度的反向就一定可以保证函数值下降，因此：
\bigtriangleup = -{f}'\left( x \right)\tag{6}\\
通常我们还会在此基础上加入步长\lambda 使得：
\bigtriangleup =-\lambda{f}'\left( x \right)\tag{7}\\
缺点：该方法过于贪心，容易走出锯齿线，反而增加迭代次数。
最速下降法对应的算法流程为：

输入：输入：初始值 x_0 ；终止阈值 \varepsilon ；目标函数 f(.)
输出：目标函数较小值  f_{min}
1.  k=0
2.  \bigtriangleup_0 =-\lambda{f}'\left( x_0 \right)
3.  while \Delta_k > \varepsilon
4.       x_{k+1}=x_k+\Delta_k
5.       k=k+1
6.      \bigtriangleup_k =-\lambda{f}'\left( x_k \right)
7.  end
8. f_{min}=f\left( x_k \right)

2.2 牛顿迭代法（二阶梯度）（一维函数）

牛顿迭代法则是将二阶泰勒展开式近似非线性函数f\left( x \right)，即：
{f}\left ( x_0+\bigtriangleup  \right ) \approx f\left( x_0 \right)+\bigtriangleup {f}'\left ( x_0 \right ) +\frac{1}{2}\bigtriangleup^2{f}''\left ( x_0 \right )  \tag{8}\\ 同理，目标函数被近似为以\bigtriangleup为变量的二次函数，且二次项系数为正，因此该函数拥有最小值。该函数取得最小值的条件就是该函数对\bigtriangleup的导数为零。即：
\frac{\partial \left ( f\left( x_0 \right)+\bigtriangleup {f}'\left ( x_0 \right ) +\frac{1}{2}\bigtriangleup^2{f}''\left ( x_0 \right ) \right ) }{\partial \triangle } =0\tag{9}\\ 解得：
\triangle =-\frac{{f}'\left ( x_0 \right ) }{{f}''\left ( x_0 \right ) } \tag{10}\\ 缺点：该方法过于需要求二阶导，计算量较大。
牛顿迭代法对应的算法流程为：

输入：初始值 x_0 ；终止阈值 \varepsilon ；目标函数 f(.)
输出：目标函数较小值  f_{min}
1.  k=0  
2.  \triangle_0 =-\frac{{f}'\left ( x_0 \right ) }{{f}''\left ( x_0 \right ) }
3.  while \Delta_k > \varepsilon
4.       x_{k+1}=x_k+\Delta_k
5.       k=k+1
6.      \triangle_k =-\frac{{f}'\left ( x_k \right ) }{{f}''\left ( x_k \right ) }
7.  end
8. f_{min}=f\left( x_k \right)

2.3 牛顿迭代法（二阶梯度）（多维函数）

对于多元函数f\left( X \right)=0, X=\left[ x_0,x_1,...,x_n \right]^\top的最优化问题，也可以利用牛顿迭代法进行求解。需要将牛顿迭代法扩展为矩阵形式。在多维函数中，使用雅可比矩阵（Jacobian Matrix）表示函数的一阶偏导：
J_f=\left [ \frac{\partial  f}{\partial x_0 } ...\frac{\partial  f}{\partial x_n }  \right ] \tag{11}\\ 使用海塞矩阵（Hessian Matrix）表示函数的二阶偏导：
H_f=J_f^\top J_f=\left [ \begin{matrix}  \frac{\partial ^2 f}{\partial x_0^2}  & \frac{\partial ^2 f}{\partial x_0 \partial x_1} & ... & \frac{\partial ^2 f}{\partial x_0 \partial x_n}\\ \frac{\partial ^2 f}{\partial x_1 \partial x_0}  & \frac{\partial ^2 f}{\partial x_1^2} & ... & \frac{\partial ^2 f}{\partial x_1 \partial x_n}\\  \vdots  &  \vdots & \ddots  & \vdots \\ \frac{\partial ^2 f}{\partial x_n \partial x_0}  & \frac{\partial ^2 f}{\partial x_n \partial x_1} & ... & \frac{\partial ^2 f}{ \partial x_n^2} \end{matrix}  \right ] \tag{12}\\牛顿迭代法可以将目标函数展开为：
{f}\left ( X_0+\bigtriangleup  \right ) \approx f\left( X_0 \right)+J_f\bigtriangleup  +\frac{1}{2}\bigtriangleup^\top H_f\bigtriangleup \tag{13}\\其中 \Delta=\left[ \Delta_0,\Delta_1,...,\Delta_n \right]^\top 同理，该函数取得最小值的条件就是该函数对\bigtriangleup的导数为零。即：
\frac{\partial \left ( f\left( X_0 \right)+J_f\bigtriangleup  +\frac{1}{2}\bigtriangleup^\top H_f \bigtriangleup \right ) }{\partial \triangle } =0\tag{14}\\ 解得：
H_f\triangle =-J_f \tag{15}\\ 缺点：该方法过于需要求海塞矩阵，计算量非常大。
  2.4 牛顿迭代法（二阶梯度）（一组多维函数）

那么对于由一组多维函数构成的目标函数 F\left ( X \right )=\sum_{i=0}^{m}{f_m\left( X \right)} ,其中f\left( X \right)=f\left( x_0,x_1,...,x_n \right)。
此时目标函数的雅可比矩阵（Jacobian Matrix）为：
J_f=\left [ \begin{matrix}   \frac{\partial f_0}{\partial x_0} & \dots  & \frac{\partial f_0}{\partial x_n}\\   \vdots & \ddots   &  \vdots\\   \frac{\partial f_n}{\partial x_0} & \dots  & \frac{\partial f_n}{\partial x_n} \end{matrix} \right ] \tag{16}\\ 其海塞矩阵计算方法与公式（12）相同：
H_f=J_f^\top J_f\tag{17}\\ 后续计算步骤与2.3小节相同。
2.5 高斯-牛顿法（一阶梯度）（一组多维函数）

高斯-牛顿法针对最小二乘问题，对海塞矩阵计进行了近似，从而减少算量。
最小二乘问题可以表达为：
\min_x{F\left ( x \right ) } =\left \| f\left ( x \right )  \right \| ^2\tag{18}\\ 那么当 x 在 x_0 处，具有增量 \Delta 时，优化目标函数取值为：
\min_x{F\left ( x_0 +\Delta\right ) } =\left \| f\left ( x_0+\Delta \right )  \right \| ^2\tag{19}\\ 不同于牛顿迭代法，高斯牛顿法不直接对目标函数进行泰勒展开，而是对优化目标函数的一部分，即 f\left( x \right) 进行一阶泰勒展开，展开后的目标函数为：
\min_x{F\left ( x_0 +\Delta\right ) } \approx\left \| f\left ( x_0 \right )+J_f \Delta \right \| ^2\tag{20}\\ 展开后的目标函数是关于\Delta的一元二次函数，且二次型系数为正，因此该函数拥有最小值。其取得最小值的条件就是该函数关于\Delta的导数为零，即：
\frac{\partial \left \| f\left ( x_0 \right )+J_f \Delta \right \| ^2}{\partial \triangle } =0 \tag{21}\\ 解得：
J_f ^\top J_f \triangle =-J_f ^\top f\left ( x_0 \right )  \tag{22} \\ 为了方便计算和记忆，我们把 J_f ^\top J_f  和 J_f ^\top f\left ( x_0 \right ) 记为：
H_f=J_f^\top J_f，  g_f=-J^\top _f f\left( x_0 \right)  \tag{23}\\注意，此处的 H_f 并不是目标函数真正的二阶偏导。
则高斯牛顿法的增量方程为：
H_f\triangle =-g_f \tag{24}\\ 高斯牛顿法的算法流程为：

输入：初始值 x_0 ；终止阈值 \varepsilon ；目标函数 \left \| f\left ( . \right )  \right \| ^2
输出：目标函数较小值  F_{min}
1.  k=0  
2.  \triangle_0 =-H_f^{-1}g_f
3.  while \Delta_k > \varepsilon
4.       x_{k+1}=x_k+\Delta_k
5.       k=k+1
6.      \triangle_k =-H_f^{-1}g_f
7.  end  
8. F_{min}=\left \| f\left ( x_k \right )  \right \| ^2

在实际求解增量\Delta 时，我们需要计算 \left( J_f^\top J_f \right)^{-1} ，但  J_f^\top J_f 只有半正定性，在实际计算中可能会遇见  J_f^\top J_f 为奇异矩阵或病态矩阵，从而导致增量 \Delta 不稳定，算法不收敛。另一方面时当我们求得的增量 \Delta 过大时，泰勒展开所获得的局部近似会不精确，从而导致算法不收敛。
2.6 L-M方法（一阶梯度）（一组多维函数）

针对高斯牛顿法的不足，L-M方法做了两点改进：1.在求解增量 \Delta时，对其设置了阻尼系数 \lambda；2.定义了量化指标 \rho，在求得增量 \Delta对其近似效果进行了量化，并根据量化结果对阻尼系数\lambda进行调整，再重新计算增量 \Delta，直到近似效果量化结果达到阈值。
L-M方法引入了阻尼系数\lambda ，优化问题从无约束最小二乘问题变为了有约束的最小二乘问题，即从公式（19）变为：
\min_x{F\left ( x_0 +\Delta\right ) } =\left \| f\left ( x_0+\Delta \right )  \right \| ^2 +\lambda \left \| D_f\Delta \right \|^2 \tag{25}\\ 其中， D_f 是系数矩阵，为 n 维向量，阻尼系数 \lambda \geq0 。列文伯格方法与马夸尔特方法的的主要区别就在于系数矩阵D不同。
根据公式（26）可以求解得L-M方法的增量方程为：
\left( H_f+\lambda D_f^\top D_f \right)\triangle =-g_f \tag{26}\\ 其中， H_f 和 g_f 的定义与高斯-牛顿法相同。
可以发现\lambda有以下影响：

• 对于所有\lambda \geq0 ，系数矩阵为正定，这确保目标函数为下降方向；
  
• 对于较大的\lambda，其增量可以近似为： \triangle \approx - \left( \lambda D_f^\top D_f \right)^{-1}g_f  。即在最陡的下降方向上的一小步。如果当前的迭代与解决方案相去甚远，这是很好的。
  
• 如果有\lambda 接近于零，则L-M方法退化为高斯-牛顿法，这对于迭代最后阶段非常有利。
  

在L-M方法中，定义量化指标 \rho 衡量近似程度，即目标函数的差值与泰勒展开函数的差值的比值，这里使用的 L(.) 是二阶泰勒展开式：
\rho =\frac{f\left ( x_0 \right )-f\left ( x_0+\triangle  \right ) }{L\left( 0 \right)-L\left( \triangle \right)} =\frac{f\left ( x_0 \right )-f\left ( x_0+\triangle  \right ) }{-J_f\bigtriangleup  -\frac{1}{2}\bigtriangleup^\top H_f\bigtriangleup }\tag{27}\\

• 当\rho较小时（甚至可能是负数），实际减小的值远小于近似函数减小的值，近似效果差，需要增大阻尼系数 \lambda。以达到接近梯度下降方向和减小步长的目的。
  
• 当\rho较大时，实际减小的值会大于近似函数减小的值，近似效果好，需要减小阻尼系数 \lambda。使算法的下一步更接近于高斯-牛顿法。
  

L-M方法的算法流程如下：

输入：初始值 x_0 ；终止阈值 \varepsilon ；目标函数 \left \| f\left ( . \right )  \right \| ^2；阻尼系数 \lambda ;倍率 v=2 ;
输出：目标函数较小值  F_{min}
01.  k=0  
02.  \triangle_0 =-\left( H_f+\lambda D_f^\top D_f \right)^{-1}g_f
03.  while \Delta_k > \varepsilon
04.      \rho =\frac{f\left ( x_k \right )-f\left ( x_k+\triangle_k  \right ) }{L\left( 0 \right)-L\left( \triangle_k \right)}  
05.      if \rho>0
06.          x_{k+1}=x_k+\Delta_k
07.          k=k+1
08.          \triangle_k =-H_f^{-1}g_f
09.          \lambda=\lambda*max\left\{ \frac{1}{3},1-\left( 2\rho-1 \right)^3 \right\}; v=2
10.          \triangle_k =-\left( H_f+\lambda D_f^\top D_f \right)^{-1}g_f
11.      else
12.          \lambda=\lambda*v;v=2*v
13.          \triangle_k =-\left( H_f+\lambda D_f^\top D_f \right)^{-1}g_f
14.      end
15.  end   
16.  F_{min}=\left \| f\left ( x_k \right )  \right \| ^2

列文伯格方法与马夸尔特方法的区别就在于方法中定义的系数矩阵不同：

• 在列文伯格方法中，系数矩阵 D_f=I ，信赖区域是一个球形；
  
• 在马夸尔特方法中，系数矩阵 D_f=\sqrt{Diag\left( J^\top J \right)} ，是对角元素的平方根；
  

列文伯格-马夸尔特在一定程度上解决了线性方程组系数矩阵的非奇异和病态问题，但算法收敛速度相较于高斯牛顿更慢。因此，当问题性质较好时，选择高斯牛顿方法，问题接近病态时，选择列文伯格-马夸尔特方法。
显然，三维重建论文中，使用的是L-M方法，采用马夸尔特式的系数矩阵。
参考文献：

Wu, Changchang, et al. "Multicore bundle adjustment." CVPR 2011. IEEE, 2011.
Jorge, Nocedal, and J. Wright Stephen. "Numerical optimization." (2006).
K. Madsen, H.B. Nielsen and O. Tingleff. "Method for non-linear least squares problems." (2004).
高翔, 张涛. "视觉SLAM十四讲" 2018.
高斯牛顿法详解_我只是一只自动小青蛙的博客-CSDN博客
Levenberg-Marquardt算法浅谈_Louis_lan的博客-CSDN博客_levenberg-marquardt
牛顿迭代法_百度百科