/*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 */
//重采样，将新采样出来的粒子保存在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__;
}
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 */
//根据产生的随即数，为每个粒子的每个系统状态分配初始值，并将每个粒子的置信度设置为相同的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_IMPL void
cvConDensInitSampleSet( CvConDensation * conDens, CvMat * lowerBound, CvMat * upperBound )
{
int i, j;
float *LBound;
float *UBound;
float Prob = 1.f / conDens->SamplesNum;
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);
}
}
static int aMatchContourTrees(void)
{
CvSeqBlock contour_blk1, contour_blk2;
CvContour contour_h1, contour_h2;
CvContourTree *tree1, *tree2;
CvMemStorage *storage; /* storage for contour and tree writing */
int block_size = 10000;
CvRandState state;
double lower, upper;
int seed;
float fr;
int type_seq;
int method;
int nPoints1 = 12, nPoints2 = 12;
int xc,yc,a1 = 10, b1 = 20, a2 = 10, b2 =20, fi = 0;
int xmin,ymin,xmax,ymax;
double error_test,rezult, eps_rez = 0.8;
double pi = 3.1415926;
double threshold = 1.e-7;
double threshold2 = 5.;
int i;
int code = TRS_OK;
int width=256,height=256;
CvPoint *cp1,*cp2;
/* read tests params */
if (!trsiRead(&nPoints1,"20","Number of points first contour"))
return TRS_UNDEF;
if (!trsiRead(&nPoints2,"20","Number of points second contour"))
return TRS_UNDEF;
if(nPoints1>0&&nPoints2>0)
{
if (!trsiRead(&a1,"10","first radius of the first elipse"))
return TRS_UNDEF;
if (!trsiRead(&b1,"20","second radius of the first elipse"))
return TRS_UNDEF;
if (!trsiRead(&a2,"15","first radius of the second elipse"))
return TRS_UNDEF;
if (!trsiRead(&b2,"30","second radius of the second elipse"))
return TRS_UNDEF;
if (!trsiRead(&fi,"0","second radius of the second elipse"))
return TRS_UNDEF;
if (!trsdRead(&upper,"3","noise amplidude"))
return TRS_UNDEF;
xc = (int)(width/2.);
yc = (int)(height/2.);
xmin = width;
ymin = height;
xmax = 0;
ymax = 0;
cp1 = (CvPoint*) trsmAlloc(nPoints1*sizeof(CvPoint));
cp2 = (CvPoint*) trsmAlloc(nPoints2*sizeof(CvPoint));
for(i=0; i<nPoints1; i++)
{
cp1[i].x = (int)(a1*cos(2*pi*i/nPoints1))+xc;
cp1[i].y = (int)(b1*sin(2*pi*i/nPoints1))+yc;
if(xmin> cp1[i].x) xmin = cp1[i].x;
if(xmax< cp1[i].x) xmax = cp1[i].x;
if(ymin> cp1[i].y) ymin = cp1[i].y;
if(ymax< cp1[i].y) ymax = cp1[i].y;
}
if(xmax>width||xmin<0||ymax>height||ymin<0) return TRS_FAIL;
lower = -upper;
/* upper = 3;*/
seed = 345753;
cvRandInit(&state, (float)lower,(float)upper, seed );
for(i=0; i<nPoints2; i++)
{
cvbRand( &state, &fr, 1 );
cp2[i].x =(int)fr+(int)(a2*cos(2*pi*i/nPoints2)*cos(2*pi*fi/360.))-
(int)(b2*sin(2*pi*i/nPoints2)*sin(2*pi*fi/360.))+xc;
cvbRand( &state, &fr, 1 );
cp2[i].y =(int)fr+(int)(a2*cos(2*pi*i/nPoints2)*sin(2*pi*fi/360.))+
(int)(b2*sin(2*pi*i/nPoints2)*cos(2*pi*fi/360.))+yc;
if(xmin> cp2[i].x) xmin = cp2[i].x;
if(xmax< cp2[i].x) xmax = cp2[i].x;
if(ymin> cp2[i].y) ymin = cp2[i].y;
if(ymax< cp2[i].y) ymax = cp2[i].y;
}
if(xmax>width||xmin<0||ymax>height||ymin<0) return TRS_FAIL;
/* contours initialazing */
type_seq = CV_SEQ_POLYGON;
cvMakeSeqHeaderForArray( type_seq, sizeof(CvContour), sizeof(CvPoint),
(char*)cp1, nPoints1, (CvSeq*)&contour_h1, &contour_blk1);
cvMakeSeqHeaderForArray( type_seq, sizeof(CvContour), sizeof(CvPoint),
(char*)cp2, nPoints2, (CvSeq*)&contour_h2, &contour_blk2);
/* contour trees created*/
storage = cvCreateMemStorage( block_size );
tree1 = cvCreateContourTree ((CvSeq*)&contour_h1, storage, threshold);
tree2 = cvCreateContourTree ((CvSeq*)&contour_h2, storage, threshold);
/* countours matchig */
error_test = 0.;
method = 1;
rezult = cvMatchContourTrees (tree1, tree2, (CvContourTreesMatchMethod)method,threshold2);
error_test+=rezult;
if(error_test > eps_rez ) code = TRS_FAIL;
else code = TRS_OK;
trsWrite( ATS_CON | ATS_LST | ATS_SUM, "contours matching error_test =%f \n",
error_test);
cvReleaseMemStorage ( &storage );
trsFree (cp2);
trsFree (cp1);
}
/* _getch(); */
return code;
}
