視覺十四講：第六講_手寫高斯牛頓法

1.優化問題：

\(y=exp(ax^{2}+bx+c)+w\),由y和x，求解a,b,c

誤差為：\(e_{i}=y_{i}-exp(ax_{i}^{2}+bx_{i}+c)\)

誤差項對每一個待估計量進行求導：

\(\frac{\partial e_{i}}{\partial a}=-x^{2}_{i}exp(ax^{2}_{i}+bx_{i}+c)\)
\(\frac{\partial e_{i}}{\partial b}=-x_{i}exp(ax^{2}_{i}+bx_{i}+c)\)
\(\frac{\partial e_{i}}{\partial c}=-exp(ax^{2}_{i}+bx_{i}+c)\)

雅可比矩陣\(J_{i}=[\frac{\partial e_{i}}{\partial a},\frac{\partial e_{i}}{\partial b},\frac{\partial e_{i}}{\partial c}]^{T}\),
高斯的增量方程為： \((\displaystyle \sum^{100}_{i=1} J_{i}(\sigma^{2})^{-1} J_{i}^{T})\Delta x_{k}=\displaystyle \sum^{100}_{i=1} -J_{i}(\sigma^{2})^{-1} e_{i}\)
\(H\Delta x_{k}=b\)

噪聲滿足 w ~ （ 0，\(\sigma^{2}\) ）

#include <iostream>
#include <chrono>
#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 = 20.0, be = -10.0, ce = 10.0; //初始值，不能太大，初始化很重要

    int N = 100;   //數據總點數

    double w_sigma = 1.0;  //噪聲sigma值
    double inv_sigma = 1.0 / w_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*w_sigma) );

    }

    int iterations = 100;  //迭代次數
    double cost = 0, lastCost= 0;  //每次迭代的誤差平方和，用於判斷退出迭代次數

    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    for ( int iter=0; iter<iterations; iter++ ){
        Matrix3d H = Matrix3d::Zero();
        Vector3d b = Vector3d::Zero();
        cost = 0;

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

            cost += error * error;
        }

        //求解線性方程  Hx = b
        Vector3d dx = H.ldlt().solve(b);
        if (isnan(dx[0])) {
            cout << "result is nan!" << endl;
            break;
        }

        if (iter > 0 && cost >= lastCost) {   //誤差變大，找到最小值，退出迭代
            cout << "cost: " << cost << ">= last 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\testimated params: " << ae << "," << be << "," << ce << endl;
    }
    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;
}

CMakelists.txt:

cmake_minimum_required(VERSION 2.8)
project(gaussnewton)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})
include_directories("/usr/include/eigen3")
set(SOURCE_FILES main.cpp)
add_executable(gaussnewton ${SOURCE_FILES})
target_link_libraries(gaussnewton ${OpenCV_LIBS})