帖子原地址：http://blog.csdn.net/wejoncy/article/details/44095527
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;
- }
982
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
