vcs.maemo.org Git - opencv/blob - src/cv/cvcondens.cpp

   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 }