目录
- 1 高斯牛顿法
- 1.1 目的
- 1.2 步骤
- 1.3 最终更新公式
- 2. LM
- 2.1 整体流程
- 2.2 算法介绍
- 2.3 opencv的实现流程
- 3 实现
- 3.1 opencv实现
- 3.2 python 实现
- 3.3 基于eigen实现
1 高斯牛顿法
高斯牛顿法是线性搜索与信赖阈搜索的优化方法基础，它将损失函数进行一阶展开：
\[f(x + \bigtriangleup x) \approx {f(x) + J(x) \bigtriangleup x}
\]
这里\(J(x)\)实际为\(m\times{n}\)矩阵，\(f(x)\)为一个参数函数\(r_ᵢ(x) = yᵢ - M(x)\)
1.1 目的
寻找\(\bigtriangleup x\)使得\(||f(x+\bigtriangleup x)||^2\)最小化，即转换为优化问题：
\[\bigtriangleup \hat{x}=arg \min_{\bigtriangleup x} \frac{1}{2} ||{f(x) + J(x) \bigtriangleup x}||^2
\]
1.2 步骤
设\(F(x)=\frac{1}{2}||f(x) + J(x)\bigtriangleup x||^2\)
\[F(x)=(f(x) + J(x)\bigtriangleup x)^T(f(x) + J(x) \bigtriangleup x)\\
= (f^T(x) + \bigtriangleup^Tx J^T(x))(f(x) + J(x) \bigtriangleup x)\\
= f^T(x) f(x) + f^T(x) J(x)\bigtriangleup x + \bigtriangleup ^TxJ^T(x) f(x) + \bigtriangleup ^Tx J^T(x)J(x) \bigtriangleup x\\
= f^2(x) + f^T(x)J(x)\bigtriangleup x + \bigtriangleup ^Tx J^T(x)J(x) \bigtriangleup x \\
= f^2(x) + 2f^T(x)J(x) \bigtriangleup x + \bigtriangleup ^Tx J^T(x)J(x) \bigtriangleup x
\]
这里解释下：
\[f(x)\in R^{n\times 1}\\
f(x)为残差，例如(y-y')^2\\
x\in R^{n \times 1} \\
f^T(x)J(x)\bigtriangleup x为标量\\
J(x) = \begin{bmatrix}
\frac{\partial r_1}{\partial x_1} & ... & \frac{\partial r_1}{\partial x_n} \\\vdots & \vdots & \vdots \\\frac{\partial r_m}{\partial x_1} & ... & \frac{\partial r_m}{\partial x_n} \\
\end{bmatrix}
\]
1.3 最终更新公式
\[\frac{\partial F}{\partial {\bigtriangleup x}} = 2J^T(x)f(x) + 2J^T(x)J(x){\bigtriangleup x}=0\\
J^T(x)J(x){\bigtriangleup x}=-J^T(x)f(x)
\]
2. LM
2.1 整体流程
2.2 算法介绍
- LM算法给增量\(Δx\)添加了一个信赖域，即
\[\min_{Δx}\frac{1}{2}||f(x_k) + J(x_k)Δx_k||^2 ,s.t ||DΔx_K||^2<=\mu
\]
- 拉格朗日乘子法转换为无约束的问题：
\[\min_{Δx}\frac{1}{2}||f(x_k) + J(x_k)Δx_k||^2 +\lambda/2||DΔx_K||^2 \\
\Longrightarrow H Δx+\lambda D^TDΔx=g
\]
- LM算法实际求解的是：\[(J^TJ+\lambda diag(J^TJ) \bigtriangleup = J^Tr \]
- 然后：\[x_{new} = x_{prev} - \bigtriangleup \]
- 注意
- 用的是 diag *= (1+λ)，不是 + λI；前者是椭圆的球形信赖域，后者为球形
- 解的是 masked 子空间
- 默认 SVD
2.3 opencv的实现流程
STARTED└─ 返回 param + J + err（算 J）
CALC_J├─ JtJ, JtErr├─ prevParam = param├─ step() 计算(J^T*J + λ*I) * Δx = -J^T*e中的Δx，并更新到状态变量x_└─ 返回 param + err（算 err）
CHECK_ERR├─ if err > prevErr│ ├─ lambda++│ ├─ step()│ └─ 返回 param + err└─ else（接受）├─ lambda--├─ 收敛判断└─ 返回 param + J + err3 实现
3.1 opencv实现
class CV_EXPORTS CvLevMarq
{
public:CvLevMarq();CvLevMarq( int nparams, int nerrs, CvTermCriteria criteria=cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER,30,DBL_EPSILON),bool completeSymmFlag=false );~CvLevMarq();/** 功能：* 参数：* [in] nparams 优化变量的参数* [in] nerrs nerrs代表样本的个数（一个测量点代表两个样本x和y）。nerrs = 0，优化时使用updateAlt函数，因为此函数不关心样本个数（JtJ外部计算，自然不需要知道J的大小）nerrs > 0，优化时使用update函数，应为内部需要使用J的大小，所以J的维度需要提前声明，其维度大小为nerrs*nparams* [in] criteria 迭代停止标准* [in] completeSymmFlag 当nerrs=0时有效。防止外部计算得到的JtJ不对称。此标记位不管是false和true都会使JtJ 变为对称。** 返回值：** 备注：* 此函数相对于updateAlt函数，计算相对简单，但自己自由发挥的空间较少。例如代权重的最小二乘此函数就无法实现。**/void init( int nparams, int nerrs, CvTermCriteria criteria=cvTermCriteria(CV_TERMCRIT_EPS+CV_TERMCRIT_ITER,30,DBL_EPSILON),bool completeSymmFlag=false );/** 功能： * 参数：* [out] param 需要优化的参数，根据内部参数的优化状况，对当前参数进行输出，方便外部误差的计算* [in] J 目标函数的偏导数，内部计算JtJ* [in] err 内部计算JtErr 和 errNorm** 返回值：** 备注：* 此函数相对于updateAlt函数，计算相对简单，但自己自由发挥的空间较少。例如代权重的最小二乘此函数就无法实现。**/bool update( const CvMat*& param, CvMat*& J, CvMat*& err );/** 功能：* 参数：* [out] param 需要优化的参数，根据内部参数的优化状况，对当前参数进行输出，方便外部误差的计算* [in] JtJ 正规方程左侧的部分，此变量与类内部成员变量关联，在此函数后面对其赋值。可以变成J'ΩJ* [in] JtErr 正规方程右侧的部分，此变量与类内部成员变量关联，在此函数后面对其赋值。* [in] errNorm 测量误差（测量值减去计算出的测量值的模），此变量与类内部成员变量关联，在此函数后面对其赋值。** 返回值：** 备注：* 目标函数的导数，正规方程的左侧和右侧计算都是在外部完成计算的，测量误差的计算也是在外部完成的。 */bool updateAlt( const CvMat*& param, CvMat*& JtJ, CvMat*& JtErr, double*& errNorm ); // 控制优化过程中的逻辑步骤（DONE, STARTED, CALC_J, CHECK_ERR）void clear();void step(); // 计算正规方程的解，用SVD计算enum { DONE=0, STARTED=1, CALC_J=2, CHECK_ERR=3 };cv::Ptr<CvMat> mask; // 标记优化过程中不优化的量。0代表不优化，1代表优化。大小跟param相同cv::Ptr<CvMat> prevParam; // 前一步的优化参数，用途有两个：1、判断变量是否在变化，不变化停止迭代。2、在此参数上减去一个高斯方向得到下一个参数。cv::Ptr<CvMat> param; // 所有要优化的变脸集合cv::Ptr<CvMat> J; // 目标函数的偏导数cv::Ptr<CvMat> err; // 测量误差向量cv::Ptr<CvMat> JtJ; // 正规方程左侧的部分cv::Ptr<CvMat> JtJN; // 去掉非优化变量后的正规方程左侧部分cv::Ptr<CvMat> JtErr; // 正规方程右侧的部分 cv::Ptr<CvMat> JtJV; // 去掉非优化变量的JtErr，cv::Ptr<CvMat> JtJW; // 去掉非优化变量后待求向量double prevErrNorm, errNorm; // 测量误差，更新前的测量误差，更新后的测量误差int lambdaLg10; // LM 算法中的lanbda，此处的lamdaLg10 加1代表lambda变大10倍，减1缩小10倍CvTermCriteria criteria;int state; // 优化步骤中的状态int iters; // 记录迭代的次数bool completeSymmFlag; // 使去掉非优化变量后的JtJN变成对称矩阵，只是在updateAlt函数是有用。int solveMethod; // 正规方程的解法
};
CvLevMarq::CvLevMarq()
{lambdaLg10 = 0; state = DONE;criteria = cvTermCriteria(0,0,0);iters = 0;completeSymmFlag = false;errNorm = prevErrNorm = DBL_MAX;solveMethod = cv::DECOMP_SVD;
}CvLevMarq::CvLevMarq( int nparams, int nerrs, CvTermCriteria criteria0, bool _completeSymmFlag )
{init(nparams, nerrs, criteria0, _completeSymmFlag);
}void CvLevMarq::clear()
{mask.release();prevParam.release();param.release();J.release();err.release();JtJ.release();JtJN.release();JtErr.release();JtJV.release();JtJW.release();
}CvLevMarq::~CvLevMarq()
{clear();
}void CvLevMarq::init( int nparams, int nerrs, CvTermCriteria criteria0, bool _completeSymmFlag )
{if( !param || param->rows != nparams || nerrs != (err ? err->rows : 0) )clear();mask.reset(cvCreateMat( nparams, 1, CV_8U ));cvSet(mask, cvScalarAll(1));prevParam.reset(cvCreateMat( nparams, 1, CV_64F ));param.reset(cvCreateMat( nparams, 1, CV_64F ));JtJ.reset(cvCreateMat( nparams, nparams, CV_64F ));JtErr.reset(cvCreateMat( nparams, 1, CV_64F ));if( nerrs > 0 ){J.reset(cvCreateMat( nerrs, nparams, CV_64F ));err.reset(cvCreateMat( nerrs, 1, CV_64F ));}errNorm = prevErrNorm = DBL_MAX;lambdaLg10 = -3;criteria = criteria0;if( criteria.type & CV_TERMCRIT_ITER )criteria.max_iter = MIN(MAX(criteria.max_iter,1),1000);elsecriteria.max_iter = 30;if( criteria.type & CV_TERMCRIT_EPS )criteria.epsilon = MAX(criteria.epsilon, 0);elsecriteria.epsilon = DBL_EPSILON;state = STARTED;iters = 0;completeSymmFlag = _completeSymmFlag;solveMethod = cv::DECOMP_SVD;
}bool CvLevMarq::update( const CvMat*& _param, CvMat*& matJ, CvMat*& _err )
{matJ = _err = 0;CV_Assert( !err.empty() );if( state == DONE ){_param = param;return false;}if( state == STARTED ){_param = param;cvZero( J );cvZero( err );matJ = J;_err = err;state = CALC_J;return true;}if( state == CALC_J ){cvMulTransposed( J, JtJ, 1 );cvGEMM( J, err, 1, 0, 0, JtErr, CV_GEMM_A_T );cvCopy( param, prevParam );step();if( iters == 0 )prevErrNorm = cvNorm(err, 0, CV_L2);_param = param;cvZero( err );_err = err;state = CHECK_ERR;return true;}CV_Assert( state == CHECK_ERR );errNorm = cvNorm( err, 0, CV_L2 );if( errNorm > prevErrNorm ){if( ++lambdaLg10 <= 16 ){step();_param = param;cvZero( err );_err = err;state = CHECK_ERR;return true;}}lambdaLg10 = MAX(lambdaLg10-1, -16);if( ++iters >= criteria.max_iter ||cvNorm(param, prevParam, CV_RELATIVE_L2) < criteria.epsilon ){_param = param;state = DONE;return true;}prevErrNorm = errNorm;_param = param;cvZero(J);matJ = J;_err = err;state = CALC_J;return true;
}namespace {
static void subMatrix(const cv::Mat& src, cv::Mat& dst, const std::vector<uchar>& cols,const std::vector<uchar>& rows) {int nonzeros_cols = cv::countNonZero(cols);cv::Mat tmp(src.rows, nonzeros_cols, CV_64FC1);for (int i = 0, j = 0; i < (int)cols.size(); i++){if (cols[i]){src.col(i).copyTo(tmp.col(j++));}}int nonzeros_rows = cv::countNonZero(rows);dst.create(nonzeros_rows, nonzeros_cols, CV_64FC1);for (int i = 0, j = 0; i < (int)rows.size(); i++){if (rows[i]){tmp.row(i).copyTo(dst.row(j++));}}
}}void CvLevMarq::step()
{using namespace cv;const double LOG10 = log(10.);double lambda = exp(lambdaLg10*LOG10);int nparams = param->rows;Mat _JtJ = cvarrToMat(JtJ);Mat _mask = cvarrToMat(mask);int nparams_nz = countNonZero(_mask);if(!JtJN || JtJN->rows != nparams_nz) {// prevent re-allocation in every stepJtJN.reset(cvCreateMat( nparams_nz, nparams_nz, CV_64F ));JtJV.reset(cvCreateMat( nparams_nz, 1, CV_64F ));JtJW.reset(cvCreateMat( nparams_nz, 1, CV_64F ));}Mat _JtJN = cvarrToMat(JtJN);Mat _JtErr = cvarrToMat(JtJV);Mat_<double> nonzero_param = cvarrToMat(JtJW);subMatrix(cvarrToMat(JtErr), _JtErr, std::vector<uchar>(1, 1), _mask);subMatrix(_JtJ, _JtJN, _mask, _mask);if( !err )completeSymm( _JtJN, completeSymmFlag );_JtJN.diag() *= 1. + lambda;solve(_JtJN, _ΔxJtErr, nonzero_param, solveMethod);int j = 0;for( int i = 0; i < nparams; i++ )param->data.db[i] = prevParam->data.db[i] - (mask->data.ptr[i] ? nonzero_param(j++) : 0);
}3.2 python 实现
'''
#Implement LM algorithm only using basic python
#Author:Leo Ma
#For csmath2019 assignment4,ZheJiang University
#Date:2019.04.28
'''
import numpy as np
import matplotlib.pyplot as plt#input data, whose shape is (num_data,1)
#data_input=np.array([[0.25, 0.5, 1, 1.5, 2, 3, 4, 6, 8]]).T
#data_output=np.array([[19.21, 18.15, 15.36, 14.10, 12.89, 9.32, 7.45, 5.24, 3.01]]).Ttao = 10**-3
threshold_stop = 10**-15
threshold_step = 10**-15
threshold_residual = 10**-15
residual_memory = []#construct a user function
def my_Func(params,input_data):a = params[0,0]b = params[1,0]#c = params[2,0]#d = params[3,0]return a*np.exp(b*input_data)#return a*np.sin(b*input_data[:,0])+c*np.cos(d*input_data[:,1])#generating the input_data and output_data,whose shape both is (num_data,1)
def generate_data(params,num_data):x = np.array(np.linspace(0,10,num_data)).reshape(num_data,1) # 产生包含噪声的数据mid,sigma = 0,5y = my_Func(params,x) + np.random.normal(mid, sigma, num_data).reshape(num_data,1)return x,y#calculating the derive of pointed parameter,whose shape is (num_data,1)
def cal_deriv(params,input_data,param_index):params1 = params.copy()params2 = params.copy()params1[param_index,0] += 0.000001params2[param_index,0] -= 0.000001data_est_output1 = my_Func(params1,input_data)data_est_output2 = my_Func(params2,input_data)return (data_est_output1 - data_est_output2) / 0.000002#calculating jacobian matrix,whose shape is (num_data,num_params)
def cal_Jacobian(params,input_data):num_params = np.shape(params)[0]num_data = np.shape(input_data)[0]J = np.zeros((num_data,num_params))for i in range(0,num_params):J[:,i] = np.asarray(list(cal_deriv(params,input_data,i))).reshape((100))return J#calculating residual, whose shape is (num_data,1)
def cal_residual(params,input_data,output_data):data_est_output = my_Func(params,input_data)residual = output_data - data_est_outputreturn residual'''
#calculating Hessian matrix, whose shape is (num_params,num_params)
def cal_Hessian_LM(Jacobian,u,num_params):H = Jacobian.T.dot(Jacobian) + u*np.eye(num_params)return H#calculating g, whose shape is (num_params,1)
def cal_g(Jacobian,residual):g = Jacobian.T.dot(residual)return g#calculating s,whose shape is (num_params,1)
def cal_step(Hessian_LM,g):s = Hessian_LM.I.dot(g)return s'''#get the init u, using equation u=tao*max(Aii)
def get_init_u(A,tao):m = np.shape(A)[0]Aii = []for i in range(0,m):Aii.append(A[i,i])u = tao*max(Aii)return u#LM algorithm
def LM(num_iter,params,input_data,output_data):num_params = np.shape(params)[0]#the number of paramsk = 0#set the init iter count is 0#calculating the init residualresidual = cal_residual(params,input_data,output_data)#calculating the init Jocobian matrixJacobian = cal_Jacobian(params,input_data)A = Jacobian.T.dot(Jacobian)#calculating the init Ag = Jacobian.T.dot(residual)#calculating the init gradient gstop = (np.linalg.norm(g, ord=np.inf) <= threshold_stop)#set the init stopu = get_init_u(A,tao)#set the init uv = 2#set the init v=2while((not stop) and (k<num_iter)):k+=1while(1):Hessian_LM = A + u*np.eye(num_params)#calculating Hessian matrix in LMstep = np.linalg.inv(Hessian_LM).dot(g)#calculating the update stepif(np.linalg.norm(step) <= threshold_step):stop = Trueelse:new_params = params + step#update params using stepnew_residual = cal_residual(new_params,input_data,output_data)#get new residual using new paramsrou = (np.linalg.norm(residual)**2 - np.linalg.norm(new_residual)**2) / (step.T.dot(u*step+g))if rou > 0:params = new_paramsresidual = new_residualresidual_memory.append(np.linalg.norm(residual)**2)#print (np.linalg.norm(new_residual)**2)Jacobian = cal_Jacobian(params,input_data)#recalculating Jacobian matrix with new paramsA = Jacobian.T.dot(Jacobian)#recalculating Ag = Jacobian.T.dot(residual)#recalculating gradient gstop = (np.linalg.norm(g, ord=np.inf) <= threshold_stop) or (np.linalg.norm(residual)**2 <= threshold_residual)u = u*max(1/3,1-(2*rou-1)**3)v = 2else:u = u*vv = 2*vif(rou > 0 or stop):break;return paramsdef main():#set the true params for generate_data() functionparams = np.zeros((2,1))params[0,0]=10.0params[1,0]=0.8num_data = 100# set the data numberdata_input,data_output = generate_data(params,num_data)#generate data as requested#set the init params for LM algorithm params[0,0]=6.0params[1,0]=0.3#using LM algorithm estimate paramsnum_iter=100 # the number of iterationest_params = LM(num_iter,params,data_input,data_output)print(est_params)a_est=est_params[0,0]b_est=est_params[1,0]#画个图看看状况plt.scatter(data_input, data_output, color='b')x = np.arange(0, 100) * 0.1 #生成0-10的共100个数据，然后设置间距为0.1plt.plot(x,a_est*np.exp(b_est*x),'r',lw=1.0)plt.xlabel("2018.06.13")plt.savefig("result_LM.png")plt.show()plt.plot(residual_memory)plt.xlabel("2018.06.13")plt.savefig("error-iter.png")plt.show()if __name__ == '__main__':main()
3.3 基于eigen实现
/*** This file is part of LevenbergMarquardt Solver.** Copyright (C) 2018-2020 Dongsheng Yang <ydsf16@buaa.edu.cn> (Beihang University)* For more information see <https://github.com/ydsf16/LevenbergMarquardt>** LevenbergMarquardt is free software: you can redistribute it and/or modify* it under the terms of the GNU General Public License as published by* the Free Software Foundation, either version 3 of the License, or* (at your option) any later version.** LevenbergMarquardt is distributed in the hope that it will be useful,* but WITHOUT ANY WARRANTY; without even the implied warranty of* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the* GNU General Public License for more details.** You should have received a copy of the GNU General Public License* along with LevenbergMarquardt. If not, see <http://www.gnu.org/licenses/>.*/#include <iostream>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include <opencv2/opencv.hpp>
#include <eigen3/Eigen/Cholesky>
#include <chrono>/* 计时类 */
class Runtimer{
public:inline void start(){t_s_ = std::chrono::steady_clock::now();}inline void stop(){t_e_ = std::chrono::steady_clock::now();}inline double duration(){return std::chrono::duration_cast<std::chrono::duration<double>>(t_e_ - t_s_).count() * 1000.0;}private:std::chrono::steady_clock::time_point t_s_; //start time ponitstd::chrono::steady_clock::time_point t_e_; //stop time point
};/* 优化方程 */
class LevenbergMarquardt{
public:LevenbergMarquardt(double* a, double* b, double* c):a_(a), b_(b), c_(c){epsilon_1_ = 1e-6;epsilon_2_ = 1e-6;max_iter_ = 50;is_out_ = true;}void setParameters(double epsilon_1, double epsilon_2, int max_iter, bool is_out){epsilon_1_ = epsilon_1;epsilon_2_ = epsilon_2;max_iter_ = max_iter;is_out_ = is_out;}void addObservation(const double& x, const double& y){obs_.push_back(Eigen::Vector2d(x, y));}void calcJ_fx(){J_ .resize(obs_.size(), 3);fx_.resize(obs_.size(), 1);for ( size_t i = 0; i < obs_.size(); i ++){const Eigen::Vector2d& ob = obs_.at(i);const double& x = ob(0);const double& y = ob(1);double j1 = -x*x*exp(*a_ * x*x + *b_*x + *c_);double j2 = -x*exp(*a_ * x*x + *b_*x + *c_);double j3 = -exp(*a_ * x*x + *b_*x + *c_);J_(i, 0 ) = j1;J_(i, 1) = j2;J_(i, 2) = j3;fx_(i, 0) = y - exp( *a_ *x*x + *b_*x +*c_);}}void calcH_g(){H_ = J_.transpose() * J_;g_ = -J_.transpose() * fx_;}double getCost(){Eigen::MatrixXd cost= fx_.transpose() * fx_;return cost(0,0);}double F(double a, double b, double c){Eigen::MatrixXd fx;fx.resize(obs_.size(), 1);for ( size_t i = 0; i < obs_.size(); i ++){const Eigen::Vector2d& ob = obs_.at(i);const double& x = ob(0);const double& y = ob(1);fx(i, 0) = y - exp( a *x*x + b*x +c);}Eigen::MatrixXd F = 0.5 * fx.transpose() * fx;return F(0,0);}double L0_L( Eigen::Vector3d& h){Eigen::MatrixXd L = -h.transpose() * J_.transpose() * fx_ - 0.5 * h.transpose() * J_.transpose() * J_ * h;return L(0,0);}void solve(){int k = 0;double nu = 2.0;calcJ_fx();calcH_g();bool found = ( g_.lpNorm<Eigen::Infinity>() < epsilon_1_ );std::vector<double> A;A.push_back( H_(0, 0) );A.push_back( H_(1, 1) );A.push_back( H_(2,2) );auto max_p = std::max_element(A.begin(), A.end());double mu = *max_p;double sumt =0;while ( !found && k < max_iter_){Runtimer t;t.start();k = k +1;Eigen::Matrix3d G = H_ + mu * Eigen::Matrix3d::Identity();Eigen::Vector3d h = G.ldlt().solve(g_);if( h.norm() <= epsilon_2_ * ( sqrt(*a_**a_ + *b_**b_ + *c_**c_ ) +epsilon_2_ ) )found = true;else{double na = *a_ + h(0);double nb = *b_ + h(1);double nc = *c_ + h(2);double rho =( F(*a_, *b_, *c_) - F(na, nb, nc) ) / L0_L(h);if( rho > 0){*a_ = na;*b_ = nb;*c_ = nc;calcJ_fx();calcH_g();found = ( g_.lpNorm<Eigen::Infinity>() < epsilon_1_ );mu = mu * std::max<double>(0.33, 1 - std::pow(2*rho -1, 3));nu = 2.0;}else{mu = mu * nu; nu = 2*nu;}// if rho > 0}// if step is too smallt.stop();if( is_out_ ){std::cout << "Iter: " << std::left <<std::setw(3) << k << " Result: "<< std::left <<std::setw(10) << *a_ << " " << std::left <<std::setw(10) << *b_ << " " << std::left <<std::setw(10) << *c_ << " step: " << std::left <<std::setw(14) << h.norm() << " cost: "<< std::left <<std::setw(14) << getCost() << " time: " << std::left <<std::setw(14) << t.duration() <<" total_time: "<< std::left <<std::setw(14) << (sumt += t.duration()) << std::endl;} } // whileif( found == true)std::cout << "\nConverged\n\n";elsestd::cout << "\nDiverged\n\n";}//function Eigen::MatrixXd fx_; Eigen::MatrixXd J_; // 雅克比矩阵Eigen::Matrix3d H_; // H矩阵Eigen::Vector3d g_;std::vector< Eigen::Vector2d> obs_; // 观测/* 要求的三个参数 */double* a_, *b_, *c_;/* parameters */double epsilon_1_, epsilon_2_;int max_iter_;bool is_out_;
};//class LevenbergMarquardtint main(int argc, char **argv) {const double aa = 0.1, bb = 0.5, cc = 2; // 实际方程的参数double a =0.0, b=0.0, c=0.0; // 初值/* 构造问题 */LevenbergMarquardt lm(&a, &b, &c);lm.setParameters(1e-10, 1e-10, 100, true);/* 制造数据 */const size_t N = 100; //数据个数cv::RNG rng(cv::getTickCount());for( size_t i = 0; i < N; i ++){/* 生产带有高斯噪声的数据 */double x = rng.uniform(0.0, 1.0) ;double y = exp(aa*x*x + bb*x + cc) + rng.gaussian(0.05);/* 添加到观测中 */lm.addObservation(x, y);}/* 用LM法求解 */lm.solve();return 0;
}