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 57CV_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*/ 115CV_IMPL void 116cvReleaseConDensation( 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*/ 158CV_IMPL void 159cvConDensUpdateByTime( 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 231CV_IMPL void 232cvConDensInitSampleSet( 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