说明

将演示如何手写高斯牛顿法

源代码

#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 = 2.0, be = -1.0, ce = 5.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));}// 开始Gauss-Newton迭代int iterations = 100;    // 迭代次数double cost = 0, lastCost = 0;  // 本次迭代的cost和上一次迭代的costchrono::steady_clock::time_point t1 = chrono::steady_clock::now();for (int iter = 0; iter < iterations; iter++) {Matrix3d H = Matrix3d::Zero();             // Hessian = J^T W^{-1} J in Gauss-NewtonVector3d b = Vector3d::Zero();             // biascost = 0;for (int i = 0; i < N; i++) {double xi = x_data[i], yi = y_data[i];  // 第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/daJ[1] = -xi * exp(ae * xi * xi + be * xi + ce);  // de/dbJ[2] = -exp(ae * xi * xi + be * xi + ce);  // de/dcH += inv_sigma * inv_sigma * J * J.transpose();b += -inv_sigma * inv_sigma * error * J;cost += error * error;}// 求解线性方程 Hx=bVector3d 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;
}

注：
该部分仅用于学习使用，如有侵权，请联系！