1 /*M///////////////////////////////////////////////////////////////////////////////////////
3 // IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
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.
10 // Intel License Agreement
11 // For Open Source Computer Vision Library
13 // Copyright (C) 2000, Intel Corporation, all rights reserved.
14 // Third party copyrights are property of their respective owners.
16 // Redistribution and use in source and binary forms, with or without modification,
17 // are permitted provided that the following conditions are met:
19 // * Redistribution's of source code must retain the above copyright notice,
20 // this list of conditions and the following disclaimer.
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.
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.
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.
42 #include "precomp.hpp"
50 typedef void (*pointer_LMJac)( const CvMat* src, CvMat* dst );
51 typedef void (*pointer_LMFunc)( const CvMat* src, CvMat* dst );
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)
61 /* This is not sparse method */
62 /* Make optimization using */
63 /* func - function to compute */
64 /* uses function to compute jacobian */
70 CvMat *resNewFunc = 0;
80 CV_FUNCNAME( "cvLevenbegrMarquardtOptimization" );
84 if( JacobianFunction == 0 || function == 0 || X0 == 0 || observRes == 0 || resultX == 0 )
86 CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" );
89 if( !CV_IS_MAT(X0) || !CV_IS_MAT(observRes) || !CV_IS_MAT(resultX) )
91 CV_ERROR( CV_StsUnsupportedFormat, "Some of input parameters must be a matrices" );
101 numFunc = observRes->rows;
103 /* test input data */
106 CV_ERROR( CV_StsUnmatchedSizes, "Number of colomn of vector X0 must be 1" );
109 if( observRes->cols != 1 )
111 CV_ERROR( CV_StsUnmatchedSizes, "Number of colomn of vector observed rusult must be 1" );
114 if( resultX->cols != 1 || resultX->rows != numVal )
116 CV_ERROR( CV_StsUnmatchedSizes, "Size of result vector X must be equals to X0" );
121 CV_ERROR( CV_StsUnmatchedSizes, "Number of maximum iteration must be > 0" );
126 CV_ERROR( CV_StsUnmatchedSizes, "Epsilon must be >= 0" );
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) );
145 /* ========== Main optimization loop ============ */
156 /* Compute value of function */
157 function(vectX,resFunc);
158 /* Print result of function to file */
161 cvSub(observRes,resFunc,error);
163 //valError = error_function(observRes,resFunc);
164 /* Need to use new version of computing error (norm) */
165 valError = cvNorm(observRes,resFunc);
167 /* Compute Jacobian for given point vectX */
168 JacobianFunction(vectX,Jac);
170 /* Define optimal delta for J'*J*delta=J'*error */
172 cvMulTransposed(Jac,matrJtJ,1);
174 cvCopy(matrJtJ,matrJtJN);
176 /* compute J'*error */
177 cvTranspose(Jac,matrJt);
178 cvmMul(matrJt,error,vectB);
181 /* Solve normal equation for given alpha and Jacobian */
184 /* Increase diagonal elements by alpha */
185 for( int i = 0; i < numVal; i++ )
188 val = cvmGet(matrJtJ,i,i);
189 cvmSet(matrJtJN,i,i,(1+alpha)*val);
192 /* Solve system to define delta */
193 cvSolve(matrJtJN,vectB,delta,CV_SVD);
195 /* We know delta and we can define new value of vector X */
196 cvAdd(vectX,delta,vectNewX);
198 /* Compute result of function for new vector X */
199 function(vectNewX,resNewFunc);
200 cvSub(observRes,resNewFunc,errorNew);
202 valNewError = cvNorm(observRes,resNewFunc);
206 if( valNewError < valError )
207 {/* accept new value */
208 valError = valNewError;
210 /* Compute relative change of required parameter vectorX. change = norm(curr-prev) / norm(curr) ) */
211 change = cvNorm(vectX, vectNewX, CV_RELATIVE_L2);
214 cvCopy(vectNewX,vectX);
222 } while ( currIter < maxIter );
223 /* new value of X and alpha were accepted */
225 } while ( change > epsilon && currIter < maxIter );
228 /* result was computed */
229 cvCopy(vectX,resultX);
233 cvReleaseMat(&vectX);
234 cvReleaseMat(&vectNewX);
235 cvReleaseMat(&resFunc);
236 cvReleaseMat(&resNewFunc);
237 cvReleaseMat(&error);
238 cvReleaseMat(&errorNew);
240 cvReleaseMat(&delta);
241 cvReleaseMat(&matrJtJ);
242 cvReleaseMat(&matrJtJN);
243 cvReleaseMat(&matrJt);
244 cvReleaseMat(&vectB);
250 /*------------------------------------------------------------------------------*/
253 void Jac_Func2(CvMat *vectX,CvMat *Jac)
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));
265 void Res_Func2(CvMat *vectX,CvMat *res)
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));
276 double Err_Func2(CvMat *obs,CvMat *res)
279 tmp = cvCreateMat(obs->rows,1,CV_64F);
292 double vectX0_dat[2];
293 vectX0 = cvMat(2,1,CV_64F,vectX0_dat);
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;
306 double optimX_dat[2];
307 optimX = cvMat(2,1,CV_64F,optimX_dat);
310 LevenbegrMarquardtOptimization( Jac_Func2, Res_Func2, Err_Func2,
311 &vectX0,&observRes,&optimX,100,0.000001);