#include <iostream> #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值 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 = 100; // 迭代次数 double cost = 0, lastCost = 0; // 本次迭代的cost和上一次迭代的cost for (int iter = 0; iter < iterations; iter++) { Matrix3d H = Matrix3d::Zero(); // Hessian = J^T J in Gauss-Newton Vector3d b = Vector3d::Zero(); // bias cost = 0; 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个数据点的计算误差 error =yi-exp(ae*xi*xi+be*xi+ce); // 填写计算error的表达式 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] = -1*exp(ae*xi*xi+be*xi+ce); // de/dc H += J * J.transpose(); // GN近似的H b += -error * J; // end your code here cost += error * error; } // 求解线性方程 Hx=b,建议用ldlt // start your code here Vector3d dx; dx= H.ldlt().solve(b); // end your code here 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; return 0; }
对应的CMakeLists.txt
cmake_minimum_required( VERSION 2.8 ) project( main ) set( CMAKE_BUILD_TYPE "Release" ) set( CMAKE_CXX_FLAGS "-std=c++11 -O3" ) # 添加Eigen头文件 include_directories( "/usr/include/eigen3" ) # 寻找OpenCV库 find_package( OpenCV 3 REQUIRED ) # 添加头文件 include_directories( ${OpenCV_INCLUDE_DIRS} ) add_executable( gaussnewton gaussnewton.cpp ) target_link_libraries(gaussnewton ${Eigen3_LIBS} ${OpenCV_LIBS} )
实验结果截图: