帖子原地址: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;
- }