+++ /dev/null
-/*M///////////////////////////////////////////////////////////////////////////////////////
-//
-// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
-//
-// By downloading, copying, installing or using the software you agree to this license.
-// If you do not agree to this license, do not download, install,
-// copy or use the software.
-//
-//
-// Intel License Agreement
-// For Open Source Computer Vision Library
-//
-// Copyright (C) 2000, Intel Corporation, all rights reserved.
-// Third party copyrights are property of their respective owners.
-//
-// Redistribution and use in source and binary forms, with or without modification,
-// are permitted provided that the following conditions are met:
-//
-// * Redistribution's of source code must retain the above copyright notice,
-// this list of conditions and the following disclaimer.
-//
-// * Redistribution's in binary form must reproduce the above copyright notice,
-// this list of conditions and the following disclaimer in the documentation
-// and/or other materials provided with the distribution.
-//
-// * The name of Intel Corporation may not be used to endorse or promote products
-// derived from this software without specific prior written permission.
-//
-// This software is provided by the copyright holders and contributors "as is" and
-// any express or implied warranties, including, but not limited to, the implied
-// warranties of merchantability and fitness for a particular purpose are disclaimed.
-// In no event shall the Intel Corporation or contributors be liable for any direct,
-// indirect, incidental, special, exemplary, or consequential damages
-// (including, but not limited to, procurement of substitute goods or services;
-// loss of use, data, or profits; or business interruption) however caused
-// and on any theory of liability, whether in contract, strict liability,
-// or tort (including negligence or otherwise) arising in any way out of
-// the use of this software, even if advised of the possibility of such damage.
-//
-//M*/
-#include "_cv.h"
-
-/*F///////////////////////////////////////////////////////////////////////////////////////
-// Name: cvCreateConDensation
-// Purpose: Creating CvConDensation structure and allocating memory for it
-// Context:
-// Parameters:
-// Kalman - double pointer to CvConDensation structure
-// DP - dimension of the dynamical vector
-// MP - dimension of the measurement vector
-// SamplesNum - number of samples in sample set used in algorithm
-// Returns:
-// Notes:
-//
-//F*/
-
-CV_IMPL CvConDensation* cvCreateConDensation( int DP, int MP, int SamplesNum )
-{
- int i;
- CvConDensation *CD = 0;
-
- CV_FUNCNAME( "cvCreateConDensation" );
- __BEGIN__;
-
- if( DP < 0 || MP < 0 || SamplesNum < 0 )
- CV_ERROR( CV_StsOutOfRange, "" );
-
- /* allocating memory for the structure */
- CV_CALL( CD = (CvConDensation *) cvAlloc( sizeof( CvConDensation )));
- /* setting structure params */
- CD->SamplesNum = SamplesNum;
- CD->DP = DP;
- CD->MP = MP;
- /* allocating memory for structure fields */
- CV_CALL( CD->flSamples = (float **) cvAlloc( sizeof( float * ) * SamplesNum ));
- CV_CALL( CD->flNewSamples = (float **) cvAlloc( sizeof( float * ) * SamplesNum ));
- CV_CALL( CD->flSamples[0] = (float *) cvAlloc( sizeof( float ) * SamplesNum * DP ));
- CV_CALL( CD->flNewSamples[0] = (float *) cvAlloc( sizeof( float ) * SamplesNum * DP ));
-
- /* setting pointers in pointer's arrays */
- for( i = 1; i < SamplesNum; i++ )
- {
- CD->flSamples[i] = CD->flSamples[i - 1] + DP;
- CD->flNewSamples[i] = CD->flNewSamples[i - 1] + DP;
- }
-
- CV_CALL( CD->State = (float *) cvAlloc( sizeof( float ) * DP ));
- CV_CALL( CD->DynamMatr = (float *) cvAlloc( sizeof( float ) * DP * DP ));
- CV_CALL( CD->flConfidence = (float *) cvAlloc( sizeof( float ) * SamplesNum ));
- CV_CALL( CD->flCumulative = (float *) cvAlloc( sizeof( float ) * SamplesNum ));
-
- CV_CALL( CD->RandS = (CvRandState *) cvAlloc( sizeof( CvRandState ) * DP ));
- CV_CALL( CD->Temp = (float *) cvAlloc( sizeof( float ) * DP ));
- CV_CALL( CD->RandomSample = (float *) cvAlloc( sizeof( float ) * DP ));
-
- /* Returning created structure */
- __END__;
-
- return CD;
-}
-
-/*F///////////////////////////////////////////////////////////////////////////////////////
-// Name: cvReleaseConDensation
-// Purpose: Releases CvConDensation structure and frees memory allocated for it
-// Context:
-// Parameters:
-// Kalman - double pointer to CvConDensation structure
-// DP - dimension of the dynamical vector
-// MP - dimension of the measurement vector
-// SamplesNum - number of samples in sample set used in algorithm
-// Returns:
-// Notes:
-//
-//F*/
-CV_IMPL void
-cvReleaseConDensation( CvConDensation ** ConDensation )
-{
- CV_FUNCNAME( "cvReleaseConDensation" );
- __BEGIN__;
-
- CvConDensation *CD = *ConDensation;
-
- if( !ConDensation )
- CV_ERROR( CV_StsNullPtr, "" );
-
- if( !CD )
- EXIT;
-
- /* freeing the memory */
- cvFree( &CD->State );
- cvFree( &CD->DynamMatr);
- cvFree( &CD->flConfidence );
- cvFree( &CD->flCumulative );
- cvFree( &CD->flSamples[0] );
- cvFree( &CD->flNewSamples[0] );
- cvFree( &CD->flSamples );
- cvFree( &CD->flNewSamples );
- cvFree( &CD->Temp );
- cvFree( &CD->RandS );
- cvFree( &CD->RandomSample );
- /* release structure */
- cvFree( ConDensation );
-
- __END__;
-
-}
-
-/*F///////////////////////////////////////////////////////////////////////////////////////
-// Name: cvConDensUpdateByTime
-// Purpose: Performing Time Update routine for ConDensation algorithm
-// Context:
-// Parameters:
-// Kalman - pointer to CvConDensation structure
-// Returns:
-// Notes:
-//
-//F*/
-CV_IMPL void
-cvConDensUpdateByTime( CvConDensation * ConDens )
-{
- int i, j;
- float Sum = 0;
-
- CV_FUNCNAME( "cvConDensUpdateByTime" );
- __BEGIN__;
-
- if( !ConDens )
- CV_ERROR( CV_StsNullPtr, "" );
-
- /* Sets Temp to Zero */
- icvSetZero_32f( ConDens->Temp, ConDens->DP, 1 );
-
- /* Calculating the Mean */
- for( i = 0; i < ConDens->SamplesNum; i++ )
- {
- icvScaleVector_32f( ConDens->flSamples[i], ConDens->State, ConDens->DP,
- ConDens->flConfidence[i] );
- icvAddVector_32f( ConDens->Temp, ConDens->State, ConDens->Temp, ConDens->DP );
- Sum += ConDens->flConfidence[i];
- ConDens->flCumulative[i] = Sum;
- }
-
- /* Taking the new vector from transformation of mean by dynamics matrix */
-
- icvScaleVector_32f( ConDens->Temp, ConDens->Temp, ConDens->DP, 1.f / Sum );
- icvTransformVector_32f( ConDens->DynamMatr, ConDens->Temp, ConDens->State, ConDens->DP,
- ConDens->DP );
- Sum = Sum / ConDens->SamplesNum;
-
- /* Updating the set of random samples */
- for( i = 0; i < ConDens->SamplesNum; i++ )
- {
- j = 0;
- while( (ConDens->flCumulative[j] <= (float) i * Sum)&&(j<ConDens->SamplesNum-1))
- {
- j++;
- }
- icvCopyVector_32f( ConDens->flSamples[j], ConDens->DP, ConDens->flNewSamples[i] );
- }
-
- /* Adding the random-generated vector to every vector in sample set */
- for( i = 0; i < ConDens->SamplesNum; i++ )
- {
- for( j = 0; j < ConDens->DP; j++ )
- {
- cvbRand( ConDens->RandS + j, ConDens->RandomSample + j, 1 );
- }
-
- icvTransformVector_32f( ConDens->DynamMatr, ConDens->flNewSamples[i],
- ConDens->flSamples[i], ConDens->DP, ConDens->DP );
- icvAddVector_32f( ConDens->flSamples[i], ConDens->RandomSample, ConDens->flSamples[i],
- ConDens->DP );
- }
-
- __END__;
-}
-
-/*F///////////////////////////////////////////////////////////////////////////////////////
-// Name: cvConDensInitSamplSet
-// Purpose: Performing Time Update routine for ConDensation algorithm
-// Context:
-// Parameters:
-// conDens - pointer to CvConDensation structure
-// lowerBound - vector of lower bounds used to random update of sample set
-// lowerBound - vector of upper bounds used to random update of sample set
-// Returns:
-// Notes:
-//
-//F*/
-
-CV_IMPL void
-cvConDensInitSampleSet( CvConDensation * conDens, CvMat * lowerBound, CvMat * upperBound )
-{
- int i, j;
- float *LBound;
- float *UBound;
- float Prob = 1.f / conDens->SamplesNum;
-
- CV_FUNCNAME( "cvConDensInitSampleSet" );
- __BEGIN__;
-
- if( !conDens || !lowerBound || !upperBound )
- CV_ERROR( CV_StsNullPtr, "" );
-
- if( CV_MAT_TYPE(lowerBound->type) != CV_32FC1 ||
- !CV_ARE_TYPES_EQ(lowerBound,upperBound) )
- CV_ERROR( CV_StsBadArg, "source has not appropriate format" );
-
- if( (lowerBound->cols != 1) || (upperBound->cols != 1) )
- CV_ERROR( CV_StsBadArg, "source has not appropriate size" );
-
- if( (lowerBound->rows != conDens->DP) || (upperBound->rows != conDens->DP) )
- CV_ERROR( CV_StsBadArg, "source has not appropriate size" );
-
- LBound = lowerBound->data.fl;
- UBound = upperBound->data.fl;
- /* Initializing the structures to create initial Sample set */
- for( i = 0; i < conDens->DP; i++ )
- {
- cvRandInit( &(conDens->RandS[i]),
- LBound[i],
- UBound[i],
- i );
- }
- /* Generating the samples */
- for( j = 0; j < conDens->SamplesNum; j++ )
- {
- for( i = 0; i < conDens->DP; i++ )
- {
- cvbRand( conDens->RandS + i, conDens->flSamples[j] + i, 1 );
- }
- conDens->flConfidence[j] = Prob;
- }
- /* Reinitializes the structures to update samples randomly */
- for( i = 0; i < conDens->DP; i++ )
- {
- cvRandInit( &(conDens->RandS[i]),
- (LBound[i] - UBound[i]) / 5,
- (UBound[i] - LBound[i]) / 5,
- i);
- }
-
- __END__;
-}
projects / opencv / blobdiff