OpenCV粒子濾波代碼怎么寫

這期內(nèi)容當(dāng)中小編將會(huì)給大家?guī)?lái)有關(guān)OpenCV粒子濾波代碼怎么寫，文章內(nèi)容豐富且以專業(yè)的角度為大家分析和敘述，閱讀完這篇文章希望大家可以有所收獲。

OpenCV中實(shí)現(xiàn)了粒子濾波的代碼，位置在c:\program
files\opencv\cv\src\cvcondens.cpp文件，通過(guò)分析這個(gè)文件，可以知道庫(kù)函數(shù)中如何實(shí)現(xiàn)粒子濾波過(guò)程的。

首先是從手冊(cè)上拷貝的粒子濾波跟蹤器的數(shù)據(jù)結(jié)構(gòu)：

typedef struct CvConDensation
{
int MP; // 測(cè)量向量的維數(shù)： Dimension of measurement vector
int DP; // 狀態(tài)向量的維數(shù)： Dimension of state vector
float* DynamMatr; // 線性動(dòng)態(tài)系統(tǒng)矩陣：Matrix of the linear Dynamics system
float* State; // 狀態(tài)向量： Vector of State
int SamplesNum; // 粒子數(shù)： Number of the Samples
float** flSamples; // 粒子向量數(shù)組： array of the Sample Vectors
float** flNewSamples; // 粒子向量臨時(shí)數(shù)組： temporary array of the Sample Vectors
float* flConfidence; // 每個(gè)粒子的置信度(譯者注：也就是粒子的權(quán)值)：Confidence
for each Sample
float* flCumulative; // 權(quán)值的累計(jì)： Cumulative confidence
float* Temp; // 臨時(shí)向量：Temporary vector
float* RandomSample; // 用來(lái)更新粒子集的隨機(jī)向量： RandomVector to update sample set
CvRandState* RandS; // 產(chǎn)生隨機(jī)向量的結(jié)構(gòu)數(shù)組： Array of structures to generate random vectors
} CvConDensation;

與粒子濾波相關(guān)的幾個(gè)函數(shù)：

cvCreateConDensation：用于構(gòu)造上述濾波器數(shù)據(jù)結(jié)構(gòu)

cvReleaseConDensation：釋放濾波器

cvConDensInitSampleSet：初始化粒子集

cvConDensUpdateByTime：更新粒子集

下面著重對(duì)這幾個(gè)函數(shù)進(jìn)行分析。

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;

}

輸入?yún)?shù)分別是系統(tǒng)狀態(tài)向量維數(shù)、測(cè)量向量維數(shù)以及粒子個(gè)數(shù)，然后根據(jù)這些參數(shù)為濾波器相應(yīng)結(jié)構(gòu)分配空間。

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

}

釋放濾波器占用的空間，沒(méi)什么好說(shuō)的

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

這里根據(jù)輸入的動(dòng)態(tài)范圍給每個(gè)系統(tǒng)狀態(tài)分配一個(gè)產(chǎn)生隨即數(shù)的結(jié)構(gòu)

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

   {

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

                   LBound[i],

                   UBound[i],

                   i );

   }

/* Generating the samples */

根據(jù)產(chǎn)生的隨即數(shù)，為每個(gè)粒子的每個(gè)系統(tǒng)狀態(tài)分配初始值，并將每個(gè)粒子的置信度設(shè)置為相同的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 */

產(chǎn)生以后更新粒子系統(tǒng)狀態(tài)的隨即結(jié)構(gòu)，采樣范圍為原來(lái)初始范圍的-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是內(nèi)聯(lián)函數(shù)，表示flConfidence乘flSamples，放到State里，即求系統(tǒng)狀態(tài)的過(guò)程，系統(tǒng)狀態(tài)保存在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 */

重采樣，將新采樣出來(lái)的粒子保存在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++
)

{

為每個(gè)新粒子產(chǎn)生一個(gè)隨即量

       for(j
= 0;j <ConDens->DP;j++
)

       {

           cvbRand(ConDens->RandS
+j,ConDens->RandomSample
+j, 1 );

       }

將flNewSamples加一個(gè)隨即量，保存入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__;

}

更新中用到的函數(shù)如下：

（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] );
}

運(yùn)行完上述函數(shù)之后，經(jīng)重采樣后的粒子保存在flSamples中，同時(shí)上次迭代的加權(quán)系統(tǒng)狀態(tài)保存在State中

上述就是小編為大家分享的OpenCV粒子濾波代碼怎么寫了，如果剛好有類似的疑惑，不妨參照上述分析進(jìn)行理解。如果想知道更多相關(guān)知識(shí)，歡迎關(guān)注億速云行業(yè)資訊頻道。