modules/legacy/src/levmar.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 "precomp.hpp"
  43 #include <float.h>
  44 #include <limits.h>
  45
  46 /* Valery Mosyagin */
  47
  48 //#define TRACKLEVMAR
  49
  50 typedef void (*pointer_LMJac)( const CvMat* src, CvMat* dst );
  51 typedef void (*pointer_LMFunc)( const CvMat* src, CvMat* dst );
  52
  53 #if 0
  54 /* Optimization using Levenberg-Marquardt */
  55 void cvLevenbergMarquardtOptimization(pointer_LMJac JacobianFunction,
  56                                     pointer_LMFunc function,
  57                                     /*pointer_Err error_function,*/
  58                                     CvMat *X0,CvMat *observRes,CvMat *resultX,
  59                                     int maxIter,double epsilon)
  60 {
  61     /* This is not sparse method */
  62     /* Make optimization using  */
  63     /* func - function to compute */
  64     /* uses function to compute jacobian */
  65
  66     /* Allocate memory */
  67     CvMat *vectX = 0;
  68     CvMat *vectNewX = 0;
  69     CvMat *resFunc = 0;
  70     CvMat *resNewFunc = 0;
  71     CvMat *error = 0;
  72     CvMat *errorNew = 0;
  73     CvMat *Jac = 0;
  74     CvMat *delta = 0;
  75     CvMat *matrJtJ = 0;
  76     CvMat *matrJtJN = 0;
  77     CvMat *matrJt = 0;
  78     CvMat *vectB = 0;
  79
  80     CV_FUNCNAME( "cvLevenbegrMarquardtOptimization" );
  81     __BEGIN__;
  82
  83
  84     if( JacobianFunction == 0 || function == 0 || X0 == 0 || observRes == 0 || resultX == 0 )
  85     {
  86         CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
  87     }
  88
  89     if( !CV_IS_MAT(X0) || !CV_IS_MAT(observRes) || !CV_IS_MAT(resultX) )
  90     {
  91         CV_ERROR( CV_StsUnsupportedFormat, "Some of input parameters must be a matrices" );
  92     }
  93
  94
  95     int numVal;
  96     int numFunc;
  97     double valError;
  98     double valNewError;
  99
 100     numVal = X0->rows;
 101     numFunc = observRes->rows;
 102
 103     /* test input data */
 104     if( X0->cols != 1 )
 105     {
 106         CV_ERROR( CV_StsUnmatchedSizes, "Number of colomn of vector X0 must be 1" );
 107     }
 108
 109     if( observRes->cols != 1 )
 110     {
 111         CV_ERROR( CV_StsUnmatchedSizes, "Number of colomn of vector observed rusult must be 1" );
 112     }
 113
 114     if( resultX->cols != 1 || resultX->rows != numVal )
 115     {
 116         CV_ERROR( CV_StsUnmatchedSizes, "Size of result vector X must be equals to X0" );
 117     }
 118
 119     if( maxIter <= 0  )
 120     {
 121         CV_ERROR( CV_StsUnmatchedSizes, "Number of maximum iteration must be > 0" );
 122     }
 123
 124     if( epsilon < 0 )
 125     {
 126         CV_ERROR( CV_StsUnmatchedSizes, "Epsilon must be >= 0" );
 127     }
 128
 129     /* copy x0 to current value of x */
 130     CV_CALL( vectX      = cvCreateMat(numVal, 1,      CV_64F) );
 131     CV_CALL( vectNewX   = cvCreateMat(numVal, 1,      CV_64F) );
 132     CV_CALL( resFunc    = cvCreateMat(numFunc,1,      CV_64F) );
 133     CV_CALL( resNewFunc = cvCreateMat(numFunc,1,      CV_64F) );
 134     CV_CALL( error      = cvCreateMat(numFunc,1,      CV_64F) );
 135     CV_CALL( errorNew   = cvCreateMat(numFunc,1,      CV_64F) );
 136     CV_CALL( Jac        = cvCreateMat(numFunc,numVal, CV_64F) );
 137     CV_CALL( delta      = cvCreateMat(numVal, 1,      CV_64F) );
 138     CV_CALL( matrJtJ    = cvCreateMat(numVal, numVal, CV_64F) );
 139     CV_CALL( matrJtJN   = cvCreateMat(numVal, numVal, CV_64F) );
 140     CV_CALL( matrJt     = cvCreateMat(numVal, numFunc,CV_64F) );
 141     CV_CALL( vectB      = cvCreateMat(numVal, 1,      CV_64F) );
 142
 143     cvCopy(X0,vectX);
 144
 145     /* ========== Main optimization loop ============ */
 146     double change;
 147     int currIter;
 148     double alpha;
 149
 150     change = 1;
 151     currIter = 0;
 152     alpha = 0.001;
 153
 154     do {
 155
 156         /* Compute value of function */
 157         function(vectX,resFunc);
 158         /* Print result of function to file */
 159
 160         /* Compute error */
 161         cvSub(observRes,resFunc,error);
 162
 163         //valError = error_function(observRes,resFunc);
 164         /* Need to use new version of computing error (norm) */
 165         valError = cvNorm(observRes,resFunc);
 166
 167         /* Compute Jacobian for given point vectX */
 168         JacobianFunction(vectX,Jac);
 169
 170         /* Define optimal delta for J'*J*delta=J'*error */
 171         /* compute J'J */
 172         cvMulTransposed(Jac,matrJtJ,1);
 173
 174         cvCopy(matrJtJ,matrJtJN);
 175
 176         /* compute J'*error */
 177         cvTranspose(Jac,matrJt);
 178         cvmMul(matrJt,error,vectB);
 179
 180
 181         /* Solve normal equation for given alpha and Jacobian */
 182         do
 183         {
 184             /* Increase diagonal elements by alpha */
 185             for( int i = 0; i < numVal; i++ )
 186             {
 187                 double val;
 188                 val = cvmGet(matrJtJ,i,i);
 189                 cvmSet(matrJtJN,i,i,(1+alpha)*val);
 190             }
 191
 192             /* Solve system to define delta */
 193             cvSolve(matrJtJN,vectB,delta,CV_SVD);
 194
 195             /* We know delta and we can define new value of vector X */
 196             cvAdd(vectX,delta,vectNewX);
 197
 198             /* Compute result of function for new vector X */
 199             function(vectNewX,resNewFunc);
 200             cvSub(observRes,resNewFunc,errorNew);
 201
 202             valNewError = cvNorm(observRes,resNewFunc);
 203
 204             currIter++;
 205
 206             if( valNewError < valError )
 207             {/* accept new value */
 208                 valError = valNewError;
 209
 210                 /* Compute relative change of required parameter vectorX. change = norm(curr-prev) / norm(curr) )  */
 211                 change = cvNorm(vectX, vectNewX, CV_RELATIVE_L2);
 212
 213                 alpha /= 10;
 214                 cvCopy(vectNewX,vectX);
 215                 break;
 216             }
 217             else
 218             {
 219                 alpha *= 10;
 220             }
 221
 222         } while ( currIter < maxIter  );
 223         /* new value of X and alpha were accepted */
 224
 225     } while ( change > epsilon && currIter < maxIter );
 226
 227
 228     /* result was computed */
 229     cvCopy(vectX,resultX);
 230
 231     __END__;
 232
 233     cvReleaseMat(&vectX);
 234     cvReleaseMat(&vectNewX);
 235     cvReleaseMat(&resFunc);
 236     cvReleaseMat(&resNewFunc);
 237     cvReleaseMat(&error);
 238     cvReleaseMat(&errorNew);
 239     cvReleaseMat(&Jac);
 240     cvReleaseMat(&delta);
 241     cvReleaseMat(&matrJtJ);
 242     cvReleaseMat(&matrJtJN);
 243     cvReleaseMat(&matrJt);
 244     cvReleaseMat(&vectB);
 245
 246     return;
 247 }
 248 #endif
 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