Shulou(Shulou.com)06/03 Report--

In this issue, the editor will bring you about how to write the OpenCV particle filter code. The article is rich in content and analyzes and describes for you from a professional point of view. I hope you can get something after reading this article.

OpenCV in the implementation of the particle filter code, the location in c:\ program

Files\ opencv\ cv\ src\ cvcondens.cpp file, by analyzing this file, you can know how to implement the particle filter process in the library function.

The first is the data structure of the particle filter tracker copied from the manual:

Typedef struct CvConDensation

{

Int MP; / / Dimensions of measurement vectors: Dimension of measurement vector

Dimension of int DP; / / state vector: Dimension of state vector

Float* DynamMatr; / / Linear dynamic system Matrix: Matrix of the linear Dynamics system

Float* State; / / status vector: Vector of State

Int SamplesNum; / / number of particles: Number of the Samples

Float** flSamples; / / Particle Vector Array: array of the Sample Vectors

Float** flNewSamples; / / temporary array of particle vectors: temporary array of the Sample Vectors

Float* flConfidence; / / confidence of each particle: Confidence

For each Sample

Accumulation of float* flCumulative; / / weights: Cumulative confidence

Float* Temp; / / temporary vector: Temporary vector

Float* RandomSample; / / Random Vector used to update the particle set: RandomVector to update sample set

CvRandState* RandS; / / A structural array that generates random vectors: Array of structures to generate random vectors

} CvConDensation

Several functions related to particle filter:

CvCreateConDensation: used to construct the above filter data structure

CvReleaseConDensation: release filter

CvConDensInitSampleSet: initializing a subset of particles

CvConDensUpdateByTime: updating particle set

The following focuses on the analysis of these functions.

CV_IMPLCvConDensation*cvCreateConDensation (intDP,intMP,intSamplesNum

)

{

Inti

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 Ting I 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__

ReturnCD

}

The input parameters are the system state vector dimension, the measurement vector dimension and the number of particles, and then allocate space for the corresponding structure of the filter according to these parameters.

CV_IMPLvoid

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__

}

There is nothing to say about releasing the space occupied by the filter.

CV_IMPLvoid

CvConDensInitSampleSet (CvConDensation

* conDens,CvMat * lowerBound,CvMat

* upperBound)

{

Inti,j

Float * LBound

Float * UBound

FloatProb

= 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 * /

Here, each system state is assigned a structure that generates a random number according to the input dynamic range.

For (I)

= 0Shi I DP;i++

)

{

CvRandInit (& (conDens- > RandS [I])

LBound [i]

UBound [i]

I)

}

/ * Generating the samples * /

According to the random number generated, the initial value is assigned to each system state of each particle, and the confidence level of each particle is set to the same 1BO.

For (j)

= 0sj SamplesNum;j++

)

{

For (I)

= 0Shi I DP;i++

)

{

CvbRand (conDens- > RandS)

+ iMagneol conDens-> flSamples [j]

+ I, 1)

}

ConDens- > flConfidence [j]

= Prob

}

/ * Reinitializes the structures to update samples randomly * /

A random structure is generated to update the state of the particle system later, and the sampling range is from-1 beat 5 to 1 hand 5 of the original initial range.

For (I)

= 0Shi I DP;i++

)

{

CvRandInit (& (conDens- > RandS [I])

(LBound [I]

-UBound [I]) / 5

(UBound [I]

-LBound [I]) / 5

I)

}

_ _ END__

}

CV_IMPLvoid

CvConDensUpdateByTime (CvConDensation

* ConDens)

{

Inti,j

FloatSum

= 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 * /

IcvScaleVector_32f is an inline function that represents flConfidence multiplied by flSamples and put into State, that is, the process of finding the system state, which is saved in temp.

For (I)

= 0Shi I 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 * /

Resample to save the newly sampled particles in flNewSamples

For (I)

= 0Shi I SamplesNum;i++

)

{

J = 0

While (ConDens- > flCumulative [j]

FlSamples [j], ConDens- > DP,ConDens- > flNewSamples [I]

);

}

/ * Adding the random-generated vector to every vector in sample set * /

For (I)

= 0Shi I SamplesNum;i++

)

{

Generate a random amount for each new particle

For (j)

= 0sj DP;j++

)

{

CvbRand (ConDens- > RandS)

+ jrecom Condens-> RandomSample

+ j, 1)

}

Add a random amount of flNewSamples and save it in flSamples

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__

}

The functions used in the update are as follows:

(1)

CV_INLINE void icvScaleVector_32f (const float* src, float* dst

Int len, double scale)

{

Int i

For (I = 0; I)

< len; i++ ) dst[i] = (float)(src[i]*scale); icvCheckVector_32f( dst, len ); } （2） #define icvAddMatrix_32f( src1, src2, dst, w, h ) \ icvAddVector_32f( (src1), (src2), (dst), (w)*(h)) CV_INLINE void icvAddVector_32f( const float* src1, const float* src2, float* dst, int len ) { int i; for( i = 0; i < len; i++ ) dst[i] = src1[i] + src2[i]; icvCheckVector_32f( dst, len ); } （3） #define icvTransformVector_32f( matr, src, dst, w, h ) \ icvMulMatrix_32f( matr, w, h, src, 1, w, dst ) CV_INLINE void icvMulMatrix_32f( const float* src1, int w1, int h2, const float* src2, int w2, int h3, float* dst ) { int i, j, k; if( w1 != h3 ) { assert(0); return; } for( i = 0; i < h2; i++, src1 += w1, dst += w2 ) for( j = 0; j < w2; j++ ) { double s = 0; for( k = 0; k < w1; k++ ) s += src1[k]*src2[j + k*w2]; dst[j] = (float)s; } icvCheckVector_32f( dst, h2*w2 ); } （4） #define icvCopyVector_32f( src, len, dst ) memcpy((dst),(src),(len)*sizeof(float)) （5） CV_INLINE void cvbRand( CvRandState* state, float* dst, int len ) { CvMat mat = cvMat( 1, len, CV_32F, (void*)dst ); cvRand( state, &mat ); } CV_INLINE void cvRand( CvRandState* state, CvArr* arr ) { if( !state ) { cvError( CV_StsNullPtr, "cvRand", "Null pointer to RNG state", "cvcompat.h", 0 ); return; } cvRandArr( &state->

State, arr, state- > disttype, state- > param [0], state- > param [1])

}

After running the above function, the resampled particles are saved in flSamples, while the weighted system state of the last iteration is saved in State.

This is how the OpenCV particle filter code shared by the editor is written. If you happen to have similar doubts, you might as well refer to the above analysis to understand. If you want to know more about it, you are welcome to follow the industry information channel.

Welcome to subscribe "Shulou Technology Information " to get latest news, interesting things and hot topics in the IT industry, and controls the hottest and latest Internet news, technology news and IT industry trends.