手写Bundle Adjustment（二）高斯牛顿法拟合曲线

最小二乘法：又称最小平方法，是一种数学优化技术。它通过最小化误差的平方和寻找数据的最佳函数匹配。利用最小二乘法可以简便地求得未知的数据，并使得这些求得的数据与实际数据之间误差的平方和为最小。完整代码可见https://github.com/YCJin9/sparse_BA

高斯牛顿法是最优化算法当中最简单的一种，这会便于我们去实现，但同时高斯牛顿法有着他本身的问题，这会在本篇博客的最后进行展示。

高博十四讲中第六讲提供了用Ceres和g2o实现曲线拟合的程序，这篇博客是要手写一个高斯牛顿法实现这个拟合曲线的程序，同时最小二乘法的理论基础也可以在第六讲找到，相信大家也都了解。

1、构建最小二乘问题：

假设有一条满足一下方程的曲线：

$y=exp(ax^{2}+bx+c)+\omega$ ，

其中a, b, c为曲线参数， $\omega$ 为高斯噪声。有N个关于x, y的观测数据点，根据这些数据求出曲线参数。那么，可以求解下面的最小二乘问题以估计曲线参数：

$\min_{a,b,c}\tfrac{1}{2}\sum _{i=1}^{N}\left \| y_{i}-exp(ax_{i}^{2}+bx_{i}+c) \right \|^{2}$

先根据模型生成N个关于x, y的真值，然后在真值中添加高斯分布的噪声产生观测数据。

为方便验证，我们使用的观测数据产生方式与高博十四讲中第六讲Ceres和g2o相同，使用OpenCV的随机数产生器。

    double a=1.0, b=2.0, c=1.0;         // 真实参数值
    int N=100;                          // 数据点
    double w_sigma=1.0;                 // 噪声Sigma值
    cv::RNG rng;                        // OpenCV随机数产生器
    double ae=2.0, be=-1.0, ce=5.0;     // abc参数的估计值

    vector<double> x_data, y_data;      // 数据

    cout<<"generating data…… "<<endl;
    for ( int i=0; i<N; i++ )
    {
        double x = i/100.0;
        x_data.push_back ( x );
        y_data.push_back (
                exp ( a*x*x + b*x + c ) + rng.gaussian ( w_sigma )
        );
        //cout<<x_data[i]<<" "<<y_data[i]<<endl;
    }

2、实现高斯牛顿迭代

高斯牛顿法的主要步骤是在每一次迭代中用 $J^{T}J$ 作为Hessian矩阵的近似，求解增量方程。

单个数据点的误差

$error=y_{i}-exp(aex_{i}^{2}+bex_{i}+ce)$ ,

雅克比矩阵

$J=-\left [ x_{i}^{2}exp(ae*x_{i}^{2}+be*x_{i}+ce), x_{i}exp(ae*x_{i}^{2}+be*x_{i}+ce), exp(ae*x_{i}^{2}+be*x_{i}+ce) \right ]^{T}$

    // 开始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
        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();
            b += -error * J;

            cost += error * error;

        }

        Vector3d dx;
        dx = H.inverse()*b;
        //dx = H.colPivHouseholderQr().solve(b);    //QR分解，可加快求解速度
        //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;

3、程序运行结果

最后输出的结果应该与十四讲中的结果基本一致，即 a = 0.890912, b = 2.1719, c = 0.943629

4、高斯牛顿法缺点展示

在程序中我们改变abc的参数估计初值，可能会导致结果不收敛，例如：我们更改abc初值为0, 0, 0，程序运行结果如下：

迭代结果不收敛的原因：一是因为高斯牛顿法使用 $J^{T}J$ 作为Hessian矩阵的近似，原则上H矩阵是可逆且正定的，但实际数据计算得到的 $J^{T}J$ 可能出现为奇异矩阵或者病态的情况，此时增量的稳定性较差，导致算法不收敛；另一方面，高斯牛顿法将f(x)一阶泰勒展开进行近似，即： $f(x+\bigtriangleup x)\approx f(x)+J\bigtriangleup x$ ，当求出的步长 $\bigtriangleup x$ 太大时也会导致近似不够准确使得结果不收敛。

当然我们并不能因为这些问题就放弃高斯牛顿法的学习，高斯牛顿法是最简单的非线性优化方法，同时也是很多优化算法的基础，很多算法都可以看作是高斯牛顿法的变种，借助了高斯牛顿法的思想改进修正其缺点。

手写Bundle Adjustment（二）高斯牛顿法拟合曲线

猜你喜欢