16acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*M/////////////////////////////////////////////////////////////////////////////////////// 26acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 36acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING. 46acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 56acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// By downloading, copying, installing or using the software you agree to this license. 66acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// If you do not agree to this license, do not download, install, 76acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// copy or use the software. 86acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 96acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Intel License Agreement 116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// For Open Source Computer Vision Library 126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Copyright (C) 2000, Intel Corporation, all rights reserved. 146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Third party copyrights are property of their respective owners. 156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Redistribution and use in source and binary forms, with or without modification, 176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// are permitted provided that the following conditions are met: 186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// * Redistribution's of source code must retain the above copyright notice, 206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// this list of conditions and the following disclaimer. 216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// * Redistribution's in binary form must reproduce the above copyright notice, 236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// this list of conditions and the following disclaimer in the documentation 246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// and/or other materials provided with the distribution. 256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// * The name of Intel Corporation may not be used to endorse or promote products 276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// derived from this software without specific prior written permission. 286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// This software is provided by the copyright holders and contributors "as is" and 306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// any express or implied warranties, including, but not limited to, the implied 316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// warranties of merchantability and fitness for a particular purpose are disclaimed. 326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// In no event shall the Intel Corporation or contributors be liable for any direct, 336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// indirect, incidental, special, exemplary, or consequential damages 346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// (including, but not limited to, procurement of substitute goods or services; 356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// loss of use, data, or profits; or business interruption) however caused 366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// and on any theory of liability, whether in contract, strict liability, 376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// or tort (including negligence or otherwise) arising in any way out of 386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// the use of this software, even if advised of the possibility of such damage. 396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//M*/ 416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#include "_cvaux.h" 436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#include "cvtypes.h" 446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#include <float.h> 456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#include <limits.h> 466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#include "cv.h" 476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Valery Mosyagin */ 496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//#define TRACKLEVMAR 516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renntypedef void (*pointer_LMJac)( const CvMat* src, CvMat* dst ); 536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renntypedef void (*pointer_LMFunc)( const CvMat* src, CvMat* dst ); 546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/* Optimization using Levenberg-Marquardt */ 566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid cvLevenbergMarquardtOptimization(pointer_LMJac JacobianFunction, 576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn pointer_LMFunc function, 586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /*pointer_Err error_function,*/ 596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *X0,CvMat *observRes,CvMat *resultX, 606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int maxIter,double epsilon) 616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* This is not sparce method */ 636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Make optimization using */ 646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* func - function to compute */ 656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* uses function to compute jacobian */ 666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Allocate memory */ 686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *vectX = 0; 696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *vectNewX = 0; 706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *resFunc = 0; 716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *resNewFunc = 0; 726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *error = 0; 736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *errorNew = 0; 746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *Jac = 0; 756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *delta = 0; 766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *matrJtJ = 0; 776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *matrJtJN = 0; 786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *matrJt = 0; 796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *vectB = 0; 806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvLevenbegrMarquardtOptimization" ); 826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( JacobianFunction == 0 || function == 0 || X0 == 0 || observRes == 0 || resultX == 0 ) 866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "Some of parameters is a NULL pointer" ); 886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CV_IS_MAT(X0) || !CV_IS_MAT(observRes) || !CV_IS_MAT(resultX) ) 916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnsupportedFormat, "Some of input parameters must be a matrices" ); 936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int numVal; 976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int numFunc; 986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double valError; 996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double valNewError; 1006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numVal = X0->rows; 1026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn numFunc = observRes->rows; 1036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* test input data */ 1056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( X0->cols != 1 ) 1066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Number of colomn of vector X0 must be 1" ); 1086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( observRes->cols != 1 ) 1116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Number of colomn of vector observed rusult must be 1" ); 1136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( resultX->cols != 1 || resultX->rows != numVal ) 1166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Size of result vector X must be equals to X0" ); 1186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( maxIter <= 0 ) 1216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Number of maximum iteration must be > 0" ); 1236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( epsilon < 0 ) 1266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsUnmatchedSizes, "Epsilon must be >= 0" ); 1286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* copy x0 to current value of x */ 1316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( vectX = cvCreateMat(numVal, 1, CV_64F) ); 1326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( vectNewX = cvCreateMat(numVal, 1, CV_64F) ); 1336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( resFunc = cvCreateMat(numFunc,1, CV_64F) ); 1346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( resNewFunc = cvCreateMat(numFunc,1, CV_64F) ); 1356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( error = cvCreateMat(numFunc,1, CV_64F) ); 1366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( errorNew = cvCreateMat(numFunc,1, CV_64F) ); 1376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( Jac = cvCreateMat(numFunc,numVal, CV_64F) ); 1386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( delta = cvCreateMat(numVal, 1, CV_64F) ); 1396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( matrJtJ = cvCreateMat(numVal, numVal, CV_64F) ); 1406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( matrJtJN = cvCreateMat(numVal, numVal, CV_64F) ); 1416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( matrJt = cvCreateMat(numVal, numFunc,CV_64F) ); 1426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( vectB = cvCreateMat(numVal, 1, CV_64F) ); 1436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCopy(X0,vectX); 1456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* ========== Main optimization loop ============ */ 1476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double change; 1486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int currIter; 1496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double alpha; 1506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn change = 1; 1526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn currIter = 0; 1536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn alpha = 0.001; 1546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn do { 1566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute value of function */ 1586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn function(vectX,resFunc); 1596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Print result of function to file */ 1606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute error */ 1626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSub(observRes,resFunc,error); 1636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn //valError = error_function(observRes,resFunc); 1656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Need to use new version of computing error (norm) */ 1666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn valError = cvNorm(observRes,resFunc); 1676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute Jacobian for given point vectX */ 1696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn JacobianFunction(vectX,Jac); 1706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Define optimal delta for J'*J*delta=J'*error */ 1726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* compute J'J */ 1736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvMulTransposed(Jac,matrJtJ,1); 1746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCopy(matrJtJ,matrJtJN); 1766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* compute J'*error */ 1786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvTranspose(Jac,matrJt); 1796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmMul(matrJt,error,vectB); 1806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Solve normal equation for given alpha and Jacobian */ 1836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn do 1846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Increase diagonal elements by alpha */ 1866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( int i = 0; i < numVal; i++ ) 1876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double val; 1896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn val = cvmGet(matrJtJ,i,i); 1906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(matrJtJN,i,i,(1+alpha)*val); 1916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Solve system to define delta */ 1946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSolve(matrJtJN,vectB,delta,CV_SVD); 1956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* We know delta and we can define new value of vector X */ 1976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvAdd(vectX,delta,vectNewX); 1986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute result of function for new vector X */ 2006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn function(vectNewX,resNewFunc); 2016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSub(observRes,resNewFunc,errorNew); 2026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn valNewError = cvNorm(observRes,resNewFunc); 2046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn currIter++; 2066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( valNewError < valError ) 2086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn {/* accept new value */ 2096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn valError = valNewError; 2106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Compute relative change of required parameter vectorX. change = norm(curr-prev) / norm(curr) ) */ 2126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn change = cvNorm(vectX, vectNewX, CV_RELATIVE_L2); 2136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn alpha /= 10; 2156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCopy(vectNewX,vectX); 2166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn break; 2176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn else 2196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn alpha *= 10; 2216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } while ( currIter < maxIter ); 2246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* new value of X and alpha were accepted */ 2256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } while ( change > epsilon && currIter < maxIter ); 2276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* result was computed */ 2306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvCopy(vectX,resultX); 2316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 2336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&vectX); 2356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&vectNewX); 2366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&resFunc); 2376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&resNewFunc); 2386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&error); 2396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&errorNew); 2406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&Jac); 2416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&delta); 2426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&matrJtJ); 2436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&matrJtJN); 2446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&matrJt); 2456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvReleaseMat(&vectB); 2466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 2486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 2496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*------------------------------------------------------------------------------*/ 2516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#if 0 2526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//tests 2536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid Jac_Func2(CvMat *vectX,CvMat *Jac) 2546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 2556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double x = cvmGet(vectX,0,0); 2566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double y = cvmGet(vectX,1,0); 2576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(Jac,0,0,2*(x-2)); 2586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(Jac,0,1,2*(y+3)); 2596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(Jac,1,0,1); 2616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(Jac,1,1,1); 2626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 2636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 2646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid Res_Func2(CvMat *vectX,CvMat *res) 2666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 2676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double x = cvmGet(vectX,0,0); 2686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double y = cvmGet(vectX,1,0); 2696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(res,0,0,(x-2)*(x-2)+(y+3)*(y+3)); 2706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvmSet(res,1,0,x+y); 2716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 2736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 2746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renndouble Err_Func2(CvMat *obs,CvMat *res) 2776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 2786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat *tmp; 2796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn tmp = cvCreateMat(obs->rows,1,CV_64F); 2806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvSub(obs,res,tmp); 2816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double e; 2836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn e = cvNorm(tmp); 2846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return e; 2866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 2876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Rennvoid TestOptimX2Y2() 2906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 2916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat vectX0; 2926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double vectX0_dat[2]; 2936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn vectX0 = cvMat(2,1,CV_64F,vectX0_dat); 2946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn vectX0_dat[0] = 5; 2956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn vectX0_dat[1] = -7; 2966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat observRes; 2986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double observRes_dat[2]; 2996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn observRes = cvMat(2,1,CV_64F,observRes_dat); 3006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn observRes_dat[0] = 0; 3016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn observRes_dat[1] = -1; 3026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn observRes_dat[0] = 0; 3036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn observRes_dat[1] = -1.2; 3046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvMat optimX; 3066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn double optimX_dat[2]; 3076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn optimX = cvMat(2,1,CV_64F,optimX_dat); 3086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn LevenbegrMarquardtOptimization( Jac_Func2, Res_Func2, Err_Func2, 3116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn &vectX0,&observRes,&optimX,100,0.000001); 3126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return; 3146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 3166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn#endif 3186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 3206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 321