jetstream.cpp

关于时下流行的粒子滤波程序的源码

		cvSetData(sampleMat, *m_flSamples , m_nStDim * sizeof(float));
		char name[50];
		sprintf(name, "samples-%d.dat", nStep);
		WriteMat32F(name, sampleMat);
		cvReleaseMatHeader(&sampleMat);

#endif

	m_nCurStep++;
	//Set current sample
	//SetVec(*m_flSamples, *x_ip1, m_nStDim * m_nSamplesNum);
}
///////////////////////////////////////////////////////////////////
//
// Selection()
//
//Purpose:
//	Sample index a(m)(m = 1, ..., M) from m_flConfidence[k]	over
//	{1,...,M}, and Set x_0:i+1(m) = (x_0:i_(a(m)), x_i+1(a(m))
//
///////////////////////////////////////////////////////////////////
void CJetStream::Selection(int nStep)
{
	//Selection performed at each step
//	if ((nStep - m_nCurStep) != 1)
	if (nStep != m_nCurStep)

		return;
	int *index = new int[m_nSamplesNum];
	//backup
	for(int i = 0; i <= nStep - 1; i++)
	{
		memcpy(*(*(m_tmpStream + i)), *(*(m_flStream + i)), m_nStDim * m_nSamplesNum * sizeof(float));
	}
	float **tmp_i, **stream_i;

	//Sample a(m)
	Resample(index);
	//a(m)
	int index_a;
	//re-set
	for (int m = 0; m < m_nSamplesNum; m ++)
	{
		//a(m)
		index_a = index[m];
		//Update x_0:i
		for (i = 0; i <= nStep - 1; i++)
		{
			tmp_i = *(m_tmpStream + i);
			stream_i = *(m_flStream + i);
			memcpy(*(stream_i + m), *(tmp_i + index_a), m_nStDim * sizeof(float));
			//SetVec(*(stream_i + m), *(tmp_i + index_a), m_nStDim);
		}
		//Update x_i+1
		stream_i = *(m_flStream + nStep);
		memcpy(*(stream_i + m), *(m_flSamples + index_a), m_nStDim * sizeof(float));
	}
	//Update current samples and confidences
#ifdef _WRITE_SAMPLE
		CvMat *sampleMat = cvCreateMatHeader(m_nSamplesNum, m_nStDim, CV_32FC1);
		cvSetData(sampleMat, *(*(m_flStream + nStep )) , m_nStDim * sizeof(float));
		char name[50];
		sprintf(name, "Updated-%d.dat", nStep);
		WriteMat32F(name, sampleMat);
		cvReleaseMatHeader(&sampleMat);

#endif
	memcpy(*(m_flSamples), *(*(m_flStream + nStep )), 
		m_nStDim * m_nSamplesNum * sizeof(float));
	SetVec(m_flConfidence, (float)(1.0 / m_nSamplesNum), m_nSamplesNum);
	CalCumulative();
	delete [] index;
//Write the samples projected to X-axis until now
//The ith row of smapleXMat is the X-Sample of the ith Step
#ifdef _WRITE_X_SAMPLE
	CvMat *sampleXMat = cvCreateMat(nStep + 1, m_nSamplesNum, CV_32FC1);
	float *data = NULL;
	float *srcSample = NULL;
	for (i = 0; i <= nStep; i++)
	{
		data = (float *)cvGetPtrAt(sampleXMat, i);
		srcSample = *(*(m_flStream + i));
		for (m = 0; m < m_nSamplesNum; m++, srcSample += m_nStDim)
		{
			*data++ = *srcSample;
		}
	}
	char Xname[50];
	//sprintf(Xname, "Samples-X-%d-%S.dat", nStep, GetTime());
	sprintf(Xname, "Samples-X-%d.dat", nStep, GetTime());
	WriteMat32F(Xname, sampleXMat);
	cvReleaseMat(&sampleXMat);
#endif

	//An iteration complete
	//m_nCurStep++;

}
/////////////////////////////////////////////////////////////////////////////////////
//
//	EstState()
//
//Purpose:
//	Estimate the states as the mean of the particles
//
/////////////////////////////////////////////////////////////////////////////////////
void CJetStream::EstState(int nStep)
{
	//Here Estimation performed after the selection/resampling
	if (nStep > m_nCurStep)
		return;
	for (int i = 0; i <= nStep; i++)
	{
		MeanSample(*(m_flStateSeq + i), *(m_flStream + i));
	}
	memcpy(m_flState, *(m_flStateSeq + nStep), m_nStDim * sizeof(float));
}
void * CJetStream::GetState(int nStep) const
{
	//Return current estmated states
	if (nStep > m_nCurStep)
		return 0;
	CvMat *state = cvCreateMat(nStep + 1, m_nStDim, CV_32FC1);
	float *data = (float *)cvGetPtrAt(state, 0, 0);
	memcpy(data, *m_flStateSeq, (nStep + 1) * m_nStDim * sizeof(float));
	return (void*)state;
}




////////////////////////////////////////////////////////////////////////////////////\
//																					|
//																					|
//	IMPLEMENTATION for CJetImgData													|
//																					|
//																					|
/////////////////////////////////////////////////////////////////////////////////////
CJetImgData::CJetImgData()
{
	corner_mask = NULL;
	gradient_norm = NULL;
	gradient_orient = NULL;
}
CJetImgData::~CJetImgData()
{
	if (corner_mask != NULL)	
		cvReleaseMat((CvMat**)&corner_mask);
	if (gradient_norm != NULL)
		cvReleaseMat((CvMat**)&gradient_norm);
	if (gradient_orient != NULL)
		cvReleaseMat((CvMat**)&gradient_orient);
}
CJetImgData::CopyOf(const CJetImgData *jetImgData)
{
	if (jetImgData->gradient_norm != NULL)
		gradient_norm = (void *)cvCloneMat((CvMat*)jetImgData->gradient_norm);
	if (jetImgData->gradient_orient != NULL)
		gradient_orient = (void *)cvCloneMat((CvMat*)jetImgData->gradient_orient);
	if (jetImgData->corner_mask != NULL)
		corner_mask = (void *)cvCloneMat((CvMat*)jetImgData->corner_mask);
}

//////////////////////////////////////////////////////////////////////////////////////
//
//	SetMagOr()
//Purpose:
//	Compute the magnitude and orientation of the gradients
//Paras:
//	dX	---------------- derivative image along x
//	dY	---------------- derivative image along y
/////////////////////////////////////////////////////////////////////////////////////
void CJetImgData::SetMagOr(void * dX, void * dY)
{
	//Access to Original Image
	if (((IplImage*)dX)->depth != IPL_DEPTH_16S || 
		((IplImage*)dY)->depth != IPL_DEPTH_16S)
		return ;
	short * _dX = (short *)(((IplImage *)dX)->imageData);
	short * _dY = (short *)(((IplImage *)dY)->imageData);
	int width = ((IplImage *)dX)->width;
	int height = ((IplImage *)dX)->height;
	//Access to allocated norm and or
	if (gradient_norm != NULL)
		cvReleaseMat( (CvMat**)&gradient_norm );
	gradient_norm = cvCreateMat(height, width, CV_32FC1);
	if (gradient_orient != NULL)
		cvReleaseMat( (CvMat**)&gradient_orient );
	gradient_orient = cvCreateMat(height, width, CV_32FC1);
	int step;
	CvSize size;
	float *_norm, *_or;
	cvGetRawData(gradient_norm, (unsigned char**)&_norm, &step, &size);
	cvGetRawData(gradient_orient , (unsigned char**)&_or, &step, &size);
	short dx, dy;
	for (int i = 0; i < width * height; i++)
	{
		dx = *(_dX + i);
		dy = *(_dY + i);
		//Cal magnitude
		//can be replaced by abs(dx) + abs(dy)
		*(_norm+i) = (float)sqrt(dx * dx + dy * dy);
		//with hadeling dx is close to zero
		*(_or+i)  = (float) atan_u( (double)dx, (double)dy );
	}
	//gradient_norm = (void *) norm;
	//gradient_orient = (void *)or;
#ifdef _DEBUG
	WriteMat32F("GradNorm.dat", gradient_norm);
	WriteMat32F("GradOr.dat", gradient_orient);
#endif
}

///////////////////////////////////////////////////////////////////////////////////////
//	SetCornerMask()
//Purpose:
//	Set corner pixels as '255' 
//Paras:
//	corners	---------------- Detected corners stored in CvPoint2D32F structure
//	corner_count------------ count of the detected corners
//
///////////////////////////////////////////////////////////////////////////////////////
void CJetImgData::SetCornerMask(void *corners, int corner_count, void * szImg)
{
	CvPoint2D32f *_corners = (CvPoint2D32f *)corners;
	CvSize *_szImg = (CvSize *)szImg;
	CvMat *mask = cvCreateMat(_szImg->height, _szImg->width, CV_8UC1);
	cvSetZero(mask);

//	int step;
//	CvSize size;
//	unsigned char * pMask;
//	cvGetRawData(or , (unsigned char**)&_or, &step, &size);
	int x, y;
	int xl, yl;
	int width = _szImg->width;
	int height = _szImg->height;
	for (int i = 0; i < corner_count; i++)
	{
		x = (int)_corners[i].x;
		y = (int)_corners[i].y;
		for (xl = MAX(0,x - mask_bound); xl < MIN(width, x + mask_bound); xl++)
			for (yl = MAX(0,y - mask_bound); yl < MIN(height, y + mask_bound); yl++)
				*(cvGetPtrAt(mask, yl, xl)) = (unsigned char)mask_value;
	//	*(cvGetPtrAt(mask, y, x)) = (unsigned char)mask_value;
	
	}
	if (corner_mask != NULL)
		cvReleaseMat( (CvMat**)corner_mask );
	corner_mask = (void *)mask;
#ifdef _DEBUG
	WriteMat("corners.dat", corner_mask);
#endif

}
//////////////////////////////////////////////////////////////////////////////
//	NormalizeNorm()
//
//Purpose:
//	Normalize norm of the gradient such that the norms fall between 0 and 1
/////////////////////////////////////////////////////////////////////////////
float CJetImgData::NormalizeNorm()
{
	assert(gradient_norm != NULL);
	CvSize size;
	float *_norm;
	int step;
	cvGetRawData(gradient_norm, (unsigned char**)&_norm, &step, &size);
	//Find the maximum
	float max = 0.0;
	for (int i = 0; i < size.width * size.height; i++)
	{
		if (*(_norm + i) > max) max = *(_norm + i);
	}
	//the maximum is not zero
	if (max > 1.0e-3)
	{
		for(i = 0; i < size.width * size.height; i++)
			*(_norm + i) /= max;
	}
	return max;
}


////////////////////////////////////////////////////////////////////////////////////\
//																					|
//																					|
//	IMPLEMENTATION for CJetInit														|
//																					|
//																					|
/////////////////////////////////////////////////////////////////////////////////////
CJetInit::CJetInit(int nPtNum)
{
	assert(nPtNum <= max_order);
	//if (nPtNum <= max_order)
	//	return;
	ptNum = nPtNum;
	for (int i = 0; i < nPtNum; i++)
	{
		pt[i] = (float*) new CvPoint2D32f;
	}
}
CJetInit::~CJetInit()
{
	for (int i = 0; i < ptNum; i++)
		delete pt[i];
}
void CJetInit::SetPoints(long *point, int i)
{
	assert(i <= ptNum);
	CvPoint2D32f * curPt = (CvPoint2D32f *)pt[i];
	curPt->x = (float)*point;
	curPt->y = (float)*(point+1);
}