Home | History | Annotate | Download | only in src 
      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 
    321