--- /dev/null
+/*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
+
+
+
projects / opencv / blobdiff