高斯牛頓迭代(GaussNewton)程式碼實現
阿新 • • 發佈:2020-09-16
#include <cstdio> #include <vector> #include <iostream> #include <opencv2/core/core.hpp> using namespace std; using namespace cv; const double DELTAX = 1e-5; const int MAXCOUNT = 100; double function(const Mat &input, const Mat params) { //給定函式已知x求y double a = params.at<double>(0,0); double b = params.at<double>(1,0); double c = params.at<double>(2,0); double x = input.at<double>(0,0); return exp( a*x*x + b*x + c ); } double derivative(double(*function)(const Mat &input, const Mat params), const Mat &input, constMat params, int n) { // 用增加分量的方式求導數 Mat params1 = params.clone(); Mat params2 = params.clone(); params1.at<double>(n,0) -= DELTAX; params2.at<double>(n,0) += DELTAX; double y1 = function(input, params1); double y2 = function(input, params2); double deri = (y2 - y1) / (2*DELTAX); return deri; } void gaussNewton(double(*function)(const Mat &input, const Mat ms), const Mat &inputs, const Mat &outputs, Mat params) { int num_estimates = inputs.rows; int num_params = params.rows; Mat r(num_estimates, 1, CV_64F); // 殘差 Mat Jf(num_estimates, num_params, CV_64F); // 雅克比矩陣 Mat input(1, 1, CV_64F); double lsumR = 0; for(int i = 0; i < MAXCOUNT; i++) { double sumR = 0; for(int j = 0; j < num_estimates; j++) { input.at<double>(0,0) = inputs.at<double>(j,0); r.at<double>(j,0) = outputs.at<double>(j,0) - function(input, params);// 計算殘差矩陣 sumR += fabs(r.at<double>(j,0)); // 殘差累加 for(int k = 0; k < num_params; k++) { Jf.at<double>(j,k) = derivative(function, input, params, k); // 根據新引數重新計算雅克比矩陣 } } sumR /= num_estimates; //均殘差 if(fabs(sumR - lsumR) < 1e-8) //均殘差足夠小達到收斂 { break; } Mat delta = ((Jf.t()*Jf)).inv() * Jf.t()*r;// ((Jf.t()*Jf)) 近似黑塞矩陣 params += delta; lsumR = sumR; } } int main() { // F = exp ( a*x*x + b*x + c ) int num_params = 3; Mat params(num_params, 1, CV_64F); //abc引數的實際值 params.at<double>(0,0) = 1.0; //a params.at<double>(1,0) = 2.0; //b params.at<double>(2,0) = 1.0; //c cout<<"real("<<"a:"<< params.at<double>(0,0) <<" b:"<< params.at<double>(1,0) << " c:"<< params.at<double>(2,0) << ")"<< endl; int N = 100; double w_sigma = 1.0; // 噪聲Sigma值 cv::RNG rng; // OpenCV隨機數產生器 Mat estimate_x(N, 1, CV_64F); Mat estimate_y(N, 1, CV_64F); for ( int i = 0; i < N; i++ ) { double x = i/100.0; estimate_x.at<double>(i,0) = x; Mat paramX(1, 1, CV_64F); paramX.at<double>(0,0) = x; estimate_y.at<double>(i,0) = function(paramX, params) + rng.gaussian ( w_sigma ); } //abc引數的初值 params.at<double>(0,0) = 0; //a params.at<double>(1,0) = 0; //b params.at<double>(2,0) = 0; //c cout<<"init("<<"a:"<< params.at<double>(0,0) <<" b:"<< params.at<double>(1,0) << " c:"<< params.at<double>(2,0) << ")"<< endl; gaussNewton(function, estimate_x, estimate_y, params); cout<<"result("<<"a:"<< params.at<double>(0,0) <<" b:"<< params.at<double>(1,0) << " c:"<< params.at<double>(2,0) << ")"<< endl; return 0; }
# Project: GaussNewtonDemo # # All rights reserved. cmake_minimum_required( VERSION 2.6 ) cmake_policy( SET CMP0004 OLD ) ### initialize project ########################################################################################### SET(CMAKE_BUILD_TYPE "Debug") SET(CMAKE_CXX_FLAGS_DEBUG "$ENV{CXXFLAGS} -O0 -Wall -g2 -ggdb") SET(CMAKE_CXX_FLAGS_RELEASE "$ENV{CXXFLAGS} -O3 -Wall") project(GaussNewtonDemo) find_package(Eigen3 REQUIRED) find_package(OpenCV REQUIRED) set(CMAKE_INSTALL_PREFIX /usr) set(BUILD_SHARED_LIBS on) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC -O0") include_directories( ${EIGEN3_INCLUDE_DIR} ${OpenCV_INCLUDE_DIR}) add_definitions( "-DPREFIX=\"${CMAKE_INSTALL_PREFIX}\"" ) ### global default options ####################################################################################### set(SOURCES main.cpp ) add_executable(GaussNewtonDemo ${SOURCES}) TARGET_LINK_LIBRARIES( GaussNewtonDemo ${OpenCV_LIBS} )