【深蓝学院第三章作业】

1 LM算法的实现

1.1 LM算法的原理

LM算法的推导有两种：阻尼法（Damped Method)与置信域法(Trust-Region Methods)。其中PPT中介绍了阻尼法，SLAM十四讲中介绍了置信域法。而论文METHODS FOR NON-LINEAR LEAST SQUARES PROBLEMS.中则将两种方法划分为一节内容，即2.4节Trust Region and Damped Methods。

1.1.1 阻尼法(包含作业三——见(5)式）

阻尼法最早由Levenberg和Marquardt提出，其初衷为构造一个既有优良的前期性质又可在后期接近收敛点的收敛过程中保持较快的速度。由此，其在正规方程(normal equation)中添加了一个阻尼项：当阻尼因子(damping parameter)较大时，正规方程退化为最速下降法(steepest descent method)表达式；当阻尼因子较小时，正规方程则退化为高斯牛顿方法的正规方程形式。因而，对于阻尼因子的选取应当保证，在前期收敛过程中较大，后期收敛过程中较小的趋势。那么如何设计阻尼因子以保证能够获得前述的性质？

L-M方法正规方程：
$$(J^{T}J+\mu I){\Delta }_{lm}=-J^{T}f(1)$$
Guass-Newton方法正规方程：
$$(J^{T}J){\Delta }_{gn}=-J^{T}f(2)$$
最速下降法正规方程：
$${\Delta }_{sd}=-J^{T}f(3)$$
为了获得合理的阻尼因子设计，首先是阻尼因子的初始值如何选取；其次是在算法收敛的过程中，阻尼因子能够以我们上面所期望的样子使算法发生变化。

• 阻尼因子的初始值选取：

阻尼因子的初始值，我们不希望过小，也不希望会过大。在初始状态下，希望$\mu$能够与$J^{T}J$中的元素保持一致的数量级。因此，对$J^{T}J$进行QR分解获得对应的特征值，使得最大的特征值与$\mu$为相同的数量级即可。然而，每一次迭代均对$J^{T}J$进行特征值分解将造成巨大的计算复杂度，幸而矩阵最大的特征值与矩阵对角线上最大的元素是处于相同数量级的，因此将$\mu$取值为 τ ⋅ m a x { ( J T J ) i i } \tau\cdot max\{(J^TJ)_{ii}\} τ⋅max{(JTJ)ii​}。其中，$\tau \sim [10^{-8},1]$。

为什么希望阻尼因子有着与$J^{T}J$元素有相同的数量级？这可能需要介绍另一种更新方式：
$$\Delta X_{lm}=-\sum _{j=1}^n\frac{v_j^T{F}^{\prime T}{v}_j}{{\lambda }_j+\mu }(4)$$
那么如何用(1)式得到(4)式呢？我们可以进行以下推导：
$$\begin{gathered} (J^{T}J+\mu I){\Delta }_{lm}=-J^{T}f \\ (V\Lambda V^T+\mu I){\Delta }_{lm}=-F^{\prime } \\ (V\Lambda V^T+\mu V{V}^T){\Delta }_{lm}=-F^{\prime } \\ V(\Lambda +\mu I)V^T{\Delta }_{lm}=-F^{\prime } \\ {\Delta }_{lm}=-V(\Lambda +\mu I{)}^{-1}{V}^T{F}^{\prime } \\ =-\left[v_1,...,v_n\right]\left[\begin{array}{cccc}\frac{1}{{\lambda }_1+\mu }& & & \\ & \frac{1}{{\lambda }_2+\mu }& & \\ & & ...& \\ & & & \frac{1}{{\lambda }_n+\mu }\end{array}\right]\left[\begin{array}{c}{v}_1^T\\ v_2^T\\ ...\\ v_n^T\end{array}\right]F^{\prime T}(5) \\ =-\left[v_1,...,v_n\right]\left[\begin{array}{c}\frac{v_1^T{F}^{\prime T}}{{\lambda }_1+\mu }\\ \frac{v_2^T{F}^{\prime T}}{{\lambda }_2+\mu }\\ ...\\ \frac{v_n^T{F}^{\prime T}}{{\lambda }_n+\mu }\end{array}\right] \\ =-(\frac{v_1^T{F}^{\prime T}}{{\lambda }_1+\mu }{v}_1+\frac{v_2^T{F}^{\prime T}}{{\lambda }_2+\mu }{v}_2+...+\frac{v_n^T{F}^{\prime T}}{{\lambda }_n+\mu }{v}_n)=-\sum _{j=1}^n\frac{v_j^T{F}^{\prime T}}{{\lambda }_j+\mu }{v}_j \end{gathered}$$
通过(5)式中的推导能够得到(4)式中的结论，从(4)式中能够看出当阻尼因子$\mu$与特征值$\lambda$保持相同的数量级时，能够达到综合sgd和guass-newton的结果。
此外，有时我们觉得自己的结果离最终的收敛状态较近，因此需要将阻尼值调小以期达到更加接近高斯牛顿收敛的方法，因为高斯牛顿在后期收敛过程中效果较好（即接近最终值的时候）。
基于上述描述，我们可以理解$\tau \sim [10^{-8},1]$的含义。

• 阻尼因子在收敛过程中的设置：
  设置一个比例因子$\rho$来定量分析阻尼因子的更新，在前面已经给出了定性分析的方法，我们来分析定量如何进行表示。

这里设置$\rho$为实际下降与近似下降之比，其中实际下降为目标函数$F(x)$与其在$\Delta$作用下的更新$F(X+{\Delta }_{lm})$之差，近似下降为依据高斯牛顿法近似后的目标函数$F(x)$(记作$L(0)$)与其在$\Delta$作用下的更新$L({\Delta }_{lm})$之差。所以$\rho$可以表示为如(6)式所示的形式。
$$\rho =\frac{F(x)-F(x+\Delta X_{lm})}{L(0)-L(\Delta X_{lm})}(6)$$
那么可以做出如下决策：

• $\rho <0$：说明$F(x)$增大了，因此需要将这次更新舍弃，并且增大阻尼因子，以达到SGD的效果，即更快速地降低目标函数的值；
• $\rho >0$且较大：说明实际下降太快，希望将下降速度调低，因此需要减小阻尼因子，以趋向于高斯牛顿；
• $\rho >0$且较小：说明实际下降太慢，希望将下降速度调高，以更快地达到最终目标，即趋向于SGD。

于是有如下的调整策略：

• $1963Marquardt$：
  $$\begin{gathered} if,\rho <0.25 \\ \mu =\mu \ast 2 \\ elif,\rho >0.75 \\ \mu =\mu \ast \frac{1}{3} \end{gathered}$$
• $1999Nielsen$：
  i f , ρ > 0 : μ = μ ∗ m a x { 1 3 , 1 − ( 2 ρ − 1 ) 3 } , v = 2 e l s e : μ = μ ∗ v , v = 2 ∗ v if,\rho>0:\\ \mu=\mu*max\{\frac{1}{3},1-(2\rho-1)^3\},v=2\\ else:\\ \mu=\mu*v,v=2*v if,ρ>0:μ=μ∗max{31​,1−(2ρ−1)3},v=2else:μ=μ∗v,v=2∗v

两种策略无所谓最优，对于Marquardt会有目标函数值上下跳跃的情况，而Nielson策略则回避了这一点，或许在某些情况下使用Nielson策略会更好一些？

1.1.2 置信域法

置信域法只是在推导思路方面与阻尼法有所不同，但是结论是一致的。阻尼法直接在正规方程上做文章，而置信域法则在目标函数上做文章。其在高斯牛顿法的基础上加上了信赖区域，即最终为求解：
$$\underset{\Delta x_k}{\min }\frac{1}{2}\Vert f(x_k)+J(x_k{)}^T\Delta x_k{\Vert }^2,s.t.\Vert D\Delta x_k{\Vert }^2\le \mu (7)$$
用拉格朗日乘子将约束项放到目标函数中，构成拉格朗日函数，之后求导能够得到类似于正规方程的形式：
$$(H+\lambda D^{T}D)\Delta x_k=g(8)$$
若将$D$认为是单位阵，那么就能够得到(1)式。

1.2 作业一

1.2.1 绘制阻尼因子随迭代变化的曲线图

• 第一步：输出阻尼因子信息以及cost变化

对problem.cc的solve函数进行如下改变：

	ostringstream csvfilename;
    std::ofstream Lambda_data;
    csvfilename << "/home/tiandao/CLionProjects/course_vio/lecture3/result/Lambda.csv";
    Lambda_data.open(csvfilename.str(), ios_base::out);
    while (!stop && (iter < iterations)) {
        std::cout << "iter: " << iter << " , chi= " << currentChi_ << " , Lambda= " << currentLambda_
                  << std::endl;
        Lambda_data << currentChi_ << "," << currentLambda_ << endl;

则能够在对应的路径下获得.csv文件，其中包含了阻尼因子信息以及cost变化。

• 第二步：用python绘制阻尼因子随迭代的变化曲线图，以及cost随迭代的变化曲线图

这里采用python3的matplotlib和pandas绘制，代码如下：

import matplotlib.pyplot as plt
import os
import pandas as pd

filepath = os.path.abspath('.')
Chi = []
Lambda = []

result_list = pd.read_csv('/home/tiandao/CLionProjects/course_vio/lecture3/result/Lambda.csv')
Lambda = result_list['Lambda']
Chi = result_list['Chi']
print(result_list)

fig1 = plt.figure(1)
plt.plot(Lambda)
fig1.savefig('./result/lambda.png')

fig2 = plt.figure(2)
plt.plot(Chi)
fig2.savefig('./result/Chi.png')

阻尼因子随迭代次数变化的结果如下所示：

cost随迭代次数变化的结果如下所示：

可以看出，cost值一直减小，直至收敛；而阻尼因子则会在初始迭代过程中上升以获得前期的sgd特性，而后期阻尼因子则会减小以获得后期的guass-newton特性。

1.2.2 获得修改后曲线的参数估计

对CurveFitting.cpp进行如下修改。
第一处：构造残差部分

//residual_(0) = std::exp(abc(0)*x_*x_ + abc(1)*x_ + abc(2)) - y_;
        residual_(0) = abc(0)*x_*x_ + abc(1)*x_ + abc(2) - y_;  // 构建残差

第二处：雅可比部分

//jaco_abc << x_ * x_ * exp_y, x_ * exp_y , 1 * exp_y;
        jaco_abc << x_ * x_, x_, 1;

第三处：观测部分

// 观测 y
        double y = a*x*x + b*x + c + n;
        // double y = std::exp(a*x*x + b*x + c) + n;

经过修改后能够输出如下结果

我们发现优化的结果很差，离ground truth还差了很远，分析原因应该是数据点过少，所以增加数据点看看是否能够提升效果。将数据点数从100修改为1000个，能够得到如下的结果：

可以明显看到精度的提升。

1.2.3 其他更优秀策略的尝试

在论文“The Levenberg-Marquardt method for nonlinear least squares curve-fitting problems”中涉及到三种策略，其中前两种策略为需要实现的策略，第三种策略为Nielson策略，在原代码中已经进行了实现，下面对前两种策略进行实现。

• 策略一：

初值选取不一致，且阻尼因子更新策略不一致。修改AddLambdatoHessianLM()、RemoveLambdaHessianLM()和IsGoodStepinLM()函数。

策略如下：

代码修改：

void Problem::AddLambdatoHessianLM() {
    ulong size = Hessian_.cols();
    assert(Hessian_.rows() == Hessian_.cols() && "Hessian is not square");
    for (ulong i = 0; i < size; ++i) {
        Hessian_(i, i) *= (1.+currentLambda_);//修改处
    }
}

void Problem::RemoveLambdaHessianLM() {
    ulong size = Hessian_.cols();
    assert(Hessian_.rows() == Hessian_.cols() && "Hessian is not square");
    // TODO:: 这里不应该减去一个，数值的反复加减容易造成数值精度出问题？而应该保存叠加lambda前的值，在这里直接赋值
    for (ulong i = 0; i < size; ++i) {
        Hessian_(i, i) /= (1.+currentLambda_);//修改处
    }
}

bool Problem::IsGoodStepInLM() {

    // recompute residuals after update state
    // 统计所有的残差
    double tempChi = 0.0;
    for (auto edge: edges_) {
        edge.second->ComputeResidual();
        tempChi += edge.second->Chi2();
    }

    ulong size = Hessian_.cols();
    MatXX diag_hessian(MatXX::Zero(size, size));
    for (ulong i = 0; i < size; ++i) {
        diag_hessian(i, i) = Hessian_(i, i);
    }

    double scale = 0;
    scale = delta_x_.transpose() * (currentLambda_ * diag_hessian * delta_x_ + b_);
    scale += 1e-3;    // make sure it's non-zero :)

    double rho = (currentChi_ - tempChi) / scale;
//    std::cout << "rho = " << rho << std::endl;

    double L_down = 9.0;
    double L_up = 11.0;

    if (rho > 0 && isfinite(tempChi))
    {// last step was good, 误差在下降{
        currentLambda_ = std::max(currentLambda_ / L_down, 1e-7);
        currentChi_ = tempChi;
        return true;
    } else {
        currentLambda_ = std::min(currentLambda_ * L_up, 1e7);
        return false;
    }
}

结果：

• 策略二：

初值选取一致，阻尼因子更新策略不一致。只需修改IsGoodStepInLM()函数。

策略如下：

修改部分：

bool Problem::IsGoodStepInLM() {

    // recompute residuals after update state
    // 统计所有的残差
    double tempChi = 0.0;
    for (auto edge: edges_) {
        edge.second->ComputeResidual();
        tempChi += edge.second->Chi2();
    }
//    cout << "currentChi_:" << currentChi_ << endl;
//    cout << "tempChi:" << tempChi << endl;

    double Numerator = b_.transpose() * delta_x_;
        double alpha = Numerator / ((currentChi_ - tempChi)/2. + 2.*Numerator);
        alpha = std::max(alpha, 1e-1);
    //    cout << "Numerator:" << Numerator << endl;
    //    cout << "alpha:" << alpha << endl;

    RollbackStates();
    delta_x_ *=alpha;
    UpdateStates();

    tempChi = 0.0;
    for (auto edge: edges_) {
        edge.second->ComputeResidual();
        tempChi += edge.second->Chi2();
    }

    double scale = 0;
    scale = delta_x_.transpose() * (currentLambda_ * delta_x_ + b_);
    scale += 1e-3;    // make sure it's non-zero :)
    double rho = (currentChi_ - tempChi) / scale;
    //    std::cout << "rho = " << rho << std::endl;

    if (rho > 0 && isfinite(tempChi))   // last step was good, 误差在下降
        {
        currentLambda_ = std::max(currentLambda_ / (1.+alpha), 1e-7);
        currentChi_ = tempChi;
        return true;
        } else {
        currentLambda_ += abs(currentChi_ - tempChi)/(2.*alpha);
        return false;
    }
}

结果：

2 作业二——雅可比公式推导

作业所示的两项如下图所示：