jetstream.cpp

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

/////////////////////////////////////////////////////////////////////
//	
//	Implementation file of JetStream, 
//				which extracts contour based on particle filter
//
//	XinFan	2003.5.26
//Reference:
//	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 <stdlib.h>
#include <math.h>
#include <assert.h>
#include "JetStream.h"
#include "MemAlloc.h"
#include "Utility.h"
#include "FileIO.h"
//#include "ipl.h"
#include "cv.h"
#include "macro.h"



//#define _WRITE_SAMPLE
//#define _WRITE_X_SAMPLE
///////////////////////////////////////////////////////////////////////////////////////
//
//	Constructors and Deconstructors
//
///////////////////////////////////////////////////////////////////////////////////////

CJetStream::CJetStream()
{
	m_nStepNum		= 0;
	m_nCurStep		= 0;
	m_nDataWidth	= 0;
	m_nDataHeight	= 0;
	m_flStateSeq	= NULL;
	m_flStream		= NULL;
	m_tmpStream		= NULL;
}

CJetStream::CJetStream(int nStateDim, int nSamplesNum, int nStepNum) : 
		CParticle(nStateDim, nSamplesNum)
{
	m_nStepNum = nStepNum;
	if (nStepNum == 0)
		return;
	m_nCurStep		= 0;
	m_nDataWidth	= 0;
	m_nDataHeight	= 0;

	//Allocate buffer for particle stream
	m_flStream = NULL;
	m_tmpStream = NULL;
	AllocMem(m_flStream, nStepNum, nStateDim, nSamplesNum);
	AllocMem(m_tmpStream, nStepNum, nStateDim, nSamplesNum);
	m_flStateSeq = NULL;
	AllocMem(m_flStateSeq, nStateDim, nStepNum);
	/*m_flStream = (float ***)malloc(nStepNum * sizeof(float**));
	m_tmpStream = (float ***)malloc(nStepNum * sizeof(float**));
	float **sample_i;
	for (int i = 0; i < nStepNum; i++)
	{
		sample_i = NULL;
		AllocMem(sample_i, nStateDim, nSamplesNum);
		memset(*sample_i, 0, nStateDim * nSamplesNum *sizeof(float));
		*(m_flStream + i) = sample_i;
		sample_i = NULL;
		
	}*/
}
CJetStream::~CJetStream()
{
	if (m_nStepNum != 0)
	{
/*		for (int i = 0; i < m_nStepNum; i++)
			*(m_flStream + i) = FreeMem(*(m_flStream + i));
		free(m_flStream);
*/
		m_flStream = FreeMem(m_flStream, m_nStepNum);
		m_tmpStream = FreeMem(m_tmpStream, m_nStepNum);
		m_flStateSeq = FreeMem(m_flStateSeq);
	}
}
/////////////////////////////////////////////////////////////////////
//	Initialization
//
/////////////////////////////////////////////////////////////////////
void CJetStream::Initialize(const JETPARA* jetInitPara, 
							const CJetImgData* jetImgData, 
							const CJetInit* jetInit)
{
	//Initialize JetStream Parameters
	//All member variables are valuse
	//bit-wise copy is OK
	m_jetPara = *jetInitPara;
	//
	//Get the norm of the gradient
	//if neccessary, operator "=" can be overloaded
	//
	m_jetData.CopyOf(jetImgData);
	CvMat *img = (CvMat *) m_jetData.gradient_norm;
	m_nDataWidth = img->cols;
	m_nDataHeight = img->rows;
	//
	for (int i = 0; i < jetInit->ptNum; i++)
	{
		//Set initial samples
		InitSample(*(m_flStream + i), jetInit->pt[i], m_jetPara.step_length);
		//Set initial estimated states as the specified ones
		//memcpy(*(m_flStateSeq + i), jetInit->pt[i], m_nStDim * sizeof(float));
		//SetVec(*(m_flStateSeq + i), jetInit->pt[i], m_nStDim);
#ifdef _WRITE_SAMPLE
		CvMat *sampleMat = cvCreateMatHeader(m_nSamplesNum, m_nStDim, CV_32FC1);
		cvSetData(sampleMat, *(*(m_flStream + i)), m_nStDim * sizeof(float));
		char name[50];
		sprintf(name, "inisample-%d.dat", i);
		WriteMat32F(name, sampleMat);
		cvReleaseMatHeader(&sampleMat);

#endif
	}
	//set current samples, weiths, and cumulative prob.
	SetVec(*m_flSamples, *(m_flStream[i - 1]), m_nStDim * m_nSamplesNum);
	SetVec(m_flConfidence, (float)1.0 / m_nSamplesNum, m_nSamplesNum);
	CalCumulative();
	//Set prior and proposal probabilites 
	memset(m_flPriorProb, 0, m_nSamplesNum * sizeof(float));
	memset(m_flProposalProb, 0, m_nSamplesNum * sizeof(float));
	m_nCurStep = jetInit->ptNum - 1;
	EstState(1);
	//Set estimated states as initial points
}
//////////////////////////////////////////////////////////////////////
//
//	InitSample(float **sampleArr, float *startPt, float d)
//Purpose:
//	Initializing samples  with a starting point specified by jetInit
//	The initial samples are generated as Xi = Xs + U(d/2)
//		Xi--------Initial samples, sampleArry
//		Xs--------Starting point, startPt
//		U(d/2)----Random numbers uniformly distributed between (-d/2, d/2]
//////////////////////////////////////////////////////////////////////
void CJetStream::InitSample(float **sampleArr, float *startPt, float d)
{
	//Initializing x(0:1)
	for (int i = 0; i < m_nSamplesNum; i++)
	{
		for (int j = 0; j < m_nStDim; j++)
		{
			*(*(sampleArr + i) + j) = *(startPt + j) + (float)(uniform_random() - 0.5) * d;
		}
	}
}
/////////////////////////////////////////////////////////////////////////////
//	EvalWeight()
//
//Purpose:
//	Evaluate the weights of predicted samples
//	Currently, the weights is the liklihood ratio
//TODO:
//	Implement weights update with propsal density 
//		differing from prior probability
//	XinFan 2003.5.28
//
/////////////////////////////////////////////////////////////////////////////
void CJetStream::EvalWeight(int nStep)
{
	//Evalating predicted states
	//if ((nStep  m_nCurStep) != 1)
	if (nStep != m_nCurStep)
		return;
	//Current point
	CvPoint2D32f *x_i_k, *x_im1_k;
	float **x_im1 = *(m_flStream + nStep - 1);
	int x, y;
	//pointer to the gradient norm and orientation at current location
	float *norm;
	float *or;
	//normal of the greadient orientation at current location
	float or_norm;

	//angle_xk, current curve orientation
	//pasia, the angle between angle_xk and the gradient normal
	float angle_xk, pasai;
	float poff, pon;

	for (int k = 0; k < m_nSamplesNum; k++)
	{
		//x(i) - x(i-1)
		x_i_k = (CvPoint2D32f *) (*(m_flSamples + k));
		x_im1_k = (CvPoint2D32f *) (*(x_im1 + k));
		angle_xk = atan_u(x_i_k->x - x_im1_k->x, x_i_k->y - x_im1_k->y);
		//the nearest point
		x = (int)(x_i_k -> x);
		y = (int)(x_i_k -> y);
		assert(x < m_nDataWidth && x >= 0);
		assert(y < m_nDataHeight && y >= 0);
		norm = (float *)(cvGetPtrAt(m_jetData.gradient_norm, y, x));
		//gradient normal
		or = (float *)(cvGetPtrAt(m_jetData.gradient_orient, y, x));
//		*or = (*or >= 0) ? (*or - PI/2) : (PI/2 + *or);
		or_norm = (*or >= 0) ? (*or - PI/2) : (PI/2 + *or);
		//Evaluate poff
		poff = (float)exp( - *norm / m_jetPara.mea_lamda);
		//Evaluate pon
		if (*(cvGetPtrAt(m_jetData.corner_mask, y,x)))
		{
			pon = (float)(1.0 / PI);
		}
		else
		{
//			pasai = *or - angle_xk;	
			pasai = or_norm - angle_xk;
			pasai = fabs(pasai) > PI / 2 ? (PI - fabs(pasai)) : pasai;
			pon = evaluate_gaussian(sqrt(*norm) * pasai, m_jetPara.mea_sigma);
		}
		//likelihood ratio
		if (poff < 1.0e-2)
		{
			m_flConfidence[k] = pon;
//		m_flConfidence[k] = (float)(1.0 / m_nSamplesNum);
		}
		else
		{
			m_flConfidence[k] = pon / poff;
		}
		m_flConfidence[k] *= m_flPriorProb[k] / m_flProposalProb[k];
	}
	//normalizing weights
	NormWeights();
	CalCumulative();
#ifdef _WRITE_SAMPLE
		CvMat *sampleMat = cvCreateMatHeader(m_nSamplesNum, 1, CV_32FC1);
		cvSetData(sampleMat, m_flConfidence, sizeof(float));
		char name[50];
		sprintf(name, "weights-%d.dat", nStep);
		WriteMat32F(name, sampleMat);
		cvReleaseMatHeader(&sampleMat);

#endif
}
/////////////////////////////////////////////////////////////////////////////
//
//	UpdateByTime()
//
//Purpose:
//	Update JetStream Particles as (Ref.[1]):
//		x_i+1 = x_i + R(sita_i)(x_i - x_i-1)
//		with q(sita_i) = mu / PI + (1 - v)N(sita_i; 0, d* sigma_sita ^2)
//
/////////////////////////////////////////////////////////////////////////////
void CJetStream::UpdateByTime(int nStep)
{
	//Predict next step
	if ((nStep - m_nCurStep) != 1)
		return;
	//Samples x_i, x_i-1
	float **x_i = *(m_flStream + nStep - 1);
	float **x_im1 = *(m_flStream + nStep - 2);
	//predicted sample
	float **x_ip1 = NULL;
	//AllocMem(x_ip1, m_nStDim, m_nSamplesNum);
	//Contour points
	int x, y;
	CvPoint2D32f *x_i_k, *x_im1_k, *x_ip1_k;
	//CvMat *corner;
	double sita;
	float d = m_jetPara.step_length;
	for (int k = 0; k < m_nSamplesNum; k++)
	{
		//the kth sample
		x_i_k = (CvPoint2D32f *) (*(x_i + k));
		x_im1_k = (CvPoint2D32f *) (*(x_im1 + k));
		x_ip1_k = (CvPoint2D32f *) (*(m_flSamples + k));
		x = (int)(x_i_k->x );
		y = (int)(x_i_k->y );
		assert(x < m_nDataWidth && x > 0);
		assert(y < m_nDataHeight && y > 0);

		//whether a corner
		//corner = (CvMat*) m_jetData.corner_mask;
//SAMPLE:	
		if (*(cvGetPtrAt(m_jetData.corner_mask, y,x)))
		{
			sita = (uniform_random() - 0.5) * 2 * PI;
			//Evaluate Proposal Probability
			m_flProposalProb[k] = (float)(1.0 / PI / 2);
		}
		else
		{
			sita = gaussian_random() * m_jetPara.dyn_sigma * 
				sqrt(m_jetPara.step_length);
			//Evaluate Proposal Probability
			m_flProposalProb[k] = evaluate_gaussian(sita, m_jetPara.dyn_sigma * 
				sqrt(m_jetPara.step_length));
		}
		//predict
		x_ip1_k->x = x_i_k->x + cos(sita) * (x_i_k->x - x_im1_k->x) 
			- sin(sita) * (x_i_k->y - x_im1_k->y);
		x_ip1_k->y = x_i_k->y + sin(sita) * (x_i_k->x - x_im1_k->x) 
			+ cos(sita) * (x_i_k->y - x_im1_k->y);
//		if (x_ip1_k->x > m_nDataWidth - 1 || x_ip1_k->y > m_nDataHeight - 1)
//			goto SAMPLE;
		x_ip1_k->x = BOUND(x_ip1_k->x, 1, m_nDataWidth - 1);
		x_ip1_k->y = BOUND(x_ip1_k->y, 1, m_nDataHeight - 1 );
		//Evaluate Prior Probability
		m_flPriorProb[k] = (float)(m_jetPara.dyn_mu / PI / 2) + (1.0 - m_jetPara.dyn_mu) *
			evaluate_gaussian(sita, m_jetPara.dyn_sigma * sqrt(m_jetPara.step_length));
	}
#ifdef _WRITE_SAMPLE
		CvMat *sampleMat = cvCreateMatHeader(m_nSamplesNum, m_nStDim, CV_32FC1);