高斯牛顿法实例c++
高斯牛顿法用来解决非线性最优问题的常见算法之一，算法的思想就是通过不断的减少总体残差来实现最优解的求解。用来求解目标函数的解析解。
算法的思路也比较简单：
可以参照原理解释以及原理示例
主要的思想就是不断地朝着梯度的方向改变参数的值直到残差最小化，梯度下降？哈哈哈 果然数学很重要。
示例如下：
1、 y = Ax1 + Bx2**2 A=2 B=1 已知数据点求解最优A B
#include <cstdio>
#include <vector>
#include <opencv2/core/core.hpp>
using namespace std;
using namespace cv;
const double DERIV_STEP = 1e-5;
const int MAX_ITER = 100;
// y = Ax1 + Bx2**2 A=2 B=1
// 3 1 1
// 6 1 2
// 5 2 1
// 1 0 1
// 2 1 0
// 7 3 1
// 11 1 3
void GaussNewton(double(*Func)(const Mat& input, const Mat& input2, const Mat params), // function pointer
const Mat& inputs, const Mat& inputs2, const Mat& outputs, Mat params);
double Deriv(double(*Func)(const Mat& input, const Mat params), // function pointer
const Mat& input, const Mat params, int n);
// The user defines their function here
double Func(const Mat& input, const Mat& input2, const Mat params);
int main()
{
// For this demo we're going to try and fit to the function
// F = A*exp(t*B)
// There are 2 parameters: A B
int num_params = 2;
// Generate random data using these parameters
int total_data = 7;
Mat inputs(total_data, 1, CV_64F);
Mat inputs2(total_data, 1, CV_64F);
Mat outputs(total_data, 1, CV_64F);
//load observation data
//for (int i = 0; i < total_data; i++) {
// inputs.at<double>(i, 0) = i + 1; //load year
//}
//
inputs.at<double>(0, 0) = 1;
inputs.at<double>(1, 0) = 1;
inputs.at<double>(2, 0) = 2;
inputs.at<double>(3, 0) = 0;
inputs.at<double>(4, 0) = 1;
inputs.at<double>(5, 0) = 3;
inputs.at<double>(6, 0) = 1;
inputs2.at<double>(0, 0) = 1;
inputs2.at<double>(1, 0) = 2;
inputs2.at<double>(2, 0) = 1;
inputs2.at<double>(3, 0) = 1;
inputs2.at<double>(4, 0) = 0;
inputs2.at<double>(5, 0) = 1;
inputs2.at<double>(6, 0) = 3;
//load America population
outputs.at<double>(0, 0) = 3;
outputs.at<double>(1, 0) = 6;
outputs.at<double>(2, 0) = 5;
outputs.at<double>(3, 0) = 1;
outputs.at<double>(4, 0) = 2;
outputs.at<double>(5, 0) = 7;
outputs.at<double>(6, 0) = 11;
// Guess the parameters, it should be close to the true value, else it can fail for very sensitive functions!
Mat params(num_params, 1, CV_64F);
//init guess
params.at<double>(0, 0) = 0.1;
params.at<double>(1, 0) = 0.1;
GaussNewton(Func, inputs, inputs2, outputs, params);
printf("Parameters from GaussNewton: %f %f\n", params.at<double>(0, 0), params.at<double>(1, 0));
return 0;
}
double Func(const Mat& input, const Mat& input2, const Mat params)
{
// Assumes input is a single row matrix
// Assumes params is a column matrix
double A = params.at<double>(0, 0);
double B = params.at<double>(1, 0);
double x = input.at<double>(0, 0);
double x2 = input2.at<double>(0, 0);
return A * x + B * pow(x2, 2);
}
//calc the n-th params' partial derivation ， the params are our final target
double Deriv(double(*Func)(const Mat& input, const Mat& input2, const Mat params), const Mat& input, const Mat& input2, const Mat params, int n)
{
// Assumes input is a single row matrix
// Returns the derivative of the nth parameter
Mat params1 = params.clone();
Mat params2 = params.clone();
// Use central difference to get derivative
params1.at<double>(n, 0) -= DERIV_STEP;
params2.at<double>(n, 0) += DERIV_STEP;
double p1 = Func(input, input2, params1);
double p2 = Func(input, input2, params2);
double d = (p2 - p1) / (2 * DERIV_STEP);
return d;
}
void GaussNewton(double(*Func)(const Mat& input, const Mat& input2, const Mat params),
const Mat& inputs, const Mat& inputs2, const Mat& outputs, Mat params)
{
int m = inputs.rows;
int n = inputs.cols;
int num_params = params.rows;
Mat r(m, 1, CV_64F); // residual matrix
Mat Jf(m, num_params, CV_64F); // Jacobian of Func()
Mat input(1, n, CV_64F); // single row input
Mat input2(1, n, CV_64F); // single row input
double last_mse = 0;
for (int i = 0; i < MAX_ITER; i++) {
double mse = 0;
for (int j = 0; j < m; j++) {
for (int k = 0; k < n; k++) {
//copy Independent variable vector, the year
input.at<double>(0, k) = inputs.at<double>(j, k);
input2.at<double>(0, k) = inputs2.at<double>(j, k);
}
r.at<double>(j, 0) = outputs.at<double>(j, 0) - Func(input, input2, params);//diff between estimate and observation population
mse += r.at<double>(j, 0) * r.at<double>(j, 0);
for (int k = 0; k < num_params; k++) {
Jf.at<double>(j, k) = Deriv(Func, input, input2, params, k);
}
}
mse /= m;
// The difference in mse is very small, so quit
if (fabs(mse - last_mse) < 1e-8) {
break;
}
Mat delta = ((Jf.t() * Jf)).inv() * Jf.t() * r;
params += delta;
//printf("%d: mse=%f\n", i, mse);
printf("%d %f\n", i, mse);
last_mse = mse;
}
}
2、对于 n对数据 x1 x2 若 E =sum(loop i in n 3 - yi) , y = Ax1 + Bx2**2, 要E最小，那么y要接近3 及x1 x2 对应的y应该越接近3越好.
#include <cstdio>
#include <vector>
#include <opencv2/core/core.hpp>
using namespace std;
using namespace cv;
const double DERIV_STEP = 1e-5;
const int MAX_ITER = 100;
// y = Ax1 + Bx2**2 A=2 B=1
void GaussNewton(double(*Func)(const Mat& input, const Mat& input2, const Mat params), // function pointer
const Mat& inputs, const Mat& inputs2, const Mat& outputs, Mat params);
double Deriv(double(*Func)(const Mat& input, const Mat params), // function pointer
const Mat& input, const Mat params, int n);
// The user defines their function here
double Func(const Mat& input, const Mat& input2, const Mat params);
int main()
{
// For this demo we're going to try and fit to the function
// F = A*exp(t*B)
// There are 2 parameters: A B
int num_params = 2;
// Generate random data using these parameters
int total_data = 7;
Mat inputs(total_data, 1, CV_64F);
Mat inputs2(total_data, 1, CV_64F);
Mat outputs(total_data, 1, CV_64F);
//load observation data
//for (int i = 0; i < total_data; i++) {
// inputs.at<double>(i, 0) = i + 1; //load year
//}
//
inputs.at<double>(0, 0) = 1.001;
inputs.at<double>(1, 0) = 1.002;
inputs.at<double>(2, 0) = 1.003;
inputs.at<double>(3, 0) = 1.004;
inputs.at<double>(4, 0) = 1.005;
inputs.at<double>(5, 0) = 1.006;
inputs.at<double>(6, 0) = 1.007;
inputs2.at<double>(0, 0) = 1.007;
inputs2.at<double>(1, 0) = 1.006;
inputs2.at<double>(2, 0) = 1.005;
inputs2.at<double>(3, 0) = 1.004;
inputs2.at<double>(4, 0) = 1.003;
inputs2.at<double>(5, 0) = 1.002;
inputs2.at<double>(6, 0) = 1.001;
//load America population
outputs.at<double>(0, 0) = 3;
outputs.at<double>(1, 0) = 3;
outputs.at<double>(2, 0) = 3;
outputs.at<double>(3, 0) = 3;
outputs.at<double>(4, 0) = 3;
outputs.at<double>(5, 0) = 3;
outputs.at<double>(6, 0) = 3;
// Guess the parameters, it should be close to the true value, else it can fail for very sensitive functions!
Mat params(num_params, 1, CV_64F);
//init guess
params.at<double>(0, 0) = 1;
params.at<double>(1, 0) = 1;
GaussNewton(Func, inputs, inputs2, outputs, params);
printf("Parameters from GaussNewton: %f %f\n", params.at<double>(0, 0), params.at<double>(1, 0));
return 0;
}
double Func(const Mat& input, const Mat& input2, const Mat params)
{
// Assumes input is a single row matrix
// Assumes params is a column matrix
double A = params.at<double>(0, 0);
double B = params.at<double>(1, 0);
double x = input.at<double>(0, 0);
double x2 = input2.at<double>(0, 0);
return A * x + B * pow(x2, 2);
}
//calc the n-th params' partial derivation ， the params are our final target
double Deriv(double(*Func)(const Mat& input, const Mat& input2, const Mat params), const Mat& input, const Mat& input2, const Mat params, int n)
{
// Assumes input is a single row matrix n为 0 和 1
// Returns the derivative of the nth parameter
Mat params1 = params.clone();
Mat params2 = params.clone();
// Use central difference to get derivative
params1.at<double>(n, 0) -= DERIV_STEP;
params2.at<double>(n, 0) += DERIV_STEP;
double p1 = Func(input, input2, params1);
double p2 = Func(input, input2, params2);
double d = (p2 - p1) / (2 * DERIV_STEP);
return d;
}
void GaussNewton(double(*Func)(const Mat& input, const Mat& input2, const Mat params),
const Mat& inputs, const Mat& inputs2, const Mat& outputs, Mat params)
{
int m = inputs.rows;
int n = inputs.cols;
int num_params = params.rows;
Mat r(m, 1, CV_64F); // residual matrix
Mat Jf(m, num_params, CV_64F); // Jacobian of Func()
Mat input(1, n, CV_64F); // single row input
Mat input2(1, n, CV_64F); // single row input
double last_mse = 0;
for (int i = 0; i < MAX_ITER; i++) {
double mse = 0;
for (int j = 0; j < m; j++) {
//for (int k = 0; k < n; k++) {//copy Independent variable vector, the year
input.at<double>(0, 0) = inputs.at<double>(j, 0);
input2.at<double>(0, 0) = inputs2.at<double>(j, 0);
//}
r.at<double>(j, 0) = outputs.at<double>(j, 0) - Func(input, input2, params);//diff between estimate and observation population
mse += r.at<double>(j, 0) * r.at<double>(j, 0);
//for (int k = 0; k < num_params; k++) {
Jf.at<double>(j, 0) = Deriv(Func, input, input2, params, 0);
Jf.at<double>(j, 1) = Deriv(Func, input, input2, params, 1);
//}
}
mse /= m;
// The difference in mse is very small, so quit
if (fabs(mse - last_mse) < 1e-20) {
break;
}
Mat delta = ((Jf.t() * Jf)).inv() * Jf.t() * r;
params += delta;
//printf("%d: mse=%f\n", i, mse);
printf("%d %f\n", i, mse);
last_mse = mse;
}
}