从零开始手写VIO第三章作业（含关键点细节及思维过程）

前言·与同主题博文的不同

延续该系列博文的风格，本博文尽量补充同主题博文所没有涉及到的关键点。对于第三章，所补充的关键点包含以下几个方面：第1题实现其他阻尼因子策略所涉及到的几乎所有细节（参考的论文，公式与代码的对应关系，代码修改后函数的整体部分），第2题中公式推导最开始表达式的由来，第3题的证明给出矩阵相乘的具体展开式。有任何问题都欢迎大家在评论区批评交流。

1.代码修改

样例代码给出了使用 LM 算法来估计曲线 y = exp(ax^2+ bx + c)参数 a, b, c 的完整过程。
1 请绘制样例代码中 LM 阻尼因子 µ 随着迭代变化的曲线图
2 将曲线函数改成 y = ax^2 + bx + c，请修改样例代码中残差计算，雅克比计算等函数，完成曲线参数估计
3 实现其他更优秀的阻尼因子策略，并给出实验对比

首先对样例代码做一个简单的介绍。app文件夹中有曲线拟合函数CurveFitting.cpp，backend文件夹是后端，求LM问题的一些函数在这个文件夹中，其中problem.cc的作用是定义和求解最小二乘问题，vertex是顶点，表示我们需要优化的状态量，edge就是构建出来的残差项。

1.1阻尼因子 µ 随着迭代变化的曲线图

编译并运行给定的样例代码，得到如下图所示结果，其中lambda就表示阻尼因子。

将上图中的lambda值整理后，用MATLAB生成对应的曲线图，代码和曲线图分别如下：

x=(0:11);%迭代次数
y=[0.001, 699.051, 1864.14, 1242.76, 414.252, 138.084, 46.028, 15.3427, 5.11423, 1.70474, 0.568247, 0.378832];%阻尼因子序列
plot (x,y,'-o','LineWidth',1.5);grid on;%画图

特别说明：图中的阻尼因子序列并不包含迭代过程中计算出的所有阻尼因子，只有满足所使用的阻尼因子更新策略中阻尼因子输出条件的才会被记录。程序中使用的是Nielsen 策略（具体实现查看problem.cc中的IsGoodStepInLM函数）。

1.2完成曲线y = ax² + bx + c的参数估计

首先需要明确修改文件CurveFitting.cpp中的哪几处代码。因为需要将y = exp(ax² + bx + c)改为y = ax²+ bx + c，小编以“exp”为关键字，利用搜索功能找到所有与exp相关的代码，经过分析，发现有三处需要修改。
第一处是main函数中的观测值y，代码如下：

// 构造 N 次观测
    for (int i = 0; i < N; ++i) {
        double x = i/100.;
        double n = noise(generator);
        // 观测 y
        double y = a*x*x + b*x + c + n;

第二处是构建残差，代码如下：

// 计算曲线模型误差
    virtual void ComputeResidual() override
    {
        Vec3 abc = verticies_[0]->Parameters();  // 估计的参数
        residual_(0) = abc(0)*x_*x_ + abc(1)*x_ + abc(2)  - y_;  // 构建残差
    }

第三处是计算残差对变量的雅克比，代码如下：

// 计算残差对变量的雅克比
    virtual void ComputeJacobians() override
    {
        Eigen::Matrix<double, 1, 3> jaco_abc;  // 误差为1维，状态量 3 个，所以是 1x3 的雅克比矩阵
        jaco_abc << x_ * x_ , x_  , 1 ;
        jacobians_[0] = jaco_abc;
    }

编译并运行修改之后的代码，结果如下：

可以发现参数估计误差较大。由于只修改了原代码中需要估计的曲线参数，并不会引起算法失效，初步分析，误差较大的原因可能是数据点数量不足。于是将数据点由100增加到1000，所需要修改的代码如下：

int main()
{
    double a=1.0, b=2.0, c=1.0;         // 真实参数值
    int N = 1000;                          // 数据点
    double w_sigma= 1.;                 // 噪声Sigma值

之后再次编译运行，结果如下：

可以看出，参数估计精度已经足够高。

1.3实现其他更优秀的阻尼因子策略

首先要说明的是，1.3这个部分小编是站在之前的优秀博文从零开始手写VIO第三期作业总结(仅供参考)的肩膀上完成的，在其中增加了自己思考的过程，并将其以阅读性较强的方式记录如下。
课件中提示可以参考论文《The Levenberg-Marquardt method for nonlinear least squares curve-fitting problems》中的阻尼因子策略。该论文中共有三种阻尼因子策略，如下图：

第三种策略与样例代码中的策略一样，都是前面提到的Nielsen 策略。下面在原样例代码problem.cc的基础上实现论文中的第一种阻尼因子策略。
通过对比分析上图中两种策略的不同点来修改样例代码。首先两种方法对阻尼因子初始化的方式不同，所以修改ComputeLambdaInitLM函数代码如下，这里一定要注意的是前面与变量currentChi_相关的语句必须保留，这里与程序中其它部分相关联。另外，为了之后与Nielsen 策略作对比，这里的阻尼因子的初值选为0.001，与1.1部分中阻尼因子的初值相同。

void Problem::ComputeLambdaInitLM() {
     currentChi_ = 0.0;
    // TODO:: robust cost chi2
    for (auto edge: edges_) {
        currentChi_ += edge.second->Chi2();
    }
    if (err_prior_.rows() > 0)
        currentChi_ += err_prior_.norm();

    stopThresholdLM_ = 1e-6 * currentChi_;          // 迭代条件为 误差下降 1e-6 倍   
    currentLambda_ = 1e-3;
}

其次，两种策略计算状态量的变化量和ρ的方式不同，论文中公式（12），公式（13），公式（15）和公式（16）如下图：

经过分析，公式（12）和公式（13）的主要区别在于两者对Hessian Matrix修改的方式不一样。
修改AddLambdatoHessianLM函数：

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) += currentLambda_ * Hessian_(i, i);
    }
}

修改RemoveLambdaHessianLM函数：

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.0 + currentLambda_;
    }
}

最后公式（15）和公式（16）的主要区别在于分母计算的不同，修改IsGoodStepInLM函数：

bool Problem::IsGoodStepInLM() {
    // 统计所有的残差
    double tempChi = 0.0;
    for (auto edge : edges_) {
        edge.second->ComputeResidual();
        tempChi += edge.second->Chi2();
    }
    // compute rho
    assert(Hessian_.rows() == Hessian_.cols() && "Hessian is not square");
    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 = delta_x_.transpose() * (currentLambda_ * diag_hessian * delta_x_ + b_);
    double rho = (currentChi_ - tempChi) / scale;
    // update currentLambda_
    double epsilon = 0.0;
    double L_down = 9.0;
    double L_up = 11.0;
    if (rho > epsilon && isfinite(tempChi)) {
        currentLambda_ = std::max(currentLambda_ / L_down, 1e-7);
        currentChi_ = tempChi;
        return true;
    }
    else {
        currentLambda_ = std::min(currentLambda_ * L_up, 1e7);
        return false;
    }
}

对于程序中参数的选取问题，建议有兴趣的读者阅读论文本身。
对修改之后的代码进行编译，运行之后得到下图：

对比分析：第一、两种策略参数估计精度相当；第二、两种策略在阻尼因子的初值相同的条件下，论文中的第一种方法所需要迭代的次数更少。说明有时候简单的策略会产生更好的效果。

2.公式推导

对于这里涉及的公式推导，同主题博文基本都是直接给出了公式推导演变的过程，并没有给出公式最开始的表达式从何而来，这是小编在下面要重点阐述的部分。
需要推导的题目如下：

严格来说，公式推导需要铺垫大量的数学理论背景知识，这是小编所做不到的。但是没有理论铺垫又会让推导无据可循。小编采取的折中的办法是，以课件中f₃₅的推导为理论依据，作为准则，所给两道题目中的推导除公式最开始的表达式外，推导过程只是在数值和符号上与f₃₅的推导有所区别。课件中f₃₅的推导过程如下图，最关键的点在于下图中红框中的-δb^g_k是如何得到的！

下面是f₁₅和g₁₂的推导，详细说明了推导最开始的表达式从何而来。前面所说的f₃₅的-δb^g_k的由来可以参考f₁₅中-δb^g_k的由来。


图片中提到的课件中P37和P44的内容截图如下：

3.证明式(9)

这道题的证明给出矩阵相乘的具体展开式。


这道题的证明小编也是站在之前的优秀博文从零开始手写VIO第三期作业总结(仅供参考)的肩膀上完成的，在这里对原博主Zkangsen表示感谢！