【VIO笔记（学习VINS的必备基础）】第五讲（2/2） 手写VIO后端

系列教程来自某学院，侵权删除。
学习完这一系列课程再去看VINS才能做到不吃力，不然直接撸网上的各种VINS解析完全云里雾里-_-!

上接第五讲1/2。

单目BA程序

上一讲讲到了H矩阵的构建，之后就要进行LM算法的初始化，ComputeLambdaInitLM()函数用于计算阻尼因子的初始值，在第三讲中说到过，阻尼因子的初始值选取方式为：

其中$\tau$是一个系数，按工程经验取值，程序里这个值取了10^-5double tau = 1e-5;，最后阻尼因子的计算为currentLambda_ = tau * maxDiagonal;maxdiagonal是H矩阵对角线上最大的块。
初始化完成之后就进入LM迭代求解的部分，迭代次数由solve函数的入口参数确定，这是最后的运行结果截图，可以看到迭代了5次，

下面我们来看一次迭代中，LM算法是怎么实现的，方便阅读再把程序放上来一次：

    while (!stop && (iter < iterations)) {
   
        std::cout << "iter: " << iter << " , chi= " << currentChi_ << " , Lambda= " << currentLambda_ << std::endl;
        bool oneStepSuccess = false;
        int false_cnt = 0;
        while (!oneStepSuccess)  // 不断尝试 Lambda, 直到成功迭代一步
        {
   
        	// 解Hx=b
            SolveLinearSystem();
            // 优化退出条件1： delta_x_ 很小则退出
            if (delta_x_.squaredNorm() <= 1e-6 || false_cnt > 10) {
   
                stop = true;
                break;
            }
            // 更新状态量
            UpdateStates();
            // 判断当前步是否可行以及 LM 的 lambda 怎么更新
            oneStepSuccess = IsGoodStepInLM();
            // 后续处理，
            if (oneStepSuccess) {
   
                // 在新线性化点 构建 hessian
                MakeHessian