高斯牛顿法.md

高斯牛顿法

手写高斯牛顿法

CMakeLists

cmake_minimum_required(VERSION 3.15) 
project(GuassNewton) 

set(CMAKE_CXX_STANDARD 14) 

#OpenCV 
find_package(OpenCV REQUIRED) 
include_directories(${OpenCV_INCLUDE_DIRS}) 

#Eigen 
include_directories("/usr/include/eigen3") 

add_executable(GuassNewton main.cpp) 
target_link_libraries(GuassNewton ${OpenCV_LIBS}) 

.cpp文件

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

using namespace std; 
using namespace Eigen; 

int main() { 
    //设定曲线真实参数 
    double ar = 1.0, br = 2.0, cr = 1.0; 
    //给定曲线参数优化初始估计值 
    double ae = 2.0, be = -1.0, ce = 5.0; 
    //设定数据点个数 
    int N = 100; 
    //设定噪声服从的正态分布的sigma值 
    double w_sigma = 1.0; 
    //计算sigma的倒数，之后用于误差归一化 
    double inv_sigma = 1.0 / w_sigma; 
    //OpenCV随机数产生器 
    cv::RNG rng; 

    //初始化数据容器，容器内元素类型为double 
    vector<double> x_data, y_data; 
    //生成N个数据点 
    for (int i=0; i < N; ++i){ 
        //x在0-1之间均匀取100个值 
        double x = i / 100.0; 
        x_data.push_back(x); 
        //y用真实函数生成再加上高斯噪声 
        y_data.push_back(exp(ar*x*x+br*x+cr)+rng.gaussian(w_sigma * w_sigma)); 
    } 

    //开始高斯牛顿迭代 
    //设定迭代次数 
    int iterations = 100; 
    //本次迭代和上次迭代的cost 
    double cost = 0, lastcost = 0; 

    //开始及时，当前时间点存储到t1中 
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now(); 

    //牛顿高斯算法迭代iterations次 
    for (int iter = 0; iter < iterations; ++iter) { 
        //初始化H矩阵，b矩阵,雅克比矩阵J和cost 
        Matrix3d H = Matrix3d::Zero(); 
        Vector3d b = Vector3d::Zero(); 
        cost = 0; 

        //对N个数据点进行处理，列出总的增量方程，计算初始误差 
        for (int i = 0; i < N; ++i) { 
            double xi = x_data[i], yi = y_data[i]; 
            double error = yi - exp(ae * xi * xi + be * xi + ce); 
            //计算雅克比矩阵在该点取值 
            Vector3d J; 
            J[0] = -xi * xi * exp(ae * xi * xi + be * xi + ce);  // de/da 
            J[1] = -xi * exp(ae * xi * xi + be * xi + ce);   // de/db 
            J[2] = -exp(ae * xi * xi + be * xi + ce);   // de/dc 

            H += inv_sigma * inv_sigma * J * J.transpose();   //这里除以sigma是归一化 
            b += -inv_sigma * inv_sigma * error * J; 

            cost += error * error; 
        } 

        //求解线性方程Hx=b 
        Vector3d dx = H.ldlt().solve(b); 
        //如果方程无解，那么dx[0]是非法字符nan，退出迭代 
        if (isnan(dx[0])) { 
            cout << "result is nan!" << endl; 
            break; 
        } 

        //如果本次迭代误差大于上次误差，算法结束，退出迭代 
        if(iter > 0 && cost >= lastcost){ 
            cout << "cost:" << cost << ">=" << lastcost << ",break." << endl; 
            break; 
        } 

 
        //进行估计参数的增量更新，存储本次代价 
        ae += dx[0]; 
        be += dx[1]; 
        ce += dx[2]; 

        lastcost = cost; 
        //输出本次迭代信息 
        cout << "total cost:" << cost << ",\t\tupdate:" << dx.transpose() << "\t\testimatec:" << ae << "," << be << 
        "," << ce << endl; 
    } 

    //及时结束，获取当前时间赋给t2 
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now(); 
    //计算算法耗时并输出 
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1); 
    cout << "solve time cost = " << time_used.count() << " seconds. " << endl; 

    //输出最终算法迭代结果 
    cout << "estimated abc = " << ae << ", " << be << ", " << ce << endl; 
    return 0; 
}