00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024 #include "Alvar.h"
00025 #include "Optimization.h"
00026 #include "time.h"
00027 #include "highgui.h"
00028
00029 #include <iostream>
00030 using namespace std;
00031
00032 namespace alvar {
00033
00034 Optimization::Optimization(int n_params, int n_meas)
00035 {
00036 estimate_param = 0;
00037 J = cvCreateMat(n_meas, n_params, CV_64F); cvZero(J);
00038 JtJ = cvCreateMat(n_params, n_params, CV_64F); cvZero(JtJ);
00039 tmp = cvCreateMat(n_params, n_meas, CV_64F); cvZero(tmp);
00040 W = cvCreateMat(n_meas, n_meas, CV_64F); cvZero(W);
00041 diag = cvCreateMat(n_params, n_params, CV_64F); cvZero(diag);
00042 err = cvCreateMat(n_meas, 1, CV_64F); cvZero(err);
00043 delta = cvCreateMat(n_params, 1, CV_64F); cvZero(delta);
00044 x_minus = cvCreateMat(n_params, 1, CV_64F); cvZero(x_minus);
00045 x_plus = cvCreateMat(n_params, 1, CV_64F); cvZero(x_plus);
00046 x_tmp1 = cvCreateMat(n_meas, 1, CV_64F); cvZero(x_tmp1);
00047 x_tmp2 = cvCreateMat(n_meas, 1, CV_64F); cvZero(x_tmp2);
00048 tmp_par = cvCreateMat(n_params, 1, CV_64F); cvZero(tmp_par);
00049 }
00050
00051 Optimization::~Optimization()
00052 {
00053 cvReleaseMat(&J);
00054 cvReleaseMat(&JtJ);
00055 cvReleaseMat(&diag);
00056 cvReleaseMat(&tmp);
00057 cvReleaseMat(&W);
00058 cvReleaseMat(&err);
00059 cvReleaseMat(&delta);
00060 cvReleaseMat(&x_plus);
00061 cvReleaseMat(&x_minus);
00062 cvReleaseMat(&x_tmp1);
00063 cvReleaseMat(&x_tmp2);
00064 cvReleaseMat(&tmp_par);
00065 estimate_param = 0;
00066 }
00067
00068 double Optimization::CalcTukeyWeight(double residual, double c)
00069 {
00070
00071 double ret=0;
00072
00073 if(fabs(residual) <= c)
00074 {
00075 double tmp = 1.0-((residual/c)*(residual/c));
00076 ret = ((c*c)/6.0)*(1.0-tmp*tmp*tmp);
00077 }
00078 else
00079 ret = (c*c)/6.0;
00080
00081 if(residual)
00082 ret = fabs(sqrt(ret)/residual);
00083 else
00084 ret = 1.0;
00085
00086 return ret;
00087 }
00088
00089 double Optimization::CalcTukeyWeightSimple(double residual, double c)
00090 {
00091
00092 double ret=0;
00093
00094 double x2 = residual*residual;
00095 if(x2<c*c) return residual;
00096 else return c;
00097 }
00098
00099 void Optimization::CalcJacobian(CvMat* x, CvMat* J, EstimateCallback Estimate)
00100 {
00101 const double step = 0.001;
00102
00103 cvZero(J);
00104 for (int i=0; i<J->cols; i++)
00105 {
00106 CvMat J_column;
00107 cvGetCol(J, &J_column, i);
00108
00109 cvZero(delta);
00110 cvmSet(delta, i, 0, step);
00111 cvAdd(x, delta, x_plus);
00112 cvmSet(delta, i, 0, -step);
00113 cvAdd(x, delta, x_minus);
00114
00115 Estimate(x_plus, x_tmp1, estimate_param);
00116 Estimate(x_minus, x_tmp2, estimate_param);
00117 cvSub(x_tmp1, x_tmp2, &J_column);
00118 cvScale(&J_column, &J_column, 1.0/(2*step));
00119 }
00120 }
00121
00122
00123 double Optimization::Optimize(CvMat* parameters,
00124 CvMat* measurements,
00125 double stop,
00126 int max_iter,
00127 EstimateCallback Estimate,
00128 void *param,
00129 OptimizeMethod method,
00130 CvMat* parameters_mask,
00131 CvMat* J_mat,
00132 CvMat* weights)
00133 {
00134
00135 int n_params = parameters->rows;
00136 int n_meas = measurements->rows;
00137 double error_new = 0;
00138 double error_old = 0;
00139 double n1, n2;
00140 int cntr = 0;
00141 estimate_param = param;
00142 lambda = 0.001;
00143
00144 while(true)
00145 {
00146 if(!J_mat)
00147 CalcJacobian(parameters, J, Estimate);
00148 else
00149 J = J_mat;
00150
00151
00152
00153 if(parameters_mask)
00154 for (int i=0; i<parameters_mask->rows; i++) {
00155 if (cvGet2D(parameters_mask, i, 0).val[0] == 0) {
00156 CvRect rect;
00157 rect.height = J->rows; rect.width = 1;
00158 rect.y = 0; rect.x = i;
00159 CvMat foo;
00160 cvGetSubRect(J, &foo, rect);
00161 cvZero(&foo);
00162 }
00163 }
00164
00165 Estimate(parameters, x_tmp1, estimate_param);
00166 cvSub(measurements, x_tmp1, err);
00167 error_old = cvNorm(err, 0, CV_L2);
00168
00169 switch(method)
00170 {
00171 case (GAUSSNEWTON) :
00172
00173 cvMulTransposed(J, JtJ, 1);
00174 cvInv(JtJ, JtJ, CV_SVD);
00175 cvGEMM(JtJ, J, 1.0, 0, 0, tmp, CV_GEMM_B_T);
00176 cvMatMul(tmp, err, delta);
00177 cvAdd(delta, parameters, parameters);
00178
00179
00180 n1 = cvNorm(delta);
00181 n2 = cvNorm(parameters);
00182
00183 if( ((n1/n2) < stop) ||
00184 (cntr >= max_iter) )
00185 goto end;
00186
00187 break;
00188
00189 case (LEVENBERGMARQUARDT) :
00190
00191 cvSetIdentity(diag, cvRealScalar(lambda));
00192
00193 if(weights)
00194 for(int k = 0; k < W->rows; ++k)
00195 cvmSet(W, k, k, weights->data.db[k]);
00196
00197
00198 if(weights)
00199 {
00200 cvGEMM(J, W, 1, 0, 0, tmp, CV_GEMM_A_T);
00201 cvGEMM(tmp, J, 1, 0, 0, JtJ, 0);
00202 }
00203 else
00204 cvMulTransposed(J, JtJ, 1);
00205
00206
00207
00208 cvAdd(JtJ, diag, JtJ);
00209 cvInv(JtJ, JtJ, CV_SVD);
00210 cvGEMM(JtJ, J, 1.0, 0, 0, tmp, CV_GEMM_B_T);
00211
00212 if(weights)
00213 cvGEMM(tmp, W, 1, 0, 0, tmp, 0);
00214
00215 cvMatMul(tmp, err, delta);
00216 cvAdd(delta, parameters, tmp_par);
00217
00218 Estimate(tmp_par, x_tmp1, estimate_param);
00219 cvSub(measurements, x_tmp1, err);
00220
00221 error_new = cvNorm(err, 0, CV_L2);
00222
00223 if(error_new < error_old)
00224 {
00225 cvCopy(tmp_par, parameters);
00226 lambda = lambda/10.0;
00227 }
00228 else
00229 {
00230 lambda = lambda*10.0;
00231 }
00232 if(lambda>10) lambda = 10;
00233 if(lambda<0.00001) lambda = 0.00001;
00234
00235 n1 = cvNorm(delta);
00236 n2 = cvNorm(parameters);
00237
00238 if( (n1/n2) < stop ||
00239 (cntr >= max_iter) )
00240 {
00241 goto end;
00242 }
00243
00244 break;
00245
00246 case (TUKEY_LM) :
00247
00248 cvSetIdentity(diag, cvRealScalar(lambda));
00249
00250
00251 for(int k = 0; k < W->rows; ++k)
00252 {
00253 if(weights)
00254 if(weights->data.db[k] != -1.0)
00255 cvmSet(W, k, k, weights->data.db[k]);
00256 else
00257 cvmSet(W, k, k, CalcTukeyWeight(err->data.db[k], 3));
00258 else
00259 cvmSet(W, k, k, CalcTukeyWeight(err->data.db[k], 3));
00260 }
00261
00262 cvGEMM(J, W, 1, 0, 0, tmp, CV_GEMM_A_T);
00263 cvGEMM(tmp, J, 1, 0, 0, JtJ, 0);
00264 cvAdd(JtJ, diag, JtJ);
00265 cvInv(JtJ, JtJ, CV_SVD);
00266 cvGEMM(JtJ, J, 1.0, 0, 0, tmp, CV_GEMM_B_T);
00267 cvGEMM(tmp, W, 1, 0, 0, tmp, 0);
00268 cvMatMul(tmp, err, delta);
00269 cvAdd(delta, parameters, tmp_par);
00270
00271 Estimate(tmp_par, x_tmp1, estimate_param);
00272 cvSub(measurements, x_tmp1, err);
00273
00274 error_new = cvNorm(err, 0, CV_L2);
00275
00276 if(error_new < error_old)
00277 {
00278 cvCopy(tmp_par, parameters);
00279 lambda = lambda/10.0;
00280 }
00281 else
00282 {
00283 lambda = lambda*10.0;
00284 }
00285 if(lambda>10) lambda = 10;
00286 if(lambda<0.00001) lambda = 0.00001;
00287
00288 n1 = cvNorm(delta);
00289 n2 = cvNorm(parameters);
00290
00291 if( ((n1/n2) < stop) ||
00292 (cntr >= max_iter) )
00293 {
00294 goto end;
00295 }
00296
00297 break;
00298 }
00299 ++cntr;
00300 }
00301
00302 end :
00303
00304 return error_old;
00305 }
00306
00307 }