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
52typedef void (*pointer_LMJac)( const CvMat* src, CvMat* dst );
53typedef void (*pointer_LMFunc)( const CvMat* src, CvMat* dst );
54
55/* Optimization using Levenberg-Marquardt */
56void 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
253void 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
265void 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
276double 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
289void 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