LM(Levenberg-Marquard) c语言实现



LM在最优化中占据着极其重要的地位，将非线性问题转为为线性问题来求解，求出最优解。

 

最近在搞相机标定，而不得不面对LM，首先的想法百度，（个人不喜欢去看e文，总觉得效率不高，事实上错了）找到了一些相关的文章，好难啊，这是第一想法。不过还是找到了几篇大牛的文章，以及相关的MATLAB实现代码，本文的c实现便是基于MATLAB代码。感谢原作者

 

http://www.codelast.com/?p=29，这是大牛对之及其通俗的解释，相信看了能明白个大概。

http://www2.imm.dtu.dk/pubdb/views/edoc_download.php/3215/pdf/imm3215.pdf

讲解的非常透彻的一篇文档，以及实现。E文的，应该看懂都没问题。

% 计算函数f的雅克比矩阵，是解析式
syms a b y x real;
f=a*exp(-b*x);
Jsym=jacobian(f,[a b])
 
 
% 拟合用数据。参见《数学试验》，p190，例2
data_1=[0.25 0.5 1 1.5 2 3 4 6 8];
obs_1=[19.21 18.15 15.36 14.10 12.89 9.32 7.45 5.24 3.01];
 
% 2. LM算法
% 初始猜测s
a0=10; b0=0.5;
y_init = a0*exp(-b0*data_1);
% 数据个数
Ndata=length(obs_1);
% 参数维数
Nparams=2;
% 迭代最大次数
n_iters=50;
% LM算法的阻尼系数初值
lamda=0.01;
 
% step1: 变量赋值
updateJ=1;
a_est=a0;
b_est=b0;
 
% step2: 迭代
for it=1:n_iters
    if updateJ==1
        % 根据当前估计值，计算雅克比矩阵
        J=zeros(Ndata,Nparams);
        for i=1:length(data_1)
            J(i,:)=[exp(-b_est*data_1(i)) -a_est*data_1(i)*exp(-b_est*data_1(i))];
        end
        % 根据当前参数，得到函数值
        y_est = a_est*exp(-b_est*data_1);
        % 计算误差
        d=obs_1-y_est;
        % 计算（拟）海塞矩阵
        H=J'*J;
        % 若是第一次迭代，计算误差
        if it==1
            e=dot(d,d);
        end
    end
 
    % 根据阻尼系数lamda混合得到H矩阵
    H_lm=H+(lamda*eye(Nparams,Nparams));
    % 计算步长dp，并根据步长计算新的可能的\参数估计值
    dp=inv(H_lm)*(J'*d(:));
    g = J'*d(:);
    a_lm=a_est+dp(1);
    b_lm=b_est+dp(2);
    % 计算新的可能估计值对应的y和计算残差e
    y_est_lm = a_lm*exp(-b_lm*data_1);
    d_lm=obs_1-y_est_lm;
    e_lm=dot(d_lm,d_lm);
    % 根据误差，决定如何更新参数和阻尼系数
    if e_lm<e
        lamda=lamda/10;
        a_est=a_lm;
        b_est=b_lm;
        e=e_lm;
        disp(e);
        updateJ=1;
    else
        updateJ=0;
        lamda=lamda*10;
    end
end
%显示优化的结果
a_est
b_est

以上是MATLAB实现，据说原作者是http://www.shenlejun.cn/my/article/show.asp?id=17&page=1但是好像打不开。

于是我自己想用c语言来实现，写了很久发现还是有点写不出来啊，看来代码实现能力还是不行。最后不得已写了一个 以上MATLAB的c语言版本。

非常感谢那些把经验和知识分享出来的牛人。

#include <stdio.h>
#include <math.h>
#include <string.h>

/*
函数表达式为 f(x)=a*exp(-b*x),a,b为待估参数
*/
double data_1[9]={0.25 ,0.5, 1 ,1.5 ,2, 3, 4, 6 ,8};
double obs_1[9]={19.21 ,18.15 ,15.36 ,14.10 ,12.89, 9.32 ,7.45 ,5.24 ,3.01};
/*
void cal_jaco(double **J, double a_bst, double b_bst)
{
	int i,j;
	j=0;
	for(i=0;i<9;i++)
	{
			J[i][j] = exp(-b_bst*data_1[i]);
			J[i][j+1] = -a_bst*data_1[i]*exp(-b_bst*data_1[i]);
	}
}
*/
//计算函数的值，目前只计算已知函数，即无法对输入的函数求解
void cal_fun_val(double *val,double a_bst, double b_bst)
{
	int i;
	for(i=0;i<9;i++)
	{
		val[i]=a_bst*exp(-b_bst*data_1[i]);
	}
}
//一位向量的减法
void matrix_sub(double *result, double *first_m, double *second_m)
{
	int i;
	for(i=8;i>=0;i--)
	{
		result[i]=first_m[i]-second_m[i];
	}
}
//一维向量的加法
void matrix_add(double *result, const double *first_m, const double *second_m)
{
	int i;
	for (i = 8; i >= 0; i--)
	{
		result[i] = first_m[i] + second_m[i];
	}
}
//二维向量的转置
void tran_matrix(double **tran_mat, double **matrix, int row,int col)
{
	int i,j;
	for (i = 0; i < row; i++)
	{
		for (j = 0; j < col; j++)
		{
			*((double *)tran_mat + row*j +i) = *((double *)matrix + col*i + j);
		}
	}
}
//打印出n*m的矩阵
void print_matrix(double **matrix, int row, int col)
{
	int i,j;
	for (i = 0; i < row; i++)
	{
		for (j = 0; j < col; j++)
		{
			printf("%f ", *((double *)matrix + col*i + j));
		}
			printf("\n");
	}
}
//二维矩阵的乘法
/*
outa=inb*inc
*/
void matrix_mut(double ** outa, double** inb, int b_row, int b_col, double** inc, int c_col)
{
	int i, j, k,num;
	double tempdata=0;
	num = 0;
	double *tempa = (double *)outa;
	double t1, t2;
	for (i = 0; i < b_row; i++)
	{
		for (k = 0; k < c_col; k++)
		{
			for (j = 0; j < b_col; j++)
			{	
				t1 = (*((double *)inb + b_col*i + j));
				t2 = (*((double *)inc + c_col*j + k));
				tempdata += t1*t2;
			}
			tempa[num]=tempdata;
			num++;
			tempdata = 0;
		}
	}
}
//矩阵与常数相乘
void cost_mat_mul(double **res_mat, int ratio, double **mat, int row, int col)
{
	int i, j;
	for (i = 0; i < row; i++)
	{
		for (j = 0; j < col; j++)
		{
			(*((double *)res_mat + col*i + j)) = ratio * (*((double *)mat + col*i + j));
		}
	}
}
//向量点积
double dot(double * mat1, double * mat2,int nofelem)
{
	double *temp_mat1 = mat1;
	double *temp_mat2 = mat2;
	double result = 0;
	int i;
	for (i = 0; i<nofelem;i++)
		result += (*temp_mat1++)*(*temp_mat2++);
	return result;
}
#define MAX 2
#define E 0.000000001

/**
* 计算矩阵行列式src的模值
*/
double calculate_A(double src[][MAX], int n)
{
	int i, j, k, x, y;
	double tmp[MAX][MAX], t;
	double result = 0.0;
	if (n == 1)
	{
		return src[0][0];
	}

	for (i = 0; i < n; ++i)
	{
		for (j = 0; j < n - 1; ++j)
		{
			for (k = 0; k < n - 1; ++k)
			{
				x = j + 1;
				y = k >= i ? k + 1 : k;

				tmp[j][k] = src[x][y];
			}
		}
		t = calculate_A(tmp, n - 1);

		if (i % 2 == 0)
		{
			result += src[0][i] * t;
		}
		else
		{
			result -= src[0][i] * t;
		}
	}

	return result;
}

/**
* 计算伴随矩阵
*/
void calculate_A_adjoint(double src[MAX][MAX], double dst[MAX][MAX], int n)
{
	int i, j, k, t, x, y;
	double tmp[MAX][MAX];

	if (n == 1)
	{
		dst[0][0] = 1;
		return;
	}

	for (i = 0; i < n; ++i)
	{
		for (j = 0; j < n; ++j)
		{
			for (k = 0; k < n - 1; ++k)
			{
				for (t = 0; t < n - 1; ++t)
				{
					x = k >= i ? k + 1 : k;
					y = t >= j ? t + 1 : t;

					tmp[k][t] = src[x][y];
				}
			}

			dst[j][i] = calculate_A(tmp, n - 1);

			if ((i + j) % 2 == 1)
			{
				dst[j][i] = -1 * dst[j][i];
			}
		}
	}
}

/**
* 得到逆矩阵
*/
int inv(const double src[MAX][MAX], double dst[MAX][MAX], int n)
{
	double A = calculate_A(src, n);
	double tmp[MAX][MAX];
	int i, j;
	if (fabs(A - 0) <= E)
	{
		//       printf("不可能有逆矩阵！\n");
		return 0;
	}

	calculate_A_adjoint(src, tmp, n);

	for (i = 0; i < n; ++i)
	{
		for (j = 0; j < n; ++j)
		{
			dst[i][j] = (double)(tmp[i][j] / A);
		}
	}

	return 1;
}
//LM算法 核心代码
int LM()
{
		// 迭代最大次数
	int n_iters=400;
	// LM算法的阻尼系数初值
	double lamda=0.01;
	//初始值
	double a0=20;
	double b0=0.5;
	// step1: 变量赋值
	int updateJ=1;
	double a_est=a0;
	double b_est=b0;

	int it=0;
	double J[9][2];
	double Jt[2][9];
	double y_est[9];
	double d[9];
	double H[2][2] = {0};
	double H_1m[2][2];
	double e;
	double tempeye[2][2] = { { 1, 0 }, { 0, 1 } };
	double eye[2][2] = { { 1, 0 }, { 0, 1 } };
	double g[2] = {0};
	int i, j;
	double invH_1m[2][2] = {0};
	double dp[2] = {0};
	double a_1m, b_1m;
	double y_est_lm[9];
	double d_1m[9], e_1m;
	double temp = 0;
	for ( it =0;it<n_iters; it++)
	{
		if(updateJ == 1)
		{
//			cal_jaco(J,a_est,b_est);
			for (i = 0; i<9; i++)
			{//计算雅克比矩阵
				J[i][0] = exp(-b_est*data_1[i]);
				J[i][1] = -a_est*data_1[i] * exp(-b_est*data_1[i]);
			}
			cal_fun_val(y_est, a_est, b_est);
			matrix_sub(d, obs_1, y_est);//计算当前的误差
			tran_matrix(Jt, J, 9, 2);
			matrix_mut(H, Jt, 2, 9, J, 2);//得到Hessian 矩阵
			if (it == 0)
			{
				e = dot(d, d, 9);//第一次时计算误差作为误差初值
			}
		}
		cost_mat_mul(tempeye, lamda, eye, 2, 2);
/*		for (i = 0; i < 2; i++)
		{
			for (j = 0; j < 2; j++)
			{
				tempeye[i][j] = eye[i][j] * lamda;
			}
		}*/
//		matrix_add(H_1m, H, tempeye);
		for (i = 0; i < 2; i++)
		{
			for (j = 0; j < 2; j++)
			{
				H_1m[i][j] = H[i][j] + tempeye[i][j];
			}
		}
		for (i = 0; i < 2; i++)
		{
			for (j = 0; j < 9; j++)
			{
				temp += Jt[i][j] * d[j];
			}
			g[i] = temp;
		}
		inv(H_1m, invH_1m, 2);
		temp = 0;
		for (i = 0; i < 2; i++)
		{
			for (j = 0; j < 2; j++)
			{
				temp += invH_1m[i][j] * g[j];
			}
			dp[i] = temp;//求解步长
		}
		a_1m = a_est + dp[0];
		b_1m = b_est + dp[1];
		for (i = 0; i < 9; i++)
		{
			y_est_lm[i] = a_1m*exp(-b_1m*data_1[i]);
			d_1m[i] = obs_1[i] - y_est_lm[i];
		}
		e_1m = dot(d_1m, d_1m, 9);//计算新的 a,b时的误差
	
		if (e_1m < e)//如果误差下降了，这是我们想要的
		{
			lamda = lamda / 10;
			a_est = a_1m;
			b_est = b_1m;
			e = e_1m;
			printf("%f  \n", e);
			updateJ = 1;
		}
		else
		{
			updateJ = 0;
			lamda = lamda * 10;
		}
	}
	printf("%f,%f  ", a_est, b_est);
	return 1;
}

int main()
{
	LM();
	return 0;
}