git.maemo.org Git - opencv/blob - cvaux/src/cvlevmar.cpp

   1 /*M///////////////////////////////////////////////////////////////////////////////////////
   2 //
   3 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
   4 //
   5 //  By downloading, copying, installing or using the software you agree to this license.
   6 //  If you do not agree to this license, do not download, install,
   7 //  copy or use the software.
   8 //
   9 //
  10 //                        Intel License Agreement
  11 //                For Open Source Computer Vision Library
  12 //
  13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
  14 // Third party copyrights are property of their respective owners.
  15 //
  16 // Redistribution and use in source and binary forms, with or without modification,
  17 // are permitted provided that the following conditions are met:
  18 //
  19 //   * Redistribution's of source code must retain the above copyright notice,
  20 //     this list of conditions and the following disclaimer.
  21 //
  22 //   * Redistribution's in binary form must reproduce the above copyright notice,
  23 //     this list of conditions and the following disclaimer in the documentation
  24 //     and/or other materials provided with the distribution.
  25 //
  26 //   * The name of Intel Corporation may not be used to endorse or promote products
  27 //     derived from this software without specific prior written permission.
  28 //
  29 // This software is provided by the copyright holders and contributors "as is" and
  30 // any express or implied warranties, including, but not limited to, the implied
  31 // warranties of merchantability and fitness for a particular purpose are disclaimed.
  32 // In no event shall the Intel Corporation or contributors be liable for any direct,
  33 // indirect, incidental, special, exemplary, or consequential damages
  34 // (including, but not limited to, procurement of substitute goods or services;
  35 // loss of use, data, or profits; or business interruption) however caused
  36 // and on any theory of liability, whether in contract, strict liability,
  37 // or tort (including negligence or otherwise) arising in any way out of
  38 // the use of this software, even if advised of the possibility of such damage.
  39 //
  40 //M*/
  41
  42 #include "_cvaux.h"
  43 #include "cvtypes.h"
  44 #include <float.h>
  45 #include <limits.h>
  46 #include "cv.h"
  47
  48 /* Valery Mosyagin */
  49
  50 //#define TRACKLEVMAR
  51
  52 typedef void (*pointer_LMJac)( const CvMat* src, CvMat* dst );
  53 typedef void (*pointer_LMFunc)( const CvMat* src, CvMat* dst );
  54
  55 /* Optimization using Levenberg-Marquardt */
  56 void cvLevenbergMarquardtOptimization(pointer_LMJac JacobianFunction,
  57                                     pointer_LMFunc function,
  58                                     /*pointer_Err error_function,*/
  59                                     CvMat *X0,CvMat *observRes,CvMat *resultX,
  60                                     int maxIter,double epsilon)
  61 {
  62     /* This is not sparce method */
  63     /* Make optimization using  */
  64     /* func - function to compute */
  65     /* uses function to compute jacobian */
  66
  67     /* Allocate memory */
  68     CvMat *vectX = 0;
  69     CvMat *vectNewX = 0;
  70     CvMat *resFunc = 0;
  71     CvMat *resNewFunc = 0;
  72     CvMat *error = 0;
  73     CvMat *errorNew = 0;
  74     CvMat *Jac = 0;
  75     CvMat *delta = 0;
  76     CvMat *matrJtJ = 0;
  77     CvMat *matrJtJN = 0;
  78     CvMat *matrJt = 0;
  79     CvMat *vectB = 0;
  80
  81     CV_FUNCNAME( "cvLevenbegrMarquardtOptimization" );
  82     __BEGIN__;
  83
  84
  85     if( JacobianFunction == 0 || function == 0 || X0 == 0 || observRes == 0 || resultX == 0 )
  86     {
  87         CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
  88     }
  89
  90     if( !CV_IS_MAT(X0) || !CV_IS_MAT(observRes) || !CV_IS_MAT(resultX) )
  91     {
  92         CV_ERROR( CV_StsUnsupportedFormat, "Some of input parameters must be a matrices" );
  93     }
  94
  95
  96     int numVal;
  97     int numFunc;
  98     double valError;
  99     double valNewError;
 100
 101     numVal = X0->rows;
 102     numFunc = observRes->rows;
 103
 104     /* test input data */
 105     if( X0->cols != 1 )
 106     {
 107         CV_ERROR( CV_StsUnmatchedSizes, "Number of colomn of vector X0 must be 1" );
 108     }
 109
 110     if( observRes->cols != 1 )
 111     {
 112         CV_ERROR( CV_StsUnmatchedSizes, "Number of colomn of vector observed rusult must be 1" );
 113     }
 114
 115     if( resultX->cols != 1 || resultX->rows != numVal )
 116     {
 117         CV_ERROR( CV_StsUnmatchedSizes, "Size of result vector X must be equals to X0" );
 118     }
 119
 120     if( maxIter <= 0  )
 121     {
 122         CV_ERROR( CV_StsUnmatchedSizes, "Number of maximum iteration must be > 0" );
 123     }
 124
 125     if( epsilon < 0 )
 126     {
 127         CV_ERROR( CV_StsUnmatchedSizes, "Epsilon must be >= 0" );
 128     }
 129
 130     /* copy x0 to current value of x */
 131     CV_CALL( vectX      = cvCreateMat(numVal, 1,      CV_64F) );
 132     CV_CALL( vectNewX   = cvCreateMat(numVal, 1,      CV_64F) );
 133     CV_CALL( resFunc    = cvCreateMat(numFunc,1,      CV_64F) );
 134     CV_CALL( resNewFunc = cvCreateMat(numFunc,1,      CV_64F) );
 135     CV_CALL( error      = cvCreateMat(numFunc,1,      CV_64F) );
 136     CV_CALL( errorNew   = cvCreateMat(numFunc,1,      CV_64F) );
 137     CV_CALL( Jac        = cvCreateMat(numFunc,numVal, CV_64F) );
 138     CV_CALL( delta      = cvCreateMat(numVal, 1,      CV_64F) );
 139     CV_CALL( matrJtJ    = cvCreateMat(numVal, numVal, CV_64F) );
 140     CV_CALL( matrJtJN   = cvCreateMat(numVal, numVal, CV_64F) );
 141     CV_CALL( matrJt     = cvCreateMat(numVal, numFunc,CV_64F) );
 142     CV_CALL( vectB      = cvCreateMat(numVal, 1,      CV_64F) );
 143
 144     cvCopy(X0,vectX);
 145
 146     /* ========== Main optimization loop ============ */
 147     double change;
 148     int currIter;
 149     double alpha;
 150
 151     change = 1;
 152     currIter = 0;
 153     alpha = 0.001;
 154
 155     do {
 156
 157         /* Compute value of function */
 158         function(vectX,resFunc);
 159         /* Print result of function to file */
 160
 161         /* Compute error */
 162         cvSub(observRes,resFunc,error);
 163
 164         //valError = error_function(observRes,resFunc);
 165         /* Need to use new version of computing error (norm) */
 166         valError = cvNorm(observRes,resFunc);
 167
 168         /* Compute Jacobian for given point vectX */
 169         JacobianFunction(vectX,Jac);
 170
 171         /* Define optimal delta for J'*J*delta=J'*error */
 172         /* compute J'J */
 173         cvMulTransposed(Jac,matrJtJ,1);
 174
 175         cvCopy(matrJtJ,matrJtJN);
 176
 177         /* compute J'*error */
 178         cvTranspose(Jac,matrJt);
 179         cvmMul(matrJt,error,vectB);
 180
 181
 182         /* Solve normal equation for given alpha and Jacobian */
 183         do
 184         {
 185             /* Increase diagonal elements by alpha */
 186             for( int i = 0; i < numVal; i++ )
 187             {
 188                 double val;
 189                 val = cvmGet(matrJtJ,i,i);
 190                 cvmSet(matrJtJN,i,i,(1+alpha)*val);
 191             }
 192
 193             /* Solve system to define delta */
 194             cvSolve(matrJtJN,vectB,delta,CV_SVD);
 195
 196             /* We know delta and we can define new value of vector X */
 197             cvAdd(vectX,delta,vectNewX);
 198
 199             /* Compute result of function for new vector X */
 200             function(vectNewX,resNewFunc);
 201             cvSub(observRes,resNewFunc,errorNew);
 202
 203             valNewError = cvNorm(observRes,resNewFunc);
 204
 205             currIter++;
 206
 207             if( valNewError < valError )
 208             {/* accept new value */
 209                 valError = valNewError;
 210
 211                 /* Compute relative change of required parameter vectorX. change = norm(curr-prev) / norm(curr) )  */
 212                 change = cvNorm(vectX, vectNewX, CV_RELATIVE_L2);
 213
 214                 alpha /= 10;
 215                 cvCopy(vectNewX,vectX);
 216                 break;
 217             }
 218             else
 219             {
 220                 alpha *= 10;
 221             }
 222
 223         } while ( currIter < maxIter  );
 224         /* new value of X and alpha were accepted */
 225
 226     } while ( change > epsilon && currIter < maxIter );
 227
 228
 229     /* result was computed */
 230     cvCopy(vectX,resultX);
 231
 232     __END__;
 233
 234     cvReleaseMat(&vectX);
 235     cvReleaseMat(&vectNewX);
 236     cvReleaseMat(&resFunc);
 237     cvReleaseMat(&resNewFunc);
 238     cvReleaseMat(&error);
 239     cvReleaseMat(&errorNew);
 240     cvReleaseMat(&Jac);
 241     cvReleaseMat(&delta);
 242     cvReleaseMat(&matrJtJ);
 243     cvReleaseMat(&matrJtJN);
 244     cvReleaseMat(&matrJt);
 245     cvReleaseMat(&vectB);
 246
 247     return;
 248 }
 249
 250 /*------------------------------------------------------------------------------*/
 251 #if 0
 252 //tests
 253 void Jac_Func2(CvMat *vectX,CvMat *Jac)
 254 {
 255     double x = cvmGet(vectX,0,0);
 256     double y = cvmGet(vectX,1,0);
 257     cvmSet(Jac,0,0,2*(x-2));
 258     cvmSet(Jac,0,1,2*(y+3));
 259
 260     cvmSet(Jac,1,0,1);
 261     cvmSet(Jac,1,1,1);
 262     return;
 263 }
 264
 265 void Res_Func2(CvMat *vectX,CvMat *res)
 266 {
 267     double x = cvmGet(vectX,0,0);
 268     double y = cvmGet(vectX,1,0);
 269     cvmSet(res,0,0,(x-2)*(x-2)+(y+3)*(y+3));
 270     cvmSet(res,1,0,x+y);
 271
 272     return;
 273 }
 274
 275
 276 double Err_Func2(CvMat *obs,CvMat *res)
 277 {
 278     CvMat *tmp;
 279     tmp = cvCreateMat(obs->rows,1,CV_64F);
 280     cvSub(obs,res,tmp);
 281
 282     double e;
 283     e = cvNorm(tmp);
 284
 285     return e;
 286 }
 287
 288
 289 void TestOptimX2Y2()
 290 {
 291     CvMat vectX0;
 292     double vectX0_dat[2];
 293     vectX0 = cvMat(2,1,CV_64F,vectX0_dat);
 294     vectX0_dat[0] = 5;
 295     vectX0_dat[1] = -7;
 296
 297     CvMat observRes;
 298     double observRes_dat[2];
 299     observRes = cvMat(2,1,CV_64F,observRes_dat);
 300     observRes_dat[0] = 0;
 301     observRes_dat[1] = -1;
 302     observRes_dat[0] = 0;
 303     observRes_dat[1] = -1.2;
 304
 305     CvMat optimX;
 306     double optimX_dat[2];
 307     optimX = cvMat(2,1,CV_64F,optimX_dat);
 308
 309
 310     LevenbegrMarquardtOptimization( Jac_Func2, Res_Func2, Err_Func2,
 311                                     &vectX0,&observRes,&optimX,100,0.000001);
 312
 313     return;
 314
 315 }
 316
 317 #endif
 318
 319
 320