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.
41 #include "precomp.hpp"
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 if( DP < 0 || MP < 0 || SamplesNum < 0 )
63 CV_Error( CV_StsOutOfRange, "" );
65 /* allocating memory for the structure */
66 CD = (CvConDensation *) cvAlloc( sizeof( CvConDensation ));
67 /* setting structure params */
68 CD->SamplesNum = SamplesNum;
71 /* allocating memory for structure fields */
72 CD->flSamples = (float **) cvAlloc( sizeof( float * ) * SamplesNum );
73 CD->flNewSamples = (float **) cvAlloc( sizeof( float * ) * SamplesNum );
74 CD->flSamples[0] = (float *) cvAlloc( sizeof( float ) * SamplesNum * DP );
75 CD->flNewSamples[0] = (float *) cvAlloc( sizeof( float ) * SamplesNum * DP );
77 /* setting pointers in pointer's arrays */
78 for( i = 1; i < SamplesNum; i++ )
80 CD->flSamples[i] = CD->flSamples[i - 1] + DP;
81 CD->flNewSamples[i] = CD->flNewSamples[i - 1] + DP;
84 CD->State = (float *) cvAlloc( sizeof( float ) * DP );
85 CD->DynamMatr = (float *) cvAlloc( sizeof( float ) * DP * DP );
86 CD->flConfidence = (float *) cvAlloc( sizeof( float ) * SamplesNum );
87 CD->flCumulative = (float *) cvAlloc( sizeof( float ) * SamplesNum );
89 CD->RandS = (CvRandState *) cvAlloc( sizeof( CvRandState ) * DP );
90 CD->Temp = (float *) cvAlloc( sizeof( float ) * DP );
91 CD->RandomSample = (float *) cvAlloc( sizeof( float ) * DP );
93 /* Returning created structure */
98 /*F///////////////////////////////////////////////////////////////////////////////////////
99 // Name: cvReleaseConDensation
100 // Purpose: Releases CvConDensation structure and frees memory allocated for it
103 // Kalman - double pointer to CvConDensation structure
104 // DP - dimension of the dynamical vector
105 // MP - dimension of the measurement vector
106 // SamplesNum - number of samples in sample set used in algorithm
112 cvReleaseConDensation( CvConDensation ** ConDensation )
114 CvConDensation *CD = *ConDensation;
117 CV_Error( CV_StsNullPtr, "" );
122 /* freeing the memory */
123 cvFree( &CD->State );
124 cvFree( &CD->DynamMatr);
125 cvFree( &CD->flConfidence );
126 cvFree( &CD->flCumulative );
127 cvFree( &CD->flSamples[0] );
128 cvFree( &CD->flNewSamples[0] );
129 cvFree( &CD->flSamples );
130 cvFree( &CD->flNewSamples );
132 cvFree( &CD->RandS );
133 cvFree( &CD->RandomSample );
134 /* release structure */
135 cvFree( ConDensation );
138 /*F///////////////////////////////////////////////////////////////////////////////////////
139 // Name: cvConDensUpdateByTime
140 // Purpose: Performing Time Update routine for ConDensation algorithm
143 // Kalman - pointer to CvConDensation structure
149 cvConDensUpdateByTime( CvConDensation * ConDens )
155 CV_Error( CV_StsNullPtr, "" );
157 /* Sets Temp to Zero */
158 icvSetZero_32f( ConDens->Temp, ConDens->DP, 1 );
160 /* Calculating the Mean */
161 for( i = 0; i < ConDens->SamplesNum; i++ )
163 icvScaleVector_32f( ConDens->flSamples[i], ConDens->State, ConDens->DP,
164 ConDens->flConfidence[i] );
165 icvAddVector_32f( ConDens->Temp, ConDens->State, ConDens->Temp, ConDens->DP );
166 Sum += ConDens->flConfidence[i];
167 ConDens->flCumulative[i] = Sum;
170 /* Taking the new vector from transformation of mean by dynamics matrix */
172 icvScaleVector_32f( ConDens->Temp, ConDens->Temp, ConDens->DP, 1.f / Sum );
173 icvTransformVector_32f( ConDens->DynamMatr, ConDens->Temp, ConDens->State, ConDens->DP,
175 Sum = Sum / ConDens->SamplesNum;
177 /* Updating the set of random samples */
178 for( i = 0; i < ConDens->SamplesNum; i++ )
181 while( (ConDens->flCumulative[j] <= (float) i * Sum)&&(j<ConDens->SamplesNum-1))
185 icvCopyVector_32f( ConDens->flSamples[j], ConDens->DP, ConDens->flNewSamples[i] );
188 /* Adding the random-generated vector to every vector in sample set */
189 for( i = 0; i < ConDens->SamplesNum; i++ )
191 for( j = 0; j < ConDens->DP; j++ )
193 cvbRand( ConDens->RandS + j, ConDens->RandomSample + j, 1 );
196 icvTransformVector_32f( ConDens->DynamMatr, ConDens->flNewSamples[i],
197 ConDens->flSamples[i], ConDens->DP, ConDens->DP );
198 icvAddVector_32f( ConDens->flSamples[i], ConDens->RandomSample, ConDens->flSamples[i],
203 /*F///////////////////////////////////////////////////////////////////////////////////////
204 // Name: cvConDensInitSamplSet
205 // Purpose: Performing Time Update routine for ConDensation algorithm
208 // conDens - pointer to CvConDensation structure
209 // lowerBound - vector of lower bounds used to random update of sample set
210 // lowerBound - vector of upper bounds used to random update of sample set
217 cvConDensInitSampleSet( CvConDensation * conDens, CvMat * lowerBound, CvMat * upperBound )
222 float Prob = 1.f / conDens->SamplesNum;
224 if( !conDens || !lowerBound || !upperBound )
225 CV_Error( CV_StsNullPtr, "" );
227 if( CV_MAT_TYPE(lowerBound->type) != CV_32FC1 ||
228 !CV_ARE_TYPES_EQ(lowerBound,upperBound) )
229 CV_Error( CV_StsBadArg, "source has not appropriate format" );
231 if( (lowerBound->cols != 1) || (upperBound->cols != 1) )
232 CV_Error( CV_StsBadArg, "source has not appropriate size" );
234 if( (lowerBound->rows != conDens->DP) || (upperBound->rows != conDens->DP) )
235 CV_Error( CV_StsBadArg, "source has not appropriate size" );
237 LBound = lowerBound->data.fl;
238 UBound = upperBound->data.fl;
239 /* Initializing the structures to create initial Sample set */
240 for( i = 0; i < conDens->DP; i++ )
242 cvRandInit( &(conDens->RandS[i]),
247 /* Generating the samples */
248 for( j = 0; j < conDens->SamplesNum; j++ )
250 for( i = 0; i < conDens->DP; i++ )
252 cvbRand( conDens->RandS + i, conDens->flSamples[j] + i, 1 );
254 conDens->flConfidence[j] = Prob;
256 /* Reinitializes the structures to update samples randomly */
257 for( i = 0; i < conDens->DP; i++ )
259 cvRandInit( &(conDens->RandS[i]),
260 (LBound[i] - UBound[i]) / 5,
261 (UBound[i] - LBound[i]) / 5,