| /*M/////////////////////////////////////////////////////////////////////////////////////// |
| // |
| // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. |
| // |
| // By downloading, copying, installing or using the software you agree to this license. |
| // If you do not agree to this license, do not download, install, |
| // copy or use the software. |
| // |
| // |
| // Intel License Agreement |
| // For Open Source Computer Vision Library |
| // |
| // Copyright (C) 2000, Intel Corporation, all rights reserved. |
| // Third party copyrights are property of their respective owners. |
| // |
| // Redistribution and use in source and binary forms, with or without modification, |
| // are permitted provided that the following conditions are met: |
| // |
| // * Redistribution's of source code must retain the above copyright notice, |
| // this list of conditions and the following disclaimer. |
| // |
| // * Redistribution's in binary form must reproduce the above copyright notice, |
| // this list of conditions and the following disclaimer in the documentation |
| // and/or other materials provided with the distribution. |
| // |
| // * The name of Intel Corporation may not be used to endorse or promote products |
| // derived from this software without specific prior written permission. |
| // |
| // This software is provided by the copyright holders and contributors "as is" and |
| // any express or implied warranties, including, but not limited to, the implied |
| // warranties of merchantability and fitness for a particular purpose are disclaimed. |
| // In no event shall the Intel Corporation or contributors be liable for any direct, |
| // indirect, incidental, special, exemplary, or consequential damages |
| // (including, but not limited to, procurement of substitute goods or services; |
| // loss of use, data, or profits; or business interruption) however caused |
| // and on any theory of liability, whether in contract, strict liability, |
| // or tort (including negligence or otherwise) arising in any way out of |
| // the use of this software, even if advised of the possibility of such damage. |
| // |
| //M*/ |
| |
| #include "_cvaux.h" |
| #include "cvtypes.h" |
| #include <float.h> |
| #include <limits.h> |
| #include "cv.h" |
| |
| /* Valery Mosyagin */ |
| |
| //#define TRACKLEVMAR |
| |
| typedef void (*pointer_LMJac)( const CvMat* src, CvMat* dst ); |
| typedef void (*pointer_LMFunc)( const CvMat* src, CvMat* dst ); |
| |
| /* Optimization using Levenberg-Marquardt */ |
| void cvLevenbergMarquardtOptimization(pointer_LMJac JacobianFunction, |
| pointer_LMFunc function, |
| /*pointer_Err error_function,*/ |
| CvMat *X0,CvMat *observRes,CvMat *resultX, |
| int maxIter,double epsilon) |
| { |
| /* This is not sparce method */ |
| /* Make optimization using */ |
| /* func - function to compute */ |
| /* uses function to compute jacobian */ |
| |
| /* Allocate memory */ |
| CvMat *vectX = 0; |
| CvMat *vectNewX = 0; |
| CvMat *resFunc = 0; |
| CvMat *resNewFunc = 0; |
| CvMat *error = 0; |
| CvMat *errorNew = 0; |
| CvMat *Jac = 0; |
| CvMat *delta = 0; |
| CvMat *matrJtJ = 0; |
| CvMat *matrJtJN = 0; |
| CvMat *matrJt = 0; |
| CvMat *vectB = 0; |
| |
| CV_FUNCNAME( "cvLevenbegrMarquardtOptimization" ); |
| __BEGIN__; |
| |
| |
| if( JacobianFunction == 0 || function == 0 || X0 == 0 || observRes == 0 || resultX == 0 ) |
| { |
| CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); |
| } |
| |
| if( !CV_IS_MAT(X0) || !CV_IS_MAT(observRes) || !CV_IS_MAT(resultX) ) |
| { |
| CV_ERROR( CV_StsUnsupportedFormat, "Some of input parameters must be a matrices" ); |
| } |
| |
| |
| int numVal; |
| int numFunc; |
| double valError; |
| double valNewError; |
| |
| numVal = X0->rows; |
| numFunc = observRes->rows; |
| |
| /* test input data */ |
| if( X0->cols != 1 ) |
| { |
| CV_ERROR( CV_StsUnmatchedSizes, "Number of colomn of vector X0 must be 1" ); |
| } |
| |
| if( observRes->cols != 1 ) |
| { |
| CV_ERROR( CV_StsUnmatchedSizes, "Number of colomn of vector observed rusult must be 1" ); |
| } |
| |
| if( resultX->cols != 1 || resultX->rows != numVal ) |
| { |
| CV_ERROR( CV_StsUnmatchedSizes, "Size of result vector X must be equals to X0" ); |
| } |
| |
| if( maxIter <= 0 ) |
| { |
| CV_ERROR( CV_StsUnmatchedSizes, "Number of maximum iteration must be > 0" ); |
| } |
| |
| if( epsilon < 0 ) |
| { |
| CV_ERROR( CV_StsUnmatchedSizes, "Epsilon must be >= 0" ); |
| } |
| |
| /* copy x0 to current value of x */ |
| CV_CALL( vectX = cvCreateMat(numVal, 1, CV_64F) ); |
| CV_CALL( vectNewX = cvCreateMat(numVal, 1, CV_64F) ); |
| CV_CALL( resFunc = cvCreateMat(numFunc,1, CV_64F) ); |
| CV_CALL( resNewFunc = cvCreateMat(numFunc,1, CV_64F) ); |
| CV_CALL( error = cvCreateMat(numFunc,1, CV_64F) ); |
| CV_CALL( errorNew = cvCreateMat(numFunc,1, CV_64F) ); |
| CV_CALL( Jac = cvCreateMat(numFunc,numVal, CV_64F) ); |
| CV_CALL( delta = cvCreateMat(numVal, 1, CV_64F) ); |
| CV_CALL( matrJtJ = cvCreateMat(numVal, numVal, CV_64F) ); |
| CV_CALL( matrJtJN = cvCreateMat(numVal, numVal, CV_64F) ); |
| CV_CALL( matrJt = cvCreateMat(numVal, numFunc,CV_64F) ); |
| CV_CALL( vectB = cvCreateMat(numVal, 1, CV_64F) ); |
| |
| cvCopy(X0,vectX); |
| |
| /* ========== Main optimization loop ============ */ |
| double change; |
| int currIter; |
| double alpha; |
| |
| change = 1; |
| currIter = 0; |
| alpha = 0.001; |
| |
| do { |
| |
| /* Compute value of function */ |
| function(vectX,resFunc); |
| /* Print result of function to file */ |
| |
| /* Compute error */ |
| cvSub(observRes,resFunc,error); |
| |
| //valError = error_function(observRes,resFunc); |
| /* Need to use new version of computing error (norm) */ |
| valError = cvNorm(observRes,resFunc); |
| |
| /* Compute Jacobian for given point vectX */ |
| JacobianFunction(vectX,Jac); |
| |
| /* Define optimal delta for J'*J*delta=J'*error */ |
| /* compute J'J */ |
| cvMulTransposed(Jac,matrJtJ,1); |
| |
| cvCopy(matrJtJ,matrJtJN); |
| |
| /* compute J'*error */ |
| cvTranspose(Jac,matrJt); |
| cvmMul(matrJt,error,vectB); |
| |
| |
| /* Solve normal equation for given alpha and Jacobian */ |
| do |
| { |
| /* Increase diagonal elements by alpha */ |
| for( int i = 0; i < numVal; i++ ) |
| { |
| double val; |
| val = cvmGet(matrJtJ,i,i); |
| cvmSet(matrJtJN,i,i,(1+alpha)*val); |
| } |
| |
| /* Solve system to define delta */ |
| cvSolve(matrJtJN,vectB,delta,CV_SVD); |
| |
| /* We know delta and we can define new value of vector X */ |
| cvAdd(vectX,delta,vectNewX); |
| |
| /* Compute result of function for new vector X */ |
| function(vectNewX,resNewFunc); |
| cvSub(observRes,resNewFunc,errorNew); |
| |
| valNewError = cvNorm(observRes,resNewFunc); |
| |
| currIter++; |
| |
| if( valNewError < valError ) |
| {/* accept new value */ |
| valError = valNewError; |
| |
| /* Compute relative change of required parameter vectorX. change = norm(curr-prev) / norm(curr) ) */ |
| change = cvNorm(vectX, vectNewX, CV_RELATIVE_L2); |
| |
| alpha /= 10; |
| cvCopy(vectNewX,vectX); |
| break; |
| } |
| else |
| { |
| alpha *= 10; |
| } |
| |
| } while ( currIter < maxIter ); |
| /* new value of X and alpha were accepted */ |
| |
| } while ( change > epsilon && currIter < maxIter ); |
| |
| |
| /* result was computed */ |
| cvCopy(vectX,resultX); |
| |
| __END__; |
| |
| cvReleaseMat(&vectX); |
| cvReleaseMat(&vectNewX); |
| cvReleaseMat(&resFunc); |
| cvReleaseMat(&resNewFunc); |
| cvReleaseMat(&error); |
| cvReleaseMat(&errorNew); |
| cvReleaseMat(&Jac); |
| cvReleaseMat(&delta); |
| cvReleaseMat(&matrJtJ); |
| cvReleaseMat(&matrJtJN); |
| cvReleaseMat(&matrJt); |
| cvReleaseMat(&vectB); |
| |
| return; |
| } |
| |
| /*------------------------------------------------------------------------------*/ |
| #if 0 |
| //tests |
| void Jac_Func2(CvMat *vectX,CvMat *Jac) |
| { |
| double x = cvmGet(vectX,0,0); |
| double y = cvmGet(vectX,1,0); |
| cvmSet(Jac,0,0,2*(x-2)); |
| cvmSet(Jac,0,1,2*(y+3)); |
| |
| cvmSet(Jac,1,0,1); |
| cvmSet(Jac,1,1,1); |
| return; |
| } |
| |
| void Res_Func2(CvMat *vectX,CvMat *res) |
| { |
| double x = cvmGet(vectX,0,0); |
| double y = cvmGet(vectX,1,0); |
| cvmSet(res,0,0,(x-2)*(x-2)+(y+3)*(y+3)); |
| cvmSet(res,1,0,x+y); |
| |
| return; |
| } |
| |
| |
| double Err_Func2(CvMat *obs,CvMat *res) |
| { |
| CvMat *tmp; |
| tmp = cvCreateMat(obs->rows,1,CV_64F); |
| cvSub(obs,res,tmp); |
| |
| double e; |
| e = cvNorm(tmp); |
| |
| return e; |
| } |
| |
| |
| void TestOptimX2Y2() |
| { |
| CvMat vectX0; |
| double vectX0_dat[2]; |
| vectX0 = cvMat(2,1,CV_64F,vectX0_dat); |
| vectX0_dat[0] = 5; |
| vectX0_dat[1] = -7; |
| |
| CvMat observRes; |
| double observRes_dat[2]; |
| observRes = cvMat(2,1,CV_64F,observRes_dat); |
| observRes_dat[0] = 0; |
| observRes_dat[1] = -1; |
| observRes_dat[0] = 0; |
| observRes_dat[1] = -1.2; |
| |
| CvMat optimX; |
| double optimX_dat[2]; |
| optimX = cvMat(2,1,CV_64F,optimX_dat); |
| |
| |
| LevenbegrMarquardtOptimization( Jac_Func2, Res_Func2, Err_Func2, |
| &vectX0,&observRes,&optimX,100,0.000001); |
| |
| return; |
| |
| } |
| |
| #endif |
| |
| |
| |
