ar_track_alvar: Optimization.cpp Source File

Go to the documentation of this file.
00001 /*
00002  * This file is part of ALVAR, A Library for Virtual and Augmented Reality.
00003  *
00004  * Copyright 2007-2012 VTT Technical Research Centre of Finland
00005  *
00006  * Contact: VTT Augmented Reality Team <alvar.info@vtt.fi>
00007  *          <http://www.vtt.fi/multimedia/alvar.html>
00008  *
00009  * ALVAR is free software; you can redistribute it and/or modify it under the
00010  * terms of the GNU Lesser General Public License as published by the Free
00011  * Software Foundation; either version 2.1 of the License, or (at your option)
00012  * any later version.
00013  *
00014  * This library is distributed in the hope that it will be useful, but WITHOUT
00015  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
00016  * FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License
00017  * for more details.
00018  *
00019  * You should have received a copy of the GNU Lesser General Public License
00020  * along with ALVAR; if not, see
00021  * <http://www.gnu.org/licenses/old-licenses/lgpl-2.1.html>.
00022  */
00023 
00024 #include "ar_track_alvar/Alvar.h"
00025 #include "ar_track_alvar/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         //const double c = 3; // squared distance in the model tracker
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         //const double c = 3;
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,      // Initial values are set
00124                                                           CvMat* measurements,    // Some observations
00125                                                           double stop,
00126                                                           int   max_iter,
00127                                                           EstimateCallback Estimate,
00128                                                           void *param,
00129                                                           OptimizeMethod method,
00130                                                           CvMat* parameters_mask, // Mask indicating non-constant parameters)
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                 // Zero the columns for constant parameters
00152                 // TODO: Make this into a J-sized mask matrix before the iteration loop
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); // err = residual
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); // inv(JtJ)Jt
00176                                 cvMatMul(tmp, err, delta);
00177                                 cvAdd(delta, parameters, parameters);
00178 
00179                                 // Lopetusehto
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                                 // JtWJ
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                                 // JtJ + lambda*I
00207                                 // or JtWJ + lambda*I if weights are used...
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                                 // Tukey weights
00251                                 for(int k = 0; k < W->rows; ++k)
00252                                 {
00253                                         if(weights)                                                                                                       // If using weight vector
00254                                                 if(weights->data.db[k] != -1.0)                                                      // If true weight given
00255                                                         cvmSet(W, k, k, weights->data.db[k]);                                     // Use given weight
00256                                                 else
00257                                                         cvmSet(W, k, k, CalcTukeyWeight(err->data.db[k], 3));     // otherwise use Tukey weight
00258                                         else
00259                                                 cvmSet(W, k, k, CalcTukeyWeight(err->data.db[k], 3));     // Otherwise use Tukey weight
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 } // namespace alvar