LM-GN 的实现

今天实现了一下某篇论文里的LM-GN,感觉效果还挺不错的，以后如果要手写ba的话可以把它用到优化器里面

//
// Created by 高翔 on 2017/12/15.
//
#include <iostream>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

const double eps = 1e-6;
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 = 200;    // 迭代次数
    double cost = 0, lastCost = 0;  // 本次迭代的cost和上一次迭代的cost

    double mu = 0, tao = 1;
    Matrix3d H = Matrix3d::Zero();             // Hessian = J^T J in Gauss-Newton
    Vector3d b = Vector3d::Zero();   
    Vector3d dx = Vector3d::Zero(); 
    for (int i = 0; i < N; i++) 
    {
        double xi = x_data[i], yi = y_data[i];  // 第i个数据点
        // start your code here
        double error = 0;   // 第i个数据点的计算误差
        Vector3d J; // 雅可比矩阵
        J[0] = 0;  // de/da
        J[1] = 0;  // de/db
        J[2] = 0;  // de/dc
        
        double Z = exp(ae * xi * xi + be * xi + ce); 
        J[0] = xi * xi * Z;
        J[1] = xi * Z;
        J[2] = Z;
        error = Z - yi;

        H += J * J.transpose(); // GN近似的H
        b += -error * J;

        cost += error * error;
    }
    lastCost = cost;
    mu = tao * H.diagonal().maxCoeff();
    bool stop = (cost < eps);
    double rou = 1, v = 2;
    for (int iter = 0; iter < iterations && (!stop); iter++) {

        do
        {
            Matrix3d muH = H + mu * Matrix3d::Identity();
            dx = muH.ldlt().solve(b);
        
            // 求解线性方程 Hx=b，建议用ldlt
        // start your code here
            Vector3d dx = H.ldlt().solve(b);
        // end your code here

            if (isnan(dx[0])) {
                std::cout << "result is nan!" << std::endl;
                break;
            }
            if(dx.squaredNorm() < eps)
            {
                break;
            }
            double tae = ae + dx[0];
            double tbe = be + dx[1];
            double tce = ce + dx[2];
            cost = 0;
            for (int i = 0; i < N; i++) {
                double xi = x_data[i], yi = y_data[i];  // 第i个数据点
                
                double Z = exp(tae * xi * xi + tbe * xi + tce); 
                double error = Z - yi;
                cost += error * error;
            }
            double rou = (lastCost - cost) / (dx.transpose() * (mu * dx + b));
            if(rou > 0)
            {
                ae += dx[0];
                be += dx[1];
                ce += dx[2];
                
                H = Matrix3d::Zero();
                b = Vector3d::Zero();
                cost = 0;
                for (int i = 0; i < N; i++) 
                {
                    double xi = x_data[i], yi = y_data[i]; 
                    double Z = exp(ae * xi * xi + be * xi + ce); 
                    Vector3d J(xi * xi * Z, xi * Z, Z); 
                    double error = Z - yi;
                    H += J * J.transpose(); 
                    b += -error * J;
                    cost += error * error;
                }
                cout << "now cost:" << cost << endl;
                stop = (cost < eps);
                lastCost = cost;
                double tmp = 2 * rou - 1;
                tmp = tmp * tmp * tmp;
                mu = mu * max(1/3.0, 1 - tmp);
                v = 2;
            }
            else
            {
                mu = mu * v;
                v = 2 * v;
            }
        }while(rou <= 0 && !stop);
    }

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