probcontour.cpp

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

//////////////////////////////////////////////////////////////////////////////////////
//
//	Implementation file  of Probabilistic Contour Extraction Approach
//
//	XinFan 2003.5.26
//
//Reference:
//	[1]	P. Pérez, A. Blake, and M. Gangnet. 
//		JetStream: Probabilistic contour extraction with particles. 
//		Proc. Int. Conf. on Computer Vision (ICCV),  II:524-531, 2001.
//////////////////////////////////////////////////////////////////////////////////////
#include <memory.h>
#include "ProbContour.h"
#include "cv.h"
#include "FileIO.h"



#include "utility.h"

#define _WRITE_RESULT

/////////////////////////////////////////////////////////////////////////////////////
//
//	Implementation Code for JetStream
//
//////////////////////////////////////////////////////////////////////////////////////

//
//An Convenient Routine for Set Jetstream Parameters
//	Currently, the parameters are set as Ref.[1]
//
void cvrJetInitPara(JETPARA *jetPara, const CJetImgData *jetData)
{
	
	jetPara->dyn_mu			= (float)0.05;
	jetPara->dyn_sigma		= (float)0.1;
	//TODO:
	//	Replace as the Mean of Gradient Norm
	//	XinFan 2003.5.26
	//COMPLETED: XinFan 2003.5.27
	if (jetData == 0) //Ad hoc specify
	{
		jetPara->mea_lamda		= 1.0;
	}
	else//Mean of image gradients
	{
		//(CvMat *)imgNorm = (CvMat *)jetData->GetNorm();
		CvScalar mean; 
		mean = cvAvg(jetData->GetNorm());
		jetPara->mea_lamda		= (float)(mean.val[0]);
	}
	jetPara->mea_sigma		= 1.0;
	jetPara->step_length	= 1.0;
}
//
//Compute gradient norm and angle 
//
void cvrJetInitData(const void * src, CJetImgData *jetData)
{
	const int max_cornercount	= 30;
	const double quanlity_level = 0.01;
	const double min_distance	= 10;	
	IplImage *pSrc = (IplImage *)src;
	//Construct derivative 
	CvSize szImg;
	szImg.width = pSrc->width;
	szImg.height = pSrc->height;
	////////////////////////////////////////////////////////////////////////\
	//																		|
	//	Image Gradients														|
	//																		|
	/////////////////////////////////////////////////////////////////////////

	IplImage* dX = cvCreateImage(szImg, IPL_DEPTH_16S, pSrc->nChannels);
	IplImage* dY = cvCreateImage(szImg, IPL_DEPTH_16S, pSrc->nChannels);

    memset( dX->imageData, 0, dX->imageSize );
    memset( dY->imageData, 0, dY->imageSize );
    
	//smooth and difference
	cvSobel(pSrc, dX, 1, 0, 3);
	cvSobel(pSrc, dY, 0, 1, 3);
	//Calculate the norm & or
	jetData->SetMagOr(dX, dY);

	cvReleaseImage(&dX);
	cvReleaseImage(&dY);
	////////////////////////////////////////////////////////////////////////\
	//																		|
	//	Corner Detection													|
	//																		|
	/////////////////////////////////////////////////////////////////////////
	int cornercount = max_cornercount;
	CvPoint2D32f *corners = new CvPoint2D32f[cornercount];
	//CvPoint *corners = new CvPoint[cornercount];
	IplImage *eigImage = cvCreateImage(szImg, IPL_DEPTH_32F, 1);
	IplImage *tmpImage = cvCreateImage(szImg, IPL_DEPTH_32F, 1);
//	CvMat *cornerMask = cvCreateMat(szImg.height, szImg.width, CV_8U);
	//detecting strong corners
	cvGoodFeaturesToTrack(pSrc, eigImage, tmpImage, corners, &cornercount, 
		quanlity_level, min_distance);
	jetData->SetCornerMask(corners, cornercount, &szImg);
	cvReleaseImage(&eigImage);
	cvReleaseImage(&tmpImage);
	delete [] corners;
}
//
//A Generic Particle Predict-Update iteration
//
void cvrParticleIteration(CParticle *cvrParticle, int nStep)
{
	//Predict...
	cvrParticle->UpdateByTime(nStep);
	//Update weights
	cvrParticle->EvalWeight(nStep);
	//if resampling is performed at each step
	//cvrParticle->Resample(nStep);
	//if Estimation performed
	//cvrParticle->EstState();
}
//
//Stream Iteration process
//
void cvrStreamIter(CJetStream *cvrStream, int nStartStep, int nEndStep)
{
	for (int i = nStartStep; i < nEndStep; i ++)
	{
		cvrParticleIteration(cvrStream, i);
		//Act as resampling, but all the previous states will be updated
		//Need to discuss more...
		cvrStream->Selection(i);
		cvrStream->EstState(i);

	}

}
///////////////////////////////////////////////////////////////////////////////////////
//
//
// Drawing routines
//
//
///////////////////////////////////////////////////////////////////////////////////////

//
//Draw estmitated contour to image
//
void cvrProbContour(void *srcImg, const CParticle *cvParticle, int nStep)
{
	const contour_color = 255;
	//source img
	//IplImage *img = (IplImage *)srcImg;
	//contour with
	//	control_point_count rows
	//	StateDim			cols(here 2)
	CvMat *contour = 0;
	contour = (CvMat*) cvParticle->GetState(nStep);
	if (contour->cols != 2)
		return;
	//
	//Draw contour
	//
	CvPoint2D32f * ctrlPt;
	CvPoint ptStart, ptEnd;
/***************************************************\
	//For testing line_pt
	int *line_x = NULL;
	int *line_y = NULL;
	int line_num = 0;
	unsigned char line_color = 255;
\****************************************************/
	for (int i = 0; i < contour->rows - 1; i ++)
	{
//		assert(x < m_nDataWidth);
//		assert(y < m_nDataHeight);

		ctrlPt = (CvPoint2D32f *)(cvGetPtrAt(contour, i));
		//I think there must be some simple way to convert 
		//CvPoint2D32f to CvPoint
		//XinFan 2003.5.28
		ptStart.x = (int)ctrlPt->x; ptStart.y = (int)ctrlPt->y;
		DrawCross(srcImg, (int *)&ptStart, 3);
		ctrlPt = (CvPoint2D32f *)(cvGetPtrAt(contour, i + 1));
		ptEnd.x = (int)ctrlPt->x; ptEnd.y = (int)ctrlPt->y;
		DrawCross(srcImg, (int *)&ptEnd, 3);
/**********************************************************************************\
	//For testing line_pt
		line_pt(ptStart.x, ptStart.y, ptEnd.x, ptEnd.y, line_x, line_y, line_num);
		for (int j = 0; j < line_num; j++)
			iplPutPixel((IplImage *)srcImg, line_x[j], line_y[j], &line_color);
		delete line_x; line_x = NULL;
		delete line_y; line_y = NULL;
		line_num = 0;
\*********************************************************************************/
		cvLine(srcImg, ptStart, ptEnd, contour_color);
	}
	
	cvReleaseMat(&contour);
}
//
//Draw Face Contour
//
void cvrDrawFaceContour(void *srcImg, 
					 const CParticle *cvParticle, 
					 int nStep, 
					 const std::vector<CAAMShape::CAAMPointInfo> &ptInfo,
					 const CString &szPathName)
{
	const contour_color = 255;

	CvMat *contour = 0;
	contour = (CvMat*) cvParticle->GetState(nStep);
	//Set Filename of the Result data, which can be read by MATLAB function
	// -- ReadMat
	if (!szPathName.IsEmpty())
	{
		WriteMat32F(RemoveExt(szPathName) + GetTime() + ".dat", contour);
	}

	if (contour->cols != 2)
		return;
	//
	//Draw contour
	//
	CvPoint2D32f * ctrlPt;
	CvPoint ptStart, ptEnd;
	CAAMShape::CAAMPointInfo curInfo; 
	int nextInd;
	for (int i = 0; i < contour->rows - 1; i ++)
	{
		//Current point info
		curInfo = ptInfo[i];
		ctrlPt = (CvPoint2D32f *)(cvGetPtrAt(contour, i));
		//I think there must be some simple way to convert 
		//CvPoint2D32f to CvPoint
		//XinFan 2003.5.28
		ptStart.x = (int)ctrlPt->x; ptStart.y = (int)ctrlPt->y;
//		DrawCross(srcImg, (int *)&ptStart, 3);
		//next point
		nextInd = curInfo.m_iConnectTo;
		ctrlPt = (CvPoint2D32f *)(cvGetPtrAt(contour, nextInd));
		ptEnd.x = (int)ctrlPt->x; ptEnd.y = (int)ctrlPt->y;
		
//		DrawCross(srcImg, (int *)&ptEnd, 3);

		cvLine(srcImg, ptStart, ptEnd, contour_color);
	}
	
	cvReleaseMat(&contour);
}
void DrawAAMShape(void *srcImg, const CAAMShape &shape)
{
	ASSERT(shape.NPoints() > 47);
	std::vector<CAAMShape::CAAMPointInfo> ptInfo = 
		shape.PointAux();
	int nextInd;
	double x, y;
	CvPoint ptStart, ptEnd;
	CAAMShape::CAAMPointInfo curInfo; 

	for (int i = 0; i < 46; i++)
	{
		shape.GetPoint(i, x, y);
		ptStart.x = (int) x; ptStart.y = (int)y;
		nextInd = ptInfo[i].m_iConnectTo;
		shape.GetPoint(nextInd, x, y);
		ptEnd.x = (int)x; ptEnd.y = (int) y; 
		cvLine(srcImg, ptStart, ptEnd, 255);
	}
}
//
//Draw crossing point on an image at a specific location
//
void DrawCross(void *srcImg, int *point, int cross_width)
{
	CvPoint *pt = (CvPoint *)point;
	CvPoint ptStart, ptEnd;
	ptStart = *pt; ptEnd = *pt;
	ptStart.x = pt->x - cross_width; ptEnd.x = pt->x + cross_width;
	cvLine(srcImg, ptStart, ptEnd, 0);
	ptStart = *pt; ptEnd = *pt;
	ptStart.y = pt->y - cross_width; ptEnd.y = pt->y + cross_width;
	cvLine(srcImg, ptStart, ptEnd,0);
}

void DrawCross(void *srcImg, long *point, int cross_width )
{
	CvPoint pt;
	pt.x = (int)*(point); pt.y = (int)*(point + 1);
	CvPoint ptStart, ptEnd;
	ptStart = pt; ptEnd = pt;
	ptStart.x = pt.x - cross_width; ptEnd.x = pt.x + cross_width;
	cvLine(srcImg, ptStart, ptEnd, 0);
	ptStart = pt; ptEnd = pt;
	ptStart.y = pt.y - cross_width; ptEnd.y = pt.y + cross_width;
	cvLine(srcImg, ptStart, ptEnd, 0);
}
void DrawCross(void *srcImg, float *point, int cross_width)
{
	CvPoint pt;
	pt.x = (int)*(point); pt.y = (int)*(point + 1);
	CvPoint ptStart, ptEnd;
	ptStart = pt; ptEnd = pt;
	ptStart.x = pt.x - cross_width; ptEnd.x = pt.x + cross_width;
	cvLine(srcImg, ptStart, ptEnd, 0);
	ptStart = pt; ptEnd = pt;
	ptStart.y = pt.y - cross_width; ptEnd.y = pt.y + cross_width;
	cvLine(srcImg, ptStart, ptEnd, 0);
}
//
//Draw a dark point on an image at a specific location to highlight a specific point, 
//	default radius = 3
//
void DrawCircle(void *srcImg, int *point, int radius)
{
	CvPoint *pCenter = (CvPoint *)point;
	cvCircle(srcImg, *pCenter, radius,0, -1);
}