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.
43 /*F///////////////////////////////////////////////////////////////////////////////////////
44 // Name: cvCreateConDensation
45 // Purpose: Creating CvConDensation structure and allocating memory for it
48 // Kalman - double pointer to CvConDensation structure
49 // DP - dimension of the dynamical vector
50 // MP - dimension of the measurement vector
51 // SamplesNum - number of samples in sample set used in algorithm
57 CV_IMPL CvConDensation* cvCreateConDensation( int DP, int MP, int SamplesNum )
60 CvConDensation *CD = 0;
62 CV_FUNCNAME( "cvCreateConDensation" );
65 if( DP < 0 || MP < 0 || SamplesNum < 0 )
66 CV_ERROR( CV_StsOutOfRange, "" );
68 /* allocating memory for the structure */
69 CV_CALL( CD = (CvConDensation *) cvAlloc( sizeof( CvConDensation )));
70 /* setting structure params */
71 CD->SamplesNum = SamplesNum;
74 /* allocating memory for structure fields */
75 CV_CALL( CD->flSamples = (float **) cvAlloc( sizeof( float * ) * SamplesNum ));
76 CV_CALL( CD->flNewSamples = (float **) cvAlloc( sizeof( float * ) * SamplesNum ));
77 CV_CALL( CD->flSamples[0] = (float *) cvAlloc( sizeof( float ) * SamplesNum * DP ));
78 CV_CALL( CD->flNewSamples[0] = (float *) cvAlloc( sizeof( float ) * SamplesNum * DP ));
80 /* setting pointers in pointer's arrays */
81 for( i = 1; i < SamplesNum; i++ )
83 CD->flSamples[i] = CD->flSamples[i - 1] + DP;
84 CD->flNewSamples[i] = CD->flNewSamples[i - 1] + DP;
87 CV_CALL( CD->State = (float *) cvAlloc( sizeof( float ) * DP ));
88 CV_CALL( CD->DynamMatr = (float *) cvAlloc( sizeof( float ) * DP * DP ));
89 CV_CALL( CD->flConfidence = (float *) cvAlloc( sizeof( float ) * SamplesNum ));
90 CV_CALL( CD->flCumulative = (float *) cvAlloc( sizeof( float ) * SamplesNum ));
92 CV_CALL( CD->RandS = (CvRandState *) cvAlloc( sizeof( CvRandState ) * DP ));
93 CV_CALL( CD->Temp = (float *) cvAlloc( sizeof( float ) * DP ));
94 CV_CALL( CD->RandomSample = (float *) cvAlloc( sizeof( float ) * DP ));
96 /* Returning created structure */
102 /*F///////////////////////////////////////////////////////////////////////////////////////
103 // Name: cvReleaseConDensation
104 // Purpose: Releases CvConDensation structure and frees memory allocated for it
107 // Kalman - double pointer to CvConDensation structure
108 // DP - dimension of the dynamical vector
109 // MP - dimension of the measurement vector
110 // SamplesNum - number of samples in sample set used in algorithm
116 cvReleaseConDensation( CvConDensation ** ConDensation )
118 CV_FUNCNAME( "cvReleaseConDensation" );
121 CvConDensation *CD = *ConDensation;
124 CV_ERROR( CV_StsNullPtr, "" );
129 /* freeing the memory */
130 cvFree( &CD->State );
131 cvFree( &CD->DynamMatr);
132 cvFree( &CD->flConfidence );
133 cvFree( &CD->flCumulative );
134 cvFree( &CD->flSamples[0] );
135 cvFree( &CD->flNewSamples[0] );
136 cvFree( &CD->flSamples );
137 cvFree( &CD->flNewSamples );
139 cvFree( &CD->RandS );
140 cvFree( &CD->RandomSample );
141 /* release structure */
142 cvFree( ConDensation );
148 /*F///////////////////////////////////////////////////////////////////////////////////////
149 // Name: cvConDensUpdateByTime
150 // Purpose: Performing Time Update routine for ConDensation algorithm
153 // Kalman - pointer to CvConDensation structure
159 cvConDensUpdateByTime( CvConDensation * ConDens )
164 CV_FUNCNAME( "cvConDensUpdateByTime" );
168 CV_ERROR( CV_StsNullPtr, "" );
170 /* Sets Temp to Zero */
171 icvSetZero_32f( ConDens->Temp, ConDens->DP, 1 );
173 /* Calculating the Mean */
174 for( i = 0; i < ConDens->SamplesNum; i++ )
176 icvScaleVector_32f( ConDens->flSamples[i], ConDens->State, ConDens->DP,
177 ConDens->flConfidence[i] );
178 icvAddVector_32f( ConDens->Temp, ConDens->State, ConDens->Temp, ConDens->DP );
179 Sum += ConDens->flConfidence[i];
180 ConDens->flCumulative[i] = Sum;
183 /* Taking the new vector from transformation of mean by dynamics matrix */
185 icvScaleVector_32f( ConDens->Temp, ConDens->Temp, ConDens->DP, 1.f / Sum );
186 icvTransformVector_32f( ConDens->DynamMatr, ConDens->Temp, ConDens->State, ConDens->DP,
188 Sum = Sum / ConDens->SamplesNum;
190 /* Updating the set of random samples */
191 for( i = 0; i < ConDens->SamplesNum; i++ )
194 while( (ConDens->flCumulative[j] <= (float) i * Sum)&&(j<ConDens->SamplesNum-1))
198 icvCopyVector_32f( ConDens->flSamples[j], ConDens->DP, ConDens->flNewSamples[i] );
201 /* Adding the random-generated vector to every vector in sample set */
202 for( i = 0; i < ConDens->SamplesNum; i++ )
204 for( j = 0; j < ConDens->DP; j++ )
206 cvbRand( ConDens->RandS + j, ConDens->RandomSample + j, 1 );
209 icvTransformVector_32f( ConDens->DynamMatr, ConDens->flNewSamples[i],
210 ConDens->flSamples[i], ConDens->DP, ConDens->DP );
211 icvAddVector_32f( ConDens->flSamples[i], ConDens->RandomSample, ConDens->flSamples[i],
218 /*F///////////////////////////////////////////////////////////////////////////////////////
219 // Name: cvConDensInitSamplSet
220 // Purpose: Performing Time Update routine for ConDensation algorithm
223 // conDens - pointer to CvConDensation structure
224 // lowerBound - vector of lower bounds used to random update of sample set
225 // lowerBound - vector of upper bounds used to random update of sample set
232 cvConDensInitSampleSet( CvConDensation * conDens, CvMat * lowerBound, CvMat * upperBound )
237 float Prob = 1.f / conDens->SamplesNum;
239 CV_FUNCNAME( "cvConDensInitSampleSet" );
242 if( !conDens || !lowerBound || !upperBound )
243 CV_ERROR( CV_StsNullPtr, "" );
245 if( CV_MAT_TYPE(lowerBound->type) != CV_32FC1 ||
246 !CV_ARE_TYPES_EQ(lowerBound,upperBound) )
247 CV_ERROR( CV_StsBadArg, "source has not appropriate format" );
249 if( (lowerBound->cols != 1) || (upperBound->cols != 1) )
250 CV_ERROR( CV_StsBadArg, "source has not appropriate size" );
252 if( (lowerBound->rows != conDens->DP) || (upperBound->rows != conDens->DP) )
253 CV_ERROR( CV_StsBadArg, "source has not appropriate size" );
255 LBound = lowerBound->data.fl;
256 UBound = upperBound->data.fl;
257 /* Initializing the structures to create initial Sample set */
258 for( i = 0; i < conDens->DP; i++ )
260 cvRandInit( &(conDens->RandS[i]),
265 /* Generating the samples */
266 for( j = 0; j < conDens->SamplesNum; j++ )
268 for( i = 0; i < conDens->DP; i++ )
270 cvbRand( conDens->RandS + i, conDens->flSamples[j] + i, 1 );
272 conDens->flConfidence[j] = Prob;
274 /* Reinitializes the structures to update samples randomly */
275 for( i = 0; i < conDens->DP; i++ )
277 cvRandInit( &(conDens->RandS[i]),
278 (LBound[i] - UBound[i]) / 5,
279 (UBound[i] - LBound[i]) / 5,