最小二乘问题求解

---

前言

使用非线性优化的方法优化相机位姿和路标点时需要进性最小二乘问题的求解

---

一、问题定义

目标是找到一个x使得F(x)有最小值：

二、损失函数F(x)泰勒展开

其中$J$为雅克比矩阵，$H$为海塞矩阵

三、残差函数f(x)泰勒展开

代入损失函数有：

三、求解方法

1.最速下降法

在梯度的负方向上取一个x的增量，容易在最优值附件震荡，收敛慢。

2.牛顿法

如果$x+\Delta x$是最优解，则损失函数对$\Delta x$的导数等于0，则有：

得到:$\Delta x=-H^{-1}{J}^T$,二阶导矩阵计算复杂

3.高斯牛顿法

残差函数泰勒展开后的式子一阶导等于0，得到：

3.列文伯格法

引入阻尼因子：

三、图优化

1.基本思想

将待优化变量看做是图中的节点(Vertex)，将残差看做是图中的边。

2.相关矩阵的维度

优化时所有的残差都需要对待优化变量求导，所以若残差项为m项，待优化变量为n项，则雅克比矩阵的维度为$m\ast n$，海塞矩阵$H=J^{T}J$，所以海塞矩阵的维度为$n\ast n$。

2.实际计算的流程

1.统计待估计的所有变量的维度

2.遍历每条边，并计算他们的残差和雅克比

3.雅克比计算时需要乘上信息矩阵

4.计算出海塞矩阵

5.计算阻尼系数

6.计算$\Delta x$

7.更新$x$

8.判断$x$是否满足条件

9.更新海塞矩阵开始下一次迭代

部分关键代码如下：

bool Problem::Solve(int iterations) {


    if (edges_.size() == 0 || verticies_.size() == 0) {
        std::cerr << "\nCannot solve problem without edges or verticies" << std::endl;
        return false;
    }

    TicToc t_solve;
    // 统计优化变量的维数，为构建 H 矩阵做准备
    SetOrdering();
    // 遍历edge, 构建 H = J^T * J 矩阵
    MakeHessian();
    // LM 初始化
    ComputeLambdaInitLM();
    // LM 算法迭代求解
    bool stop = false;
    int iter = 0;
    while (!stop && (iter < iterations)) {
        std::cout << "iter: " << iter << " , chi= " << currentChi_ << " , Lambda= " << currentLambda_
                  << std::endl;
        bool oneStepSuccess = false;
        int false_cnt = 0;
        while (!oneStepSuccess)  // 不断尝试 Lambda, 直到成功迭代一步
        {
            // setLambda
            AddLambdatoHessianLM();
            // 第四步，解线性方程 H X = B
            SolveLinearSystem();
            //
            RemoveLambdaHessianLM();

            // 优化退出条件1： delta_x_ 很小则退出
            if (delta_x_.squaredNorm() <= 1e-6 || false_cnt > 10) {
                stop = true;
                break;
            }

            // 更新状态量 X = X+ delta_x
            UpdateStates();
            // 判断当前步是否可行以及 LM 的 lambda 怎么更新
            oneStepSuccess = IsGoodStepInLM();
            // 后续处理，
            if (oneStepSuccess) {
                // 在新线性化点 构建 hessian
                MakeHessian();
                // TODO:: 这个判断条件可以丢掉，条件 b_max <= 1e-12 很难达到，这里的阈值条件不应该用绝对值，而是相对值
//                double b_max = 0.0;
//                for (int i = 0; i < b_.size(); ++i) {
//                    b_max = max(fabs(b_(i)), b_max);
//                }
//                // 优化退出条件2： 如果残差 b_max 已经很小了，那就退出
//                stop = (b_max <= 1e-12);
                false_cnt = 0;
            } else {
                false_cnt++;
                RollbackStates();   // 误差没下降，回滚
            }
        }
        iter++;

        // 优化退出条件3： currentChi_ 跟第一次的chi2相比，下降了 1e6 倍则退出
        if (sqrt(currentChi_) <= stopThresholdLM_)
            stop = true;
    }
    std::cout << "problem solve cost: " << t_solve.toc() << " ms" << std::endl;
    std::cout << "   makeHessian cost: " << t_hessian_cost_ << " ms" << std::endl;
    return true;
}

总结

不断的迭代获取最优解，这部分和神经网络中的反向传播有点类似，还有许多相关的知识点，后续再补上。