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 #include "_cv.h"
42
43 /*F///////////////////////////////////////////////////////////////////////////////////////
44 // Name: cvCreateConDensation
45 // Purpose: Creating CvConDensation structure and allocating memory for it
46 // Context:
47 // Parameters:
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
52 // Returns:
53 // Notes:
54 //
55 //F*/
56
cvCreateConDensation(int DP,int MP,int SamplesNum)57 CV_IMPL CvConDensation* cvCreateConDensation( int DP, int MP, int SamplesNum )
58 {
59 int i;
60 CvConDensation *CD = 0;
61
62 CV_FUNCNAME( "cvCreateConDensation" );
63 __BEGIN__;
64
65 if( DP < 0 || MP < 0 || SamplesNum < 0 )
66 CV_ERROR( CV_StsOutOfRange, "" );
67
68 /* allocating memory for the structure */
69 CV_CALL( CD = (CvConDensation *) cvAlloc( sizeof( CvConDensation )));
70 /* setting structure params */
71 CD->SamplesNum = SamplesNum;
72 CD->DP = DP;
73 CD->MP = MP;
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 ));
79
80 /* setting pointers in pointer's arrays */
81 for( i = 1; i < SamplesNum; i++ )
82 {
83 CD->flSamples[i] = CD->flSamples[i - 1] + DP;
84 CD->flNewSamples[i] = CD->flNewSamples[i - 1] + DP;
85 }
86
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 ));
91
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 ));
95
96 /* Returning created structure */
97 __END__;
98
99 return CD;
100 }
101
102 /*F///////////////////////////////////////////////////////////////////////////////////////
103 // Name: cvReleaseConDensation
104 // Purpose: Releases CvConDensation structure and frees memory allocated for it
105 // Context:
106 // Parameters:
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
111 // Returns:
112 // Notes:
113 //
114 //F*/
115 CV_IMPL void
cvReleaseConDensation(CvConDensation ** ConDensation)116 cvReleaseConDensation( CvConDensation ** ConDensation )
117 {
118 CV_FUNCNAME( "cvReleaseConDensation" );
119 __BEGIN__;
120
121 CvConDensation *CD = *ConDensation;
122
123 if( !ConDensation )
124 CV_ERROR( CV_StsNullPtr, "" );
125
126 if( !CD )
127 EXIT;
128
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 );
138 cvFree( &CD->Temp );
139 cvFree( &CD->RandS );
140 cvFree( &CD->RandomSample );
141 /* release structure */
142 cvFree( ConDensation );
143
144 __END__;
145
146 }
147
148 /*F///////////////////////////////////////////////////////////////////////////////////////
149 // Name: cvConDensUpdateByTime
150 // Purpose: Performing Time Update routine for ConDensation algorithm
151 // Context:
152 // Parameters:
153 // Kalman - pointer to CvConDensation structure
154 // Returns:
155 // Notes:
156 //
157 //F*/
158 CV_IMPL void
cvConDensUpdateByTime(CvConDensation * ConDens)159 cvConDensUpdateByTime( CvConDensation * ConDens )
160 {
161 int i, j;
162 float Sum = 0;
163
164 CV_FUNCNAME( "cvConDensUpdateByTime" );
165 __BEGIN__;
166
167 if( !ConDens )
168 CV_ERROR( CV_StsNullPtr, "" );
169
170 /* Sets Temp to Zero */
171 icvSetZero_32f( ConDens->Temp, ConDens->DP, 1 );
172
173 /* Calculating the Mean */
174 for( i = 0; i < ConDens->SamplesNum; i++ )
175 {
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;
181 }
182
183 /* Taking the new vector from transformation of mean by dynamics matrix */
184
185 icvScaleVector_32f( ConDens->Temp, ConDens->Temp, ConDens->DP, 1.f / Sum );
186 icvTransformVector_32f( ConDens->DynamMatr, ConDens->Temp, ConDens->State, ConDens->DP,
187 ConDens->DP );
188 Sum = Sum / ConDens->SamplesNum;
189
190 /* Updating the set of random samples */
191 for( i = 0; i < ConDens->SamplesNum; i++ )
192 {
193 j = 0;
194 while( (ConDens->flCumulative[j] <= (float) i * Sum)&&(j<ConDens->SamplesNum-1))
195 {
196 j++;
197 }
198 icvCopyVector_32f( ConDens->flSamples[j], ConDens->DP, ConDens->flNewSamples[i] );
199 }
200
201 /* Adding the random-generated vector to every vector in sample set */
202 for( i = 0; i < ConDens->SamplesNum; i++ )
203 {
204 for( j = 0; j < ConDens->DP; j++ )
205 {
206 cvbRand( ConDens->RandS + j, ConDens->RandomSample + j, 1 );
207 }
208
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],
212 ConDens->DP );
213 }
214
215 __END__;
216 }
217
218 /*F///////////////////////////////////////////////////////////////////////////////////////
219 // Name: cvConDensInitSamplSet
220 // Purpose: Performing Time Update routine for ConDensation algorithm
221 // Context:
222 // Parameters:
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
226 // Returns:
227 // Notes:
228 //
229 //F*/
230
231 CV_IMPL void
cvConDensInitSampleSet(CvConDensation * conDens,CvMat * lowerBound,CvMat * upperBound)232 cvConDensInitSampleSet( CvConDensation * conDens, CvMat * lowerBound, CvMat * upperBound )
233 {
234 int i, j;
235 float *LBound;
236 float *UBound;
237 float Prob = 1.f / conDens->SamplesNum;
238
239 CV_FUNCNAME( "cvConDensInitSampleSet" );
240 __BEGIN__;
241
242 if( !conDens || !lowerBound || !upperBound )
243 CV_ERROR( CV_StsNullPtr, "" );
244
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" );
248
249 if( (lowerBound->cols != 1) || (upperBound->cols != 1) )
250 CV_ERROR( CV_StsBadArg, "source has not appropriate size" );
251
252 if( (lowerBound->rows != conDens->DP) || (upperBound->rows != conDens->DP) )
253 CV_ERROR( CV_StsBadArg, "source has not appropriate size" );
254
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++ )
259 {
260 cvRandInit( &(conDens->RandS[i]),
261 LBound[i],
262 UBound[i],
263 i );
264 }
265 /* Generating the samples */
266 for( j = 0; j < conDens->SamplesNum; j++ )
267 {
268 for( i = 0; i < conDens->DP; i++ )
269 {
270 cvbRand( conDens->RandS + i, conDens->flSamples[j] + i, 1 );
271 }
272 conDens->flConfidence[j] = Prob;
273 }
274 /* Reinitializes the structures to update samples randomly */
275 for( i = 0; i < conDens->DP; i++ )
276 {
277 cvRandInit( &(conDens->RandS[i]),
278 (LBound[i] - UBound[i]) / 5,
279 (UBound[i] - LBound[i]) / 5,
280 i);
281 }
282
283 __END__;
284 }
285