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#include "_cv.h" 426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*F/////////////////////////////////////////////////////////////////////////////////////// 446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Name: cvCreateConDensation 456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Purpose: Creating CvConDensation structure and allocating memory for it 466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Context: 476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Parameters: 486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Kalman - double pointer to CvConDensation structure 496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// DP - dimension of the dynamical vector 506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// MP - dimension of the measurement vector 516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// SamplesNum - number of samples in sample set used in algorithm 526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Returns: 536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Notes: 546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//F*/ 566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL CvConDensation* cvCreateConDensation( int DP, int MP, int SamplesNum ) 586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i; 606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvConDensation *CD = 0; 616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvCreateConDensation" ); 636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( DP < 0 || MP < 0 || SamplesNum < 0 ) 666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsOutOfRange, "" ); 676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* allocating memory for the structure */ 696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( CD = (CvConDensation *) cvAlloc( sizeof( CvConDensation ))); 706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* setting structure params */ 716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CD->SamplesNum = SamplesNum; 726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CD->DP = DP; 736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CD->MP = MP; 746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* allocating memory for structure fields */ 756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( CD->flSamples = (float **) cvAlloc( sizeof( float * ) * SamplesNum )); 766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( CD->flNewSamples = (float **) cvAlloc( sizeof( float * ) * SamplesNum )); 776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( CD->flSamples[0] = (float *) cvAlloc( sizeof( float ) * SamplesNum * DP )); 786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( CD->flNewSamples[0] = (float *) cvAlloc( sizeof( float ) * SamplesNum * DP )); 796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* setting pointers in pointer's arrays */ 816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 1; i < SamplesNum; i++ ) 826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CD->flSamples[i] = CD->flSamples[i - 1] + DP; 846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CD->flNewSamples[i] = CD->flNewSamples[i - 1] + DP; 856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( CD->State = (float *) cvAlloc( sizeof( float ) * DP )); 886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( CD->DynamMatr = (float *) cvAlloc( sizeof( float ) * DP * DP )); 896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( CD->flConfidence = (float *) cvAlloc( sizeof( float ) * SamplesNum )); 906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( CD->flCumulative = (float *) cvAlloc( sizeof( float ) * SamplesNum )); 916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( CD->RandS = (CvRandState *) cvAlloc( sizeof( CvRandState ) * DP )); 936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( CD->Temp = (float *) cvAlloc( sizeof( float ) * DP )); 946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_CALL( CD->RandomSample = (float *) cvAlloc( sizeof( float ) * DP )); 956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Returning created structure */ 976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn return CD; 1006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 1016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*F/////////////////////////////////////////////////////////////////////////////////////// 1036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Name: cvReleaseConDensation 1046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Purpose: Releases CvConDensation structure and frees memory allocated for it 1056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Context: 1066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Parameters: 1076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Kalman - double pointer to CvConDensation structure 1086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// DP - dimension of the dynamical vector 1096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// MP - dimension of the measurement vector 1106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// SamplesNum - number of samples in sample set used in algorithm 1116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Returns: 1126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Notes: 1136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 1146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//F*/ 1156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL void 1166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvReleaseConDensation( CvConDensation ** ConDensation ) 1176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 1186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvReleaseConDensation" ); 1196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 1206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CvConDensation *CD = *ConDensation; 1226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !ConDensation ) 1246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "" ); 1256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !CD ) 1276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn EXIT; 1286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* freeing the memory */ 1306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &CD->State ); 1316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &CD->DynamMatr); 1326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &CD->flConfidence ); 1336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &CD->flCumulative ); 1346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &CD->flSamples[0] ); 1356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &CD->flNewSamples[0] ); 1366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &CD->flSamples ); 1376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &CD->flNewSamples ); 1386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &CD->Temp ); 1396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &CD->RandS ); 1406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( &CD->RandomSample ); 1416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* release structure */ 1426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvFree( ConDensation ); 1436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 1456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 1476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*F/////////////////////////////////////////////////////////////////////////////////////// 1496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Name: cvConDensUpdateByTime 1506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Purpose: Performing Time Update routine for ConDensation algorithm 1516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Context: 1526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Parameters: 1536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Kalman - pointer to CvConDensation structure 1546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Returns: 1556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Notes: 1566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 1576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//F*/ 1586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL void 1596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvConDensUpdateByTime( CvConDensation * ConDens ) 1606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 1616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, j; 1626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float Sum = 0; 1636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvConDensUpdateByTime" ); 1656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 1666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !ConDens ) 1686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "" ); 1696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Sets Temp to Zero */ 1716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvSetZero_32f( ConDens->Temp, ConDens->DP, 1 ); 1726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Calculating the Mean */ 1746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < ConDens->SamplesNum; i++ ) 1756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvScaleVector_32f( ConDens->flSamples[i], ConDens->State, ConDens->DP, 1776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ConDens->flConfidence[i] ); 1786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvAddVector_32f( ConDens->Temp, ConDens->State, ConDens->Temp, ConDens->DP ); 1796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Sum += ConDens->flConfidence[i]; 1806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ConDens->flCumulative[i] = Sum; 1816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Taking the new vector from transformation of mean by dynamics matrix */ 1846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1856acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvScaleVector_32f( ConDens->Temp, ConDens->Temp, ConDens->DP, 1.f / Sum ); 1866acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvTransformVector_32f( ConDens->DynamMatr, ConDens->Temp, ConDens->State, ConDens->DP, 1876acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ConDens->DP ); 1886acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn Sum = Sum / ConDens->SamplesNum; 1896acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 1906acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Updating the set of random samples */ 1916acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < ConDens->SamplesNum; i++ ) 1926acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1936acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn j = 0; 1946acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn while( (ConDens->flCumulative[j] <= (float) i * Sum)&&(j<ConDens->SamplesNum-1)) 1956acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 1966acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn j++; 1976acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 1986acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvCopyVector_32f( ConDens->flSamples[j], ConDens->DP, ConDens->flNewSamples[i] ); 1996acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2006acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2016acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Adding the random-generated vector to every vector in sample set */ 2026acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < ConDens->SamplesNum; i++ ) 2036acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2046acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < ConDens->DP; j++ ) 2056acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2066acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvbRand( ConDens->RandS + j, ConDens->RandomSample + j, 1 ); 2076acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2086acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2096acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvTransformVector_32f( ConDens->DynamMatr, ConDens->flNewSamples[i], 2106acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ConDens->flSamples[i], ConDens->DP, ConDens->DP ); 2116acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn icvAddVector_32f( ConDens->flSamples[i], ConDens->RandomSample, ConDens->flSamples[i], 2126acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn ConDens->DP ); 2136acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2146acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2156acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 2166acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 2176acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2186acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn/*F/////////////////////////////////////////////////////////////////////////////////////// 2196acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Name: cvConDensInitSamplSet 2206acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Purpose: Performing Time Update routine for ConDensation algorithm 2216acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Context: 2226acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Parameters: 2236acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// conDens - pointer to CvConDensation structure 2246acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// lowerBound - vector of lower bounds used to random update of sample set 2256acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// lowerBound - vector of upper bounds used to random update of sample set 2266acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Returns: 2276acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// Notes: 2286acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn// 2296acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn//F*/ 2306acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2316acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RennCV_IMPL void 2326acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius RenncvConDensInitSampleSet( CvConDensation * conDens, CvMat * lowerBound, CvMat * upperBound ) 2336acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn{ 2346acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn int i, j; 2356acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float *LBound; 2366acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float *UBound; 2376acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn float Prob = 1.f / conDens->SamplesNum; 2386acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2396acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_FUNCNAME( "cvConDensInitSampleSet" ); 2406acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __BEGIN__; 2416acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2426acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( !conDens || !lowerBound || !upperBound ) 2436acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsNullPtr, "" ); 2446acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2456acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( CV_MAT_TYPE(lowerBound->type) != CV_32FC1 || 2466acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn !CV_ARE_TYPES_EQ(lowerBound,upperBound) ) 2476acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadArg, "source has not appropriate format" ); 2486acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2496acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (lowerBound->cols != 1) || (upperBound->cols != 1) ) 2506acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadArg, "source has not appropriate size" ); 2516acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2526acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn if( (lowerBound->rows != conDens->DP) || (upperBound->rows != conDens->DP) ) 2536acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn CV_ERROR( CV_StsBadArg, "source has not appropriate size" ); 2546acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2556acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn LBound = lowerBound->data.fl; 2566acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn UBound = upperBound->data.fl; 2576acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Initializing the structures to create initial Sample set */ 2586acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < conDens->DP; i++ ) 2596acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2606acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvRandInit( &(conDens->RandS[i]), 2616acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn LBound[i], 2626acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn UBound[i], 2636acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn i ); 2646acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2656acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Generating the samples */ 2666acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( j = 0; j < conDens->SamplesNum; j++ ) 2676acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2686acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < conDens->DP; i++ ) 2696acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2706acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvbRand( conDens->RandS + i, conDens->flSamples[j] + i, 1 ); 2716acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2726acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn conDens->flConfidence[j] = Prob; 2736acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2746acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn /* Reinitializes the structures to update samples randomly */ 2756acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn for( i = 0; i < conDens->DP; i++ ) 2766acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn { 2776acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn cvRandInit( &(conDens->RandS[i]), 2786acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (LBound[i] - UBound[i]) / 5, 2796acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn (UBound[i] - LBound[i]) / 5, 2806acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn i); 2816acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn } 2826acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn 2836acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn __END__; 2846acb9a7ea3d7564944e12cbc73a857b88c1301eeMarius Renn} 285