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 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 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 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 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
