OpenCV粒子滤波代码学习

OpenCV中实现了粒子滤波的代码，位置在c:\program files\opencv\cv\src\cvcondens.cpp文件，通过分析这个文件，可以知道库函数中如何实现粒子滤波过程的。

首先是从手册上拷贝的粒子滤波跟踪器的数据结构：

typedef struct CvConDensation
{
int MP; // 测量向量的维数： Dimension of measurement vector
int DP; // 状态向量的维数： Dimension of state vector
float* DynamMatr; // 线性动态系统矩阵：Matrix of the linear Dynamics system
float* State; // 状态向量： Vector of State
int SamplesNum; // 粒子数： Number of the Samples
float** flSamples; // 粒子向量数组： array of the Sample Vectors
float** flNewSamples; // 粒子向量临时数组： temporary array of the Sample Vectors
float* flConfidence; // 每个粒子的置信度(译者注：也就是粒子的权值)：Confidence for each Sample
float* flCumulative; // 权值的累计： Cumulative confidence
float* Temp; // 临时向量：Temporary vector
float* RandomSample; // 用来更新粒子集的随机向量： RandomVector to update sample set
CvRandState* RandS; // 产生随机向量的结构数组： Array of structures to generate random vectors
} CvConDensation;

与粒子滤波相关的几个函数：

cvCreateConDensation：用于构造上述滤波器数据结构

cvReleaseConDensation：释放滤波器

cvConDensInitSampleSet：初始化粒子集

cvConDensUpdateByTime：更新粒子集

下面着重对这几个函数进行分析。

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

 

   returnCD;

}

 

输入参数分别是系统状态向量维数、测量向量维数以及粒子个数，然后根据这些参数为滤波器相应结构分配空间。

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

 

}

释放滤波器占用的空间，没什么好说的

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

这里根据输入的动态范围给每个系统状态分配一个产生随即数的结构

   for(i = 0;i <conDens->DP;i++ )

   {

       cvRandInit( &(conDens->RandS[i]),

                   LBound[i],

                   UBound[i],

                   i );

   }

/* Generating the samples */

根据产生的随即数，为每个粒子的每个系统状态分配初始值，并将每个粒子的置信度设置为相同的1/n

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

产生以后更新粒子系统状态的随即结构，采样范围为原来初始范围的-1/5到1/5

   for(i = 0;i <conDens->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是内联函数，表示flConfidence乘flSamples，放到State里，即求系统状态的过程，系统状态保存在temp中

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

重采样，将新采样出来的粒子保存在flNewSamples中

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

       }

将flNewSamples加一个随即量，保存入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__;

}

更新中用到的函数如下：

（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 h1,
                                 const float* src2, int w2, int h2,
                                 float* dst )
{
    int i, j, k;


    if( w1 != h2 )
    {
        assert(0);
        return;
    }


    for( i = 0; i < h1; 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, h1*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] );
}

运行完上述函数之后，经重采样后的粒子保存在flSamples中，同时上次迭代的加权系统状态保存在State中