基于高斯牛顿法的非线性优化（附C++代码）

非线性优化

1 理论部分

高斯牛顿法可以简化为如下算法流程：

L-M方法：

那么在视觉slam中，利用李代数将PNP问题的前端的非线性优化问题转换成为无约束的非线性优化问题，这里的核心就是采用链式求导的方式求雅可比矩阵。当然，这里有个疑问：为什么对差分量进行求导？不符合前面介绍的高斯牛顿的方法要求

2 非线性优化的示例

2.1 如何设计一个优化系统？

对离散系统的误差进行优化设计，主要是将其构造成为最小二乘估计的形式，采用梯度下降或者高斯牛顿法求解优化问题，以得到想要的最大似然估计。下面是一个简单的示例：

2.2 基于高斯牛顿法的非线性优化（附C++代码）

非线性优化中，采用高斯牛顿法是最为简单和常用的一个方式。下面给出由C++的示例。这里主要解决的问题是曲线拟合。求出拟合曲线的三个参数值a,b,c。¹

#include <iostream>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

int main(int argc, char **argv) {
    double ar = 1.0, br = 2.0, cr = 1.0;         // 真实参数值
    double ae = 2.0, be = -1.0, ce = 5.0;        // 估计参数值
    int N = 100;                                 // 数据点
    double w_sigma = 1.0;                        // 噪声Sigma值
    cv::RNG rng;                                 // OpenCV随机数产生器

    vector<double> x_data, y_data;      // 数据
    for (int i = 0; i < N; i++) {
        double x = i / 100.0;
        x_data.push_back(x);
        y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma));
    }

    // 开始Gauss-Newton迭代
    int iterations = 100;    // 迭代次数
    double cost = 0, lastCost = 0;  // 本次迭代的cost和上一次迭代的cost

    for (int iter = 0; iter < iterations; iter++) {
        Matrix3d H = Matrix3d::Zero();             // Hessian = J^T J in Gauss-Newton
        Vector3d b = Vector3d::Zero();             // bias
        cost = 0;
        for (int i = 0; i < N; i++) {
            double xi = x_data[i], yi = y_data[i];  // 第i个数据点
            double error = 0;   // 第i个数据点的计算误差
            error = yi-exp(ae*xi*xi+be*xi+ce);
            Vector3d J;
            J[0] = -xi*xi*exp(ae*xi*xi+be*xi+ce);
            J[1] = -xi*exp(ae*xi*xi+be*xi+ce);
            J[2] = -exp(ae*xi*xi+be*xi+ce);
            H += J * J.transpose(); // GN近似的H
            b += -error * J;
            cost += error * error;
        }
        // 求解线性方程 Hx=b，建议用ldlt
        Vector3d dx;
        //dx=H.inverse()*b; //直接求逆方法求解增量
        dx = H.ldlt().solve(b); // ldlt方法
        if (isnan(dx[0])) {
            cout << "result is nan!" << endl;
            break;
        }

        if (iter > 0 && cost > lastCost) {
            // 误差增长了，说明近似的不够好
            cout << "cost: " << cost << ", last cost: " << lastCost << endl;
            break;
        }

        // 更新abc估计值
        ae += dx[0];
        be += dx[1];
        ce += dx[2];

        lastCost = cost;

        cout << "total cost: " << cost << endl;
    }

    cout << "estimated abc = " << ae << ", " << be << ", " << ce << endl;
    return 0;
}

该误差关于自变量的雅克比矩阵是什么？
$\begin{aligned} \frac{\partial e(\xi)}{\partial \xi} &=\frac{\partial e(\xi)}{\partial p^{\prime}} \frac{\partial p^{\prime}}{\partial \xi} \\ &=\left[\begin{array}{ccccc}f_{x} \frac{1}{z^{\prime}} & 0 & -f_{x} \frac{x^{\prime}}{z^{2}} & -f_{x} \frac{x^{\prime} y^{\prime}}{z^{2}} & f_{x}\left(1+\frac{x^{2}}{z^{2}}\right) & -f_{x} \frac{y^{\prime}}{z^{\prime}} \\ 0 & f_{y} \frac{1}{z^{\prime}} & -f_{y} \frac{y^{\prime}}{z^{2}} & -f_{y}\left(1+\frac{y^{2}}{Z^{2}}\right) & f_{y} \frac{X^{\prime} Y^{\prime}}{Z^{2}} & f_{y} \frac{x^{\prime}}{Z^{\prime}}\end{array}\right] \end{aligned}$
实际上这里与张所说的链式求导是比较一致的。

• 解出更新量之后，如何更新至之前的估计上？通过李代数左乘更新，或四元素左乘更新。（右乘也行）代码中我是通过李代数左乘的方法。

---

1. **(主要参考slam练习5)**设计基于高斯牛顿法的BA ↩︎