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 */
cvLevenbergMarquardtOptimization(pointer_LMJac JacobianFunction,pointer_LMFunc function,CvMat * X0,CvMat * observRes,CvMat * resultX,int maxIter,double epsilon)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