[Robotflow-cvs] RobotFlow/Vision/src DrawPFParticle.cc,NONE,1.1 PFGenericParticle.cc,NONE,1.1 PFGenericParticleFilter.cc,NONE,1.1 PFPMRandomWalk.cc,NONE,1.1 PFState2VisualROI.cc,NONE,1.1 PFUtilityFct.cc,NONE,1.1 VisualTargetPFInterface.cc,NONE,1.1

[Pierre M. <sid...@us...>]

Update of /cvsroot/robotflow/RobotFlow/Vision/src
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv7826

Added Files:
	DrawPFParticle.cc PFGenericParticle.cc 
	PFGenericParticleFilter.cc PFPMRandomWalk.cc 
	PFState2VisualROI.cc PFUtilityFct.cc 
	VisualTargetPFInterface.cc 
Log Message:
Generic particle filter implementation for visual tracking purposes

--- NEW FILE: PFGenericParticleFilter.cc ---
/* Copyright (C) 2005 Pierre Moisan (Pie...@US...) 

   This library is free software; you can redistribute it and/or
   modify it under the terms of the GNU Lesser General Public
   License as published by the Free Software Foundation; either
   version 2.1 of the License, or (at your option) any later version.
   
   This library is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
   Lesser General Public License for more details.
   
   You should have received a copy of the GNU Lesser General Public
   License along with this library; if not, write to the Free Software
   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#include "PFGenericParticleFilter.h"

using namespace std;
using namespace FD;

namespace RobotFlow {

DECLARE_NODE(PFGenericParticleFilter)

  /*Node
   *
   * @name PFGenericParticleFilter
   * @category RobotFlow:Vision:Tracking
   * @description Generic implementation of a particle filter.
   *
   * @parameter_name NUM_PARTICLES
   * @parameter_type int
   * @parameter_value 200
   * @parameter_description Number of particles (samples) to use.
   *
   * @parameter_name PARTICLE_STATE_SIZE
   * @parameter_type int
   * @parameter_value 4
   * @parameter_description Particle state size.
   *
   * @input_name INIT_VARIANCE
   * @input_type Vector<float>
   * @input_description Variance for initialization of each of the particle state.
   *
   * @input_name REFERENCE_MEAN_STATE
   * @input_type PFGenericParticle
   * @input_description Current reference mean state to track.
   *
   * @input_name PREDICT_COMPLETED
   * @input_type bool
   * @input_description Flag indicating the success of the prediction.
   *
   * @input_name PARTICLE_LIKELIHOOD
   * @input_type float
   * @input_description Resulting measurement likelihood for current particle.
   *
   * @output_name FILTERING_FINISHED
   * @output_type bool
   * @output_description Flag indicating that the tracking has completed.
   *
   * @output_name CURRENT_PARTICLE
   * @output_type PFGenericParticle
   * @output_description Current particle (sample) to apply prediction model.
   *
   * @output_name TRACKED_MEAN_STATE
   * @output_type PFGenericParticle
   * @output_description Resulting mean state after particle filtering.
   *
   END*/

PFGenericParticleFilter::PFGenericParticleFilter()
: PFParticleFilter(e_PF_GenericFilter),
m_init(false),
m_initPF(false),
m_initSamples(false),
m_finished(false),
m_curSampleIdx(-1),
m_numSamples(0),
m_sampleStateSize(0),
m_samples(NULL),
m_tmpSamples(NULL),
m_cumulWeight(NULL),
m_initVariance(NULL)
{ 
}
	
PFGenericParticleFilter::PFGenericParticleFilter(unsigned int i_numSamples, 
		unsigned int i_sampleStateSize, const Vector<float> *i_initVariance) 
: PFParticleFilter(e_PF_GenericFilter),
m_init(false),
m_initPF(false),
m_initSamples(false),
m_finished(false),
m_curSampleIdx(-1),
m_numSamples(i_numSamples),
m_sampleStateSize(i_sampleStateSize),
m_samples(NULL),
m_tmpSamples(NULL),
m_cumulWeight(NULL),
m_initVariance(NULL)
{ 
	Initialize(i_initVariance);
}
	
PFGenericParticleFilter::PFGenericParticleFilter(string nodeName, ParameterSet params)
: PFParticleFilter(nodeName, params),
m_init(false),
m_initPF(false),
m_initSamples(false),
m_finished(false),
m_curSampleIdx(-1),
m_numSamples(0),
m_sampleStateSize(0),
m_samples(NULL),
m_tmpSamples(NULL),
m_cumulWeight(NULL),
m_initVariance(NULL)
{
	SetType(e_PF_GenericFilter);
	
	m_initVarianceInID = addInput("INIT_VARIANCE");
	m_refMeanStateInID = addInput("REFERENCE_MEAN_STATE");
	m_predictInID = addInput("PREDICT_COMPLETED");
	m_likelihoodInID = addInput("PARTICLE_LIKELIHOOD");
	
	m_finishedOutID = addOutput("FILTERING_FINISHED");
	m_particleOutID = addOutput("CURRENT_PARTICLE");
	m_meanStateOutID = addOutput("TRACKED_MEAN_STATE");
	
	m_numSamples = dereference_cast<int>(parameters.get("NUM_PARTICLES"));
	m_sampleStateSize = dereference_cast<int>(parameters.get("PARTICLE_STATE_SIZE"));
}

PFGenericParticleFilter::~PFGenericParticleFilter()
{
	if (m_initVariance) {
		m_initVariance->destroy();
	}
	
	delete [] m_samples;
	delete [] m_tmpSamples;
	delete [] m_cumulWeight;
}

// Modified BufferedNode request method to support cyclic node connection
void PFGenericParticleFilter::request(int output_id, const ParameterSet &req) 
{
	if (req.exist("LOOKAHEAD")) {
		outputs[output_id].lookAhead = max(outputs[output_id].lookAhead,dereference_cast<int> (req.get("LOOKAHEAD")));
	}
	
	if (req.exist("LOOKBACK")) {
		outputs[output_id].lookBack = max(outputs[output_id].lookBack,dereference_cast<int> (req.get("LOOKBACK")));
	}
	
	if (req.exist("INORDER")) {
		inOrder = true;
	}
	
	int outputLookAhead=0, outputLookBack=0;

	outputLookAhead=max(outputLookAhead, outputs[output_id].lookAhead);
	outputLookBack =max(outputLookBack, outputs[output_id].lookBack);
	
	if (output_id == m_finishedOutID) {
		// TRACKING_FINISHED output does not required any inputs
		return;
		
	}
	else if (output_id == m_particleOutID) {
		// CURRENT_PARTICLE output does not required any inputs
		return;
	}
	else if (output_id == m_meanStateOutID) {
		ParameterSet myReq, myReq2, myReq3, myReq4;
		
		myReq.add("LOOKAHEAD", ObjectRef(Int::alloc(inputsCache[m_initVarianceInID].lookAhead+outputLookAhead)));
		myReq.add("LOOKBACK", ObjectRef(Int::alloc(inputsCache[m_initVarianceInID].lookBack+outputLookBack)));
		inputs[m_initVarianceInID].node->request(inputs[m_initVarianceInID].outputID,myReq);
		
		myReq2.add("LOOKAHEAD", ObjectRef(Int::alloc(inputsCache[m_refMeanStateInID].lookAhead+outputLookAhead)));
		myReq2.add("LOOKBACK", ObjectRef(Int::alloc(inputsCache[m_refMeanStateInID].lookBack+outputLookBack)));
		inputs[m_refMeanStateInID].node->request(inputs[m_refMeanStateInID].outputID,myReq2);
		
		myReq3.add("LOOKAHEAD", ObjectRef(Int::alloc(inputsCache[m_predictInID].lookAhead+outputLookAhead)));
		myReq3.add("LOOKBACK", ObjectRef(Int::alloc(inputsCache[m_predictInID].lookBack+outputLookBack)));
		inputs[m_predictInID].node->request(inputs[m_predictInID].outputID,myReq3);
		
		myReq4.add("LOOKAHEAD", ObjectRef(Int::alloc(inputsCache[m_likelihoodInID].lookAhead+outputLookAhead)));
		myReq4.add("LOOKBACK", ObjectRef(Int::alloc(inputsCache[m_likelihoodInID].lookBack+outputLookBack)));
		inputs[m_likelihoodInID].node->request(inputs[m_likelihoodInID].outputID,myReq4);
	}
	else {
		throw new GeneralException ("PFGenericParticleFilter::request : unknown output ID.",__FILE__,__LINE__);
	}
}


void PFGenericParticleFilter::calculate(int output_id, int count, Buffer &out)
{
	try {
		if (output_id == m_finishedOutID) {
			(*outputs[m_finishedOutID].buffer)[count] = ObjectRef(Bool::alloc(!m_finished));
			if (m_finished) {
				m_initPF = false;
				m_finished = false;
			}
		}
		else if (output_id == m_particleOutID) {
			(*outputs[m_particleOutID].buffer)[count] = ObjectRef(m_curSample);
		}
		else if (output_id == m_meanStateOutID){
			if (!m_initPF) {
				// First get reference mean state to track
				ObjectRef meanStateRef = getInput(m_refMeanStateInID, count);
				
				if (meanStateRef->isNil()) {
					// Tracking failed of has not yet started
					m_initSamples = false;
					
					// Invalid target, output nilObject
					(*outputs[m_meanStateOutID].buffer)[count] = ObjectRef(nilObject);
					return;
				}
				
				m_refMeanState = RCPtr<PFGenericParticle>(meanStateRef);
				
				// Get INIT_VARIANCE only once for intialization
				if (!m_init) {
					ObjectRef varObjRef = getInput(m_initVarianceInID, count);
					
					if (!varObjRef->isNil()) {
						RCPtr<Vector<float> > varRef = varObjRef;
						
						Initialize(&(*varRef));
					}
					else {
						throw new GeneralException ("PFGenericParticleFilter::calculate : invalid (nilObject) INIT_VARIANCE input.",__FILE__,__LINE__);
					}
				}
				
				if (!m_initSamples) {
					InitSamples();
				}
				
				m_curSampleIdx = 0;
				m_likelihoodsSum = 0.f;
				
				m_initPF = true;
			}
			
			// Get a reference to current particle
			m_curSample = RCPtr<PFGenericParticle>(m_samples[m_curSampleIdx]);
			
			//
			// Prediction step
			// Apply prediction to current particle
			//
			bool predictCompleted = dereference_cast<bool>(getInput(m_predictInID, count));
			
			if (!predictCompleted) {
				throw new GeneralException ("PFGenericParticleFilter::calculate : prediction failed on current particle.",__FILE__,__LINE__);
			}
			
			//
			// Measurement step
			// Compute likelihood of the current particle
			//
			float likelihood = dereference_cast<float>(getInput(m_likelihoodInID, count));
			
			if (likelihood < 0.f || likelihood > 1.f) {
				throw new GeneralException ("PFGenericParticleFilter::calculate : invalid likelihood for current particle (should be between [0.0, 1.0]).",__FILE__,__LINE__);
			}
			
			// Assign likelihood to current particle's weight
			m_curSample->SetWeight(likelihood);
			m_likelihoodsSum += likelihood;
			
			/*
			cout << "Current particle info: likelihood=" << likelihood << endl;
			float *p_refState = m_refMeanState->GetState();
			for (int i=0; i<m_sampleStateSize; i++) {
				cout << "State[" << i << "]=" << *p_refState++ << endl;
			}
			*/
			
			m_curSampleIdx++;
			
			// Verify if we have done every particle
			if (m_curSampleIdx >= m_numSamples) {
				m_finished = true;
			}
				
			if (!m_finished) {
				(*outputs[m_meanStateOutID].buffer)[count] = ObjectRef(nilObject);
			}
			else {
				//
				// Update step
				// Propagate densities and update samples
				//
				Update();
				
				// Compute mean state
				ComputeMeanState();
				
				//
				// Resampling step
				//
				Resample();
				
				// Output mean state
				(*outputs[m_meanStateOutID].buffer)[count] = ObjectRef(m_outMeanState);
			}
		}
	}
	catch (BaseException *e) {
		throw e->add(new GeneralException("Exception in PFGenericParticleFilter::calculate:",__FILE__,__LINE__));
	}
}

void PFGenericParticleFilter::Initialize(const Vector<float> *i_variance)
{
	try {
		if (m_sampleStateSize != i_variance->size()) {
			throw new GeneralException ("PFGenericParticleFilter::Initialize : particle state size differs from variance vector size.",__FILE__,__LINE__);
		}
		m_initVariance = Vector<float>::alloc(m_sampleStateSize);
		
		for (int i=0; i<m_sampleStateSize; i++) {
			(*m_initVariance)[i] = (*i_variance)[i];
		}
		
		m_samples = new RCPtr<PFGenericParticle>[m_numSamples];
		m_tmpSamples = new RCPtr<PFGenericParticle>[m_numSamples];
		
		for (int s=0; s<m_numSamples; s++) {
			m_samples[s] = RCPtr<PFGenericParticle>(new PFGenericParticle(m_sampleStateSize));
			m_tmpSamples[s] = RCPtr<PFGenericParticle>(new PFGenericParticle(m_sampleStateSize));
		}
		
		m_cumulWeight = new float[m_numSamples];
		
		m_outMeanState = RCPtr<PFGenericParticle>(new PFGenericParticle(m_sampleStateSize));
		
		m_init = true;
	}
	catch (BaseException *e) {
		throw e->add(new GeneralException("Exception in PFGenericParticleFilter::Initialize:",__FILE__,__LINE__));
	}
}

void PFGenericParticleFilter::InitSamples()
{
	int i, s;
	float *p_sampleState, *p_refState;
	
	if (!m_samples) {
		throw new GeneralException ("PFGenericParticleFilter::InitSamples : impossible to initialize samples without proper memory allocation.",__FILE__,__LINE__);
	}
	
	if (m_refMeanState->GetType() == e_PFP_Unknown) {
		throw new GeneralException ("PFGenericParticleFilter::InitSamples : cannot use particle with unknown type.",__FILE__,__LINE__);
	}
	
	for (s=0; s<m_numSamples; s++) {
		p_sampleState = m_samples[s]->GetState();
		p_refState = m_refMeanState->GetState();
		
		for (i=0; i<m_sampleStateSize; i++) {
			*p_sampleState++ = (*p_refState++)*(1.f + (*m_initVariance)[i]*(PFUTIL_randn()-0.5f));
		}
	}
	
	m_initSamples = true;
}

void PFGenericParticleFilter::Update()
{
	int s;
	float *p_cumulWeight, curWeight;
	float cumulSum=1e-30, weightSum=1e-30, weightSumInv, weightBias;
	
	if (!m_samples) {
		throw new GeneralException ("PFGenericParticleFilter::Update : impossible to update with uninitialized samples.",__FILE__,__LINE__);
	}
	
	p_cumulWeight = m_cumulWeight;
	
	/*
	if (m_likelihoodsSum != 0.f) {
		likelihoodsSumInv = 1.f/m_likelihoodsSum;
	}
	else {
		likelihoodsSumInv = 1.f;
	}
	*/
	
	for (s=0; s<m_numSamples; s++) {
		// Normalize weights
		//curWeight = m_samples[s]->GetWeight()*likelihoodsSumInv;
		curWeight = m_samples[s]->GetWeight();
		m_samples[s]->SetWeight(curWeight);
		cumulSum += curWeight;
		
		// Compute cumulative weights
		*p_cumulWeight++ = cumulSum;
		weightSum += cumulSum;
	}
	
	weightSumInv = 1.f/weightSum;
	weightBias = (1.f-m_likelihoodsSum)/m_numSamples;
	
	p_cumulWeight = m_cumulWeight;
	
	for (s=0; s<m_numSamples; s++) {
		*p_cumulWeight = weightSumInv*(*p_cumulWeight)*m_likelihoodsSum + weightBias;
		p_cumulWeight++;
	}
	
	p_cumulWeight = m_cumulWeight;
	weightSum = 1e-30;
	for (s=0; s<m_numSamples; s++) {
		curWeight = m_samples[s]->GetWeight()*(*p_cumulWeight);
		m_samples[s]->SetWeight(curWeight);
		weightSum += curWeight;
		p_cumulWeight++;
	}
	
	weightSumInv = 1.f/weightSum;
	for (s=0; s<m_numSamples; s++) {
		curWeight = m_samples[s]->GetWeight()*weightSumInv;
		m_samples[s]->SetWeight(curWeight);
	}
}

void PFGenericParticleFilter::ComputeMeanState()
{
	int i, s;
	float *p_sampleState, *p_meanState, likelihood;
	
	if (!m_samples) {
		throw new GeneralException ("PFGenericParticleFilter::ComputeMeanState : impossible to compute mean state with uninitialized samples.",__FILE__,__LINE__);
	}
	
	p_meanState = m_outMeanState->GetState();
	for (i=0; i<m_sampleStateSize; i++) {
		*p_meanState++ = 0.f;
	}
	
	// Perform a weighted sum to get the mean state
	for (s=0; s<m_numSamples; s++) {
		likelihood = m_samples[s]->GetWeight();
		p_sampleState = m_samples[s]->GetState();
		p_meanState = m_outMeanState->GetState();
		
		for (i=0; i<m_sampleStateSize; i++) {
			*p_meanState += (*p_sampleState++)*likelihood;
			p_meanState++;
		}
	}
}

void PFGenericParticleFilter::Resample()
{
	int s;
	float effectiveN = 0.f, curWeight, sizeInv, sumWeights;
	
	if (!m_samples) {
		throw new GeneralException ("PFGenericParticleFilter::Resample : impossible to resample with uninitialized samples.",__FILE__,__LINE__);
	}
	
	for (s=0; s<m_numSamples; s++) {
		curWeight = m_samples[s]->GetWeight();
		effectiveN += curWeight * curWeight;
	}
	
	effectiveN = 1.f/(effectiveN*m_numSamples);
	
	if (effectiveN > 0.6f) {
		// Current distribution modelled by the particles set has a sufficient density
		return;
	}
	
	CopyTmpSamples();
	
	m_cumulWeight[0] = m_tmpSamples[0]->GetWeight();
	for (s=1; s<m_numSamples; s++) {
		m_cumulWeight[s] = m_cumulWeight[s-1]+m_samples[s]->GetWeight();
	}
	
	sumWeights = 0.f;
	for (s=0; s<m_numSamples; s++) {
		float r = PFUTIL_ran();
		int j=PFUTIL_find_range(r, m_cumulWeight, m_numSamples);
		*(m_samples[s]) = *(m_tmpSamples[j]);
		m_samples[s]->SetWeight(1.f);
		sumWeights += m_samples[s]->GetWeight();
	}
	
	sumWeights = 1.f/sumWeights;
	for (s=0; s<m_numSamples; s++) {
		curWeight = m_samples[s]->GetWeight();
		m_samples[s]->SetWeight(curWeight*sumWeights);
	}
}

//
// Private methods
//

void PFGenericParticleFilter::CopyTmpSamples()
{
	int s;
	
	for (s=0; s<m_numSamples; s++) {
		*(m_tmpSamples[s]) = *(m_samples[s]);
	}
}

}


--- NEW FILE: PFUtilityFct.cc ---
//
// Utility functions mainly for random numbers generation
//  Copyright (C) 2004 Jean-Marc Valin
//

#include "PFUtilityFct.h"

using namespace std;

namespace RobotFlow {

static float PFUTIL_logtable2[PFUTIL_FLOGLOOKUP2SIZE];

void PFUTIL_build_flog_table()
{
	static bool init=false;
	if (!init) {
		PFUTIL_FloatManip m;
		
		for (int i=0;i<PFUTIL_FLOGLOOKUP2SIZE;i++) {
			m.i = (i<<PFUTIL_FLOGLOOKUP2SHIFT) | 0x3f800000;
			PFUTIL_logtable2[i]=log(m.f);
		}
		
		init=true;
	}
}

float PFUTIL_flog(const float in)
{
	PFUTIL_build_flog_table();
	PFUTIL_FloatManip m;
	m.f = in;
	
	//Extract the mantissa and perform lookup for log(mantissa)
	float tb = PFUTIL_logtable2[(m.i & 0x007fffff)>>PFUTIL_FLOGLOOKUP2SHIFT];
	//Extract the exponent
	int id = (m.i>>23)-127;
	
	return id*M_LN2 + tb;
}

float PFUTIL_randn()
{
	static bool avail = false;
	static float cached=0.f;
	
	if (avail) {
		avail = false;
		return cached;
	} 
	else {
		float U1, U2, S, x;
		do {
			U1 = float(rand())/float(RAND_MAX);
			U2 = float(rand())/float(RAND_MAX);
			U1 = 2*U1-1;
			U2 = 2*U2-1;
			S = U1*U1 + U2*U2;
		} while (S >= 1 || S == 0.0f);
		
		float tmp = sqrt(-2 * PFUTIL_flog(S) / S);
		cached = tmp * U1;
		avail = true;
		
		return tmp * U2;
	}
}

float PFUTIL_ran()
{
	const unsigned int jflone = 0x3f800000;
	const unsigned int jflmsk = 0x007fffff;
	static int idum = 10000;
	union {int i; float f;} ran;
	
	idum = 1664525*idum + 1013904223;
	ran.i = jflone | (jflmsk & idum);
	ran.f -= 1;
	
	return ran.f;
}

int PFUTIL_find_range(float i_x, const float *i_cumul, int i_cumulSize) 
{
	int low=0;
	int high=i_cumulSize-1;
	int cur = high>>1;
	
	while (1) {
		if (i_cumul[cur] > i_x) {
			if (cur == 0 || i_cumul[cur-1] <= i_x) {
				break;
			} 
			else {
				high = cur-1;
				cur = (low+cur) >> 1;
			}
		} 
		else {
			low = cur;
			cur = (high+cur+1) >> 1;
			
			if (cur==high) {
				break;
			}
		}
	}

	return cur;
}

}

--- NEW FILE: PFGenericParticle.cc ---
/* Copyright (C) 2005 Pierre Moisan (Pie...@US...) 

   This library is free software; you can redistribute it and/or
   modify it under the terms of the GNU Lesser General Public
   License as published by the Free Software Foundation; either
   version 2.1 of the License, or (at your option) any later version.
   
   This library is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
   Lesser General Public License for more details.
   
   You should have received a copy of the GNU Lesser General Public
   License along with this library; if not, write to the Free Software
   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#include "PFGenericParticle.h"

using namespace std;
using namespace FD;

namespace RobotFlow {

DECLARE_TYPE(PFGenericParticle)

PFGenericParticle::PFGenericParticle()
: PFParticle(e_PFP_Generic),
m_stateSize(0),
m_state(NULL),
m_weight(-1)
{ 
}

PFGenericParticle::PFGenericParticle(unsigned int i_stateSize) 
: PFParticle(e_PFP_Generic),
m_stateSize(i_stateSize),
m_state(NULL),
m_weight(-1)
{ 
	m_state = new float [m_stateSize];
}

PFGenericParticle::PFGenericParticle(const PFGenericParticle &i_ref) 
{ 
	this->SetType(i_ref.GetType());
	m_stateSize = i_ref.m_stateSize;
	m_state = new float [m_stateSize];
	m_weight = i_ref.m_weight;
	
	for (int i=0; i<m_stateSize; i++) {
		m_state[i] = i_ref.m_state[i];
	}
}

PFGenericParticle::~PFGenericParticle()
{
	delete [] m_state;
}

PFGenericParticle & PFGenericParticle::operator =(const PFGenericParticle &i_ref)
{
	// Avoid self assignment
	if (&i_ref != this) {
		this->SetType(i_ref.GetType());
		
		if (m_stateSize != i_ref.m_stateSize) {
			m_stateSize = i_ref.m_stateSize;
			delete m_state;
			m_state = new float [m_stateSize];
		}
		
		if (m_state && i_ref.m_state) {
			for (int i=0; i<m_stateSize; i++) {
				m_state[i] = i_ref.m_state[i];
			}
		}
		
		m_weight = i_ref.m_weight;
	}
	
	return *this;
}

PFGenericParticle* PFGenericParticle::clone()  const
{
	return new PFGenericParticle(*this);
}

// Default routine to print a PFGenericParticle object to an output stream
void PFGenericParticle::printOn(ostream &out) const
{
	throw new GeneralException("Exception in PFGenericParticle::printOn: routine not yet implemented.",__FILE__,__LINE__);
}

// Default routine to read a PFGenericParticle object from an input stream
void PFGenericParticle::readFrom(istream &in)
{
	throw new GeneralException("Exception in PFGenericParticle::readFrom: routine not yet implemented.",__FILE__,__LINE__);
}

unsigned int PFGenericParticle::GetStateSize() const
{
	return m_stateSize;
}

float *PFGenericParticle::GetState()
{
	return m_state;
}

const float *PFGenericParticle::GetCstState() const
{
	return (const float *) m_state;
}

float PFGenericParticle::GetStateIdx(int i_idx) const
{
	if (i_idx < 0 || i_idx >= m_stateSize) {
		throw new GeneralException("Exception in PFGenericParticle::GetStateIdx: invalid state index.",__FILE__,__LINE__);
	}
	
	return m_state[i_idx];
}

float PFGenericParticle::GetWeight() const
{
	return m_weight;
}

void PFGenericParticle::SetStateSize(unsigned int i_size)
{
	if (i_size != m_stateSize) {
		m_stateSize = i_size;
		
		delete [] m_state;
		m_state = new float[m_stateSize];
	}
}

// Assumes that given ptr has the same size as current particle
void PFGenericParticle::SetState(const float *i_state)
{
	for (int i=0; i<m_stateSize; i++) {
		m_state[i] = i_state[i];
	}
}
	
void PFGenericParticle::SetStateIdx(int i_idx, float i_val)
{
	if (i_idx < 0 || i_idx >= m_stateSize) {
		throw new GeneralException("Exception in PFGenericParticle::SetStateIdx: invalid state index.",__FILE__,__LINE__);
	}
	
	m_state[i_idx] = i_val;
}
	
void PFGenericParticle::SetWeight(float i_weight)
{
	m_weight = i_weight;
}

}


--- NEW FILE: VisualTargetPFInterface.cc ---
/* Copyright (C) 2005 Pierre Moisan (Pie...@US...) 

   This library is free software; you can redistribute it and/or
   modify it under the terms of the GNU Lesser General Public
   License as published by the Free Software Foundation; either
   version 2.1 of the License, or (at your option) any later version.
   
   This library is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
   Lesser General Public License for more details.
   
   You should have received a copy of the GNU Lesser General Public
   License along with this library; if not, write to the Free Software
   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#ifndef _VISUALROI2PFSTATE_CC_
#define _VISUALROI2PFSTATE_CC_

#include "BufferedNode.h"
#include "VisualTarget.h"
#include "PFGenericParticle.h"

using namespace std;
using namespace FD;

namespace RobotFlow {

class VisualTargetPFInterface;
DECLARE_NODE(VisualTargetPFInterface)

  /*Node
   *
   * @name VisualTargetPFInterface
   * @category RobotFlow:Vision:Tracking
   * @description Interface for tracking a VisualTarget using a particle filter framework.
   *
   * @parameter_name USE_SCALE
   * @parameter_type bool
   * @parameter_value true
   * @parameter_description Flag indicating to use the ROI scale (width and height) in the particle's state.
   *
   * @parameter_name USE_ROTATION_ANGLE
   * @parameter_type bool
   * @parameter_value false
   * @parameter_description Flag indicating to use the ROI rotation angle in the particle's state.
   *
   * @input_name IN_TARGET
   * @input_type VisualTarget<double>
   * @input_description Current VisualTarget to convert.
   *
   * @input_name IN_FEATURES_VECTOR
   * @input_type Vector<VisualFeatureDesc<double> *>
   * @input_description Features descriptors vector (to compute a particle likelihood).
   *
   * @input_name IN_MEAN_STATE
   * @input_type PFGenericParticle
   * @input_description Mean state particle resulting from partcile filter tracking.
   *
   * @input_name PREPROCESS_COMPLETED
   * @input_type Vector<int>
   * @input_description Flags indicating the completion of the features extraction preprocessing stage.
   *
   * @input_name FILTERING_FINISHED
   * @input_type bool
   * @input_description Flags indicating the completion of the particle filtering stage.
   *
   * @output_name OUT_PARTICLE
   * @output_type PFGenericParticle
   * @output_description Resulting particle with appropriate state.
   *
   * @output_name OUT_LIKELIHOOD
   * @output_type float
   * @output_description Current particle likelihood.
   *
   * @output_name OUT_TARGET
   * @output_type VisualTarget<double>
   * @output_description Resulting tracked target.
   *
   * @output_name TRACKING_FINISHED
   * @output_type bool
   * @output_description Flag indicating that the tracking has completed.
   *
   END*/

class VisualTargetPFInterface : public BufferedNode
{
public:
	VisualTargetPFInterface::VisualTargetPFInterface(string nodeName, ParameterSet params)
	: BufferedNode(nodeName, params),
	m_needNewTarget(true)
	{
		m_targetInID = addInput("IN_TARGET");
		m_featuresInID = addInput("IN_FEATURES_VECTOR");
		m_meanParticleInID = addInput("IN_MEAN_STATE");
		m_ppCompletedInID = addInput("PREPROCESS_COMPLETED");
		m_filtFinishedInID = addInput("FILTERING_FINISHED");
		
		m_particleOutID = addOutput("OUT_PARTICLE");
		m_likelihoodOutID = addOutput("OUT_LIKELIHOOD");
		m_targetOutID = addOutput("OUT_TARGET");
		m_finishedOutID = addOutput("TRACKING_FINISHED");
		
		m_useScale = dereference_cast<bool>(parameters.get("USE_SCALE"));
		m_useAngle = dereference_cast<bool>(parameters.get("USE_ROTATION_ANGLE"));
		
		// First 2 elements of state is center position x,y
		m_stateSize = 2;
		
		if (m_useScale) {
			// Add scale state which is described by the half-width and half-height of ROI
			m_stateSize += 2;
		}
		
		if (m_useAngle) {
			// Add rotation angle state
			m_stateSize += 1;
		}
		
		// Allocate particle
		m_curSample = RCPtr<PFGenericParticle>(new PFGenericParticle(m_stateSize));
	}
	
	VisualTargetPFInterface::~VisualTargetPFInterface()
	{
	
	}
	
	// Modified BufferedNode request method to support cyclic node connection
	void VisualTargetPFInterface::request(int output_id, const ParameterSet &req) 
	{
		if (req.exist("LOOKAHEAD")) {
			outputs[output_id].lookAhead = max(outputs[output_id].lookAhead,dereference_cast<int> (req.get("LOOKAHEAD")));
		}
		
		if (req.exist("LOOKBACK")) {
			outputs[output_id].lookBack = max(outputs[output_id].lookBack,dereference_cast<int> (req.get("LOOKBACK")));
		}
		
		if (req.exist("INORDER")) {
			inOrder = true;
		}
		
		int outputLookAhead=0, outputLookBack=0;
	
		outputLookAhead=max(outputLookAhead, outputs[output_id].lookAhead);
		outputLookBack =max(outputLookBack, outputs[output_id].lookBack);
		
		if (output_id == m_particleOutID) {
			// OUT_PARTICLE output does not required any inputs (if flow is respected)
			return;
			
		}
		else if (output_id == m_likelihoodOutID) {
			// Likelihood output requires features vector
			ParameterSet myReq;
			
			myReq.add("LOOKAHEAD", ObjectRef(Int::alloc(inputsCache[m_featuresInID].lookAhead+outputLookAhead)));
			myReq.add("LOOKBACK", ObjectRef(Int::alloc(inputsCache[m_featuresInID].lookBack+outputLookBack)));
			inputs[m_featuresInID].node->request(inputs[m_featuresInID].outputID,myReq);
			
			return;
		}
		else if (output_id == m_targetOutID) {
			ParameterSet myReq, myReq2, myReq3;
			/*
			myReq.add("LOOKAHEAD", ObjectRef(Int::alloc(inputsCache[m_targetInID].lookAhead+outputLookAhead)));
			myReq.add("LOOKBACK", ObjectRef(Int::alloc(inputsCache[m_targetInID].lookBack+outputLookBack)));
			inputs[m_targetInID].node->request(inputs[m_targetInID].outputID,myReq);
			*/
			myReq2.add("LOOKAHEAD", ObjectRef(Int::alloc(inputsCache[m_meanParticleInID].lookAhead+outputLookAhead)));
			myReq2.add("LOOKBACK", ObjectRef(Int::alloc(inputsCache[m_meanParticleInID].lookBack+outputLookBack)));
			inputs[m_meanParticleInID].node->request(inputs[m_meanParticleInID].outputID,myReq2);
			
			myReq3.add("LOOKAHEAD", ObjectRef(Int::alloc(inputsCache[m_ppCompletedInID].lookAhead+outputLookAhead)));
			myReq3.add("LOOKBACK", ObjectRef(Int::alloc(inputsCache[m_ppCompletedInID].lookBack+outputLookBack)));
			inputs[m_ppCompletedInID].node->request(inputs[m_ppCompletedInID].outputID,myReq3);
		}
		else if (output_id == m_finishedOutID) {
			ParameterSet myReq, myReq2;
			
			myReq.add("LOOKAHEAD", ObjectRef(Int::alloc(inputsCache[m_filtFinishedInID].lookAhead+outputLookAhead)));
			myReq.add("LOOKBACK", ObjectRef(Int::alloc(inputsCache[m_filtFinishedInID].lookBack+outputLookBack)));
			inputs[m_filtFinishedInID].node->request(inputs[m_filtFinishedInID].outputID,myReq);
			
			myReq2.add("LOOKAHEAD", ObjectRef(Int::alloc(inputsCache[m_targetInID].lookAhead+outputLookAhead)));
			myReq2.add("LOOKBACK", ObjectRef(Int::alloc(inputsCache[m_targetInID].lookBack+outputLookBack)));
			inputs[m_targetInID].node->request(inputs[m_targetInID].outputID,myReq2);
		}
		else {
			throw new GeneralException ("VisualTargetPFInterface::request : unknown output ID.",__FILE__,__LINE__);
		}
	}
	
	void VisualTargetPFInterface::calculate(int output_id, int count, Buffer &out)
	{
		try {
			if (output_id == m_finishedOutID) {
				if (m_needNewTarget) {
					// Get current target
					ObjectRef targetObjRef = getInput(m_targetInID, count);
					
					if (!targetObjRef->isNil()) {
						m_curTarget = RCPtr<VisualTarget<double> >(targetObjRef);
						
						if (!m_curTarget->GetCstROI()) {
							throw new GeneralException ("VisualTargetPFInterface::calculate : current VisualTarget has not a valid VisualROI member.",__FILE__,__LINE__);
						}
						
						// Force features preprocessing since we should be 
						// starting a tracking iteration
						RCPtr<Vector<int> > preprocessRef = getInput(m_ppCompletedInID, count);
						
						// Set current sample according to target's ROI
						const VisualROI *roiRef = m_curTarget->GetCstROI();
						float *p_state = m_curSample->GetState();
						
						// Set position
						*p_state++ = (float)(roiRef->GetXCen());
						*p_state++ = (float)(roiRef->GetYCen());
						
						if (m_useScale) {
							// Add scale state
							*p_state++ = (float)(roiRef->GetHSX());
							*p_state++ = (float)(roiRef->GetHSY());
						}
						
						if (m_useAngle) {
							// Add rotation angle state
							*p_state++ = (float)(roiRef->GetAngle());
						}
						
						m_needNewTarget = false;
					}
				}
				
				if (m_needNewTarget) {
					(*outputs[m_finishedOutID].buffer)[count] = ObjectRef(Bool::alloc(false));
				}
				else {
					// See if particle filtering has finished
					bool filtFinished = dereference_cast<bool>(getInput(m_filtFinishedInID, count));
					(*outputs[m_finishedOutID].buffer)[count] = ObjectRef(Bool::alloc(filtFinished));
				}
			}
			else if (output_id == m_likelihoodOutID) {
				// Get features vector
				ObjectRef featObjRef = getInput(m_featuresInID, count);
				
				if (!featObjRef->isNil()) {
					RCPtr<Vector<VisualFeatureDesc<double> *> > featRef = featObjRef;
					
					// Use current reference target to estimate the 
					// likelihood of the current features vector
					float sim = m_curTarget->Similarity(featRef.get());
					float likelihood = exp(20.0*(sim-1.f));
					//cout << "Likelihood = " << likelihood << endl;
					
					// Output current likehood
					(*outputs[m_likelihoodOutID].buffer)[count] = ObjectRef(Float::alloc(likelihood));
				}
				else {
					throw new GeneralException ("VisualTargetPFInterface::calculate : invalid (nilObject) IN_FEATURES_VECTOR input.",__FILE__,__LINE__);
				}
			}
			else if (output_id == m_particleOutID) {
				// Simply output current particle reference that should be valid
				(*outputs[m_particleOutID].buffer)[count] = ObjectRef(m_curSample);
			}
			else if (output_id == m_targetOutID) {
				if (m_needNewTarget) {
					// Output nilObject since tracking is just starting
					(*outputs[m_targetOutID].buffer)[count] = nilObject;
				}
				else {
					// Get mean state particle
					ObjectRef particleObjRef = getInput(m_meanParticleInID, count);
				
					if (!particleObjRef->isNil()) {
						RCPtr<PFGenericParticle> meanParticleRef = particleObjRef;
						
						VisualROI *curROI = m_curTarget->GetROI();
						
						// Set current target's ROI according to mean state particle
						const float *p_state = meanParticleRef->GetCstState();
						// Set position
						curROI->SetXCen((int)(*p_state++));
						curROI->SetYCen((int)(*p_state++));
						
						if (m_useScale) {
							// Set scale
							curROI->SetHSX((int)(*p_state++));
							curROI->SetHSY((int)(*p_state++));
						}
						
						if (m_useAngle) {
							// Set rotation angle state
							curROI->SetAngle((int)(*p_state++));
						}
						
						// Set flag to indicate we need a new target
						m_needNewTarget = true;
						
						// Output resulting target
						(*outputs[m_targetOutID].buffer)[count] = ObjectRef(m_curTarget);
					}
					else {
						// Output nilObject since tracking is not finished
						(*outputs[m_targetOutID].buffer)[count] = nilObject;
					}
				}
			}
		}
		catch (BaseException *e) {
			throw e->add(new GeneralException("Exception in VisualTargetPFInterface::calculate:",__FILE__,__LINE__));
		}
	}

private:
	// Input IDs
	int m_targetInID;
	int m_featuresInID;
	int m_meanParticleInID;
	int m_ppCompletedInID;
	int m_filtFinishedInID;
	
	// Output IDs
	int m_particleOutID;
	int m_likelihoodOutID;
	int m_targetOutID;
	int m_finishedOutID;
	
	// Parameters
	bool m_useScale;
	bool m_useAngle;
	
	unsigned int m_stateSize;
	bool m_needNewTarget;
	RCPtr<VisualTarget<double> > m_curTarget;
	RCPtr<PFGenericParticle> m_curSample;
};

}

#endif

--- NEW FILE: PFPMRandomWalk.cc ---
/* Copyright (C) 2005 Pierre Moisan (Pie...@US...) 

   This library is free software; you can redistribute it and/or
   modify it under the terms of the GNU Lesser General Public
   License as published by the Free Software Foundation; either
   version 2.1 of the License, or (at your option) any later version.
   
   This library is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
   Lesser General Public License for more details.
   
   You should have received a copy of the GNU Lesser General Public
   License along with this library; if not, write to the Free Software
   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#include "PFPMRandomWalk.h"

using namespace std;
using namespace FD;

namespace RobotFlow {

DECLARE_NODE(PFPMRandomWalk)
DECLARE_TYPE(PFPMRandomWalk)

  /*Node
   *
   * @name PFPMRandomWalk
   * @category RobotFlow:Vision:Tracking
   * @description Random walk prediction model for particle filters.
   *
   * @input_name NOISE_VARIANCE
   * @input_type Vector<float>
   * @input_description Noise variance for each of the particle state.
   *
   * @input_name CURRENT_PARTICLE
   * @input_type PFGenericParticle
   * @input_description Current particle (sample) to apply prediction model.
   *
   * @output_name PREDICT_COMPLETED
   * @output_type bool
   * @output_description Flag indicating the success of the prediction.
   *
   END*/

PFPMRandomWalk::PFPMRandomWalk()
: PFPredictionModel(e_PFPM_RandomWalk),
m_init(false),
m_stateSize(0),
m_noiseVariance(NULL)
{ 
}
	
PFPMRandomWalk::PFPMRandomWalk(const Vector<float> *i_variance) 
: PFPredictionModel(e_PFPM_RandomWalk),
m_init(false),
m_stateSize(0),
m_noiseVariance(NULL)
{ 
	Initialize(i_variance);
}
	
PFPMRandomWalk::PFPMRandomWalk(string nodeName, ParameterSet params)
: PFPredictionModel(nodeName, params),
m_init(false),
m_stateSize(0),
m_noiseVariance(NULL)
{
	SetType(e_PFPM_RandomWalk);
	
	m_varianceInID = addInput("NOISE_VARIANCE");
	m_particleInID = addInput("CURRENT_PARTICLE");
	
	m_completedOutID = addOutput("PREDICT_COMPLETED");
}

PFPMRandomWalk::~PFPMRandomWalk()
{
	if (m_noiseVariance) {
		m_noiseVariance->destroy();
	}
}

void PFPMRandomWalk::calculate(int output_id, int count, Buffer &out)
{
	try {
		// Get NOISE_VARIANCE only once for intialization
		if (!m_init) {
			ObjectRef varObjRef = getInput(m_varianceInID, count);
			
			if (!varObjRef->isNil()) {
				RCPtr<Vector<float> > varRef = varObjRef;
				
				Initialize(&(*varRef));
			}
			else {
				throw new GeneralException ("PFPMRandomWalk::calculate : invalid (nilObject) NOISE_VARIANCE input.",__FILE__,__LINE__);
			}
		}
		
		// Get current particle to apply prediction model to
		ObjectRef sampleObjRef = getInput(m_particleInID, count);
		
		if (!sampleObjRef->isNil()) {
			RCPtr<PFGenericParticle> sampleRef = sampleObjRef;
			
			// Particle is modified by reference
			Predict(sampleRef.get());
		}
		else {
			throw new GeneralException ("PFPMRandomWalk::calculate : invalid (nilObject) CURRENT_PARTICLE input.",__FILE__,__LINE__);
		}
		
		// Output true indicating prediction was correctly applied
		(*outputs[m_completedOutID].buffer)[count] = ObjectRef(Bool::alloc(true));
	}
	catch (BaseException *e) {
		throw e->add(new GeneralException("Exception in PFPMRandomWalk::calculate:",__FILE__,__LINE__));
	}
}

void PFPMRandomWalk::Predict(PFParticle *io_sample)
{
	try {
		float *p_state;
		
		// Check for current particle sanity
		if (m_stateSize != io_sample->GetStateSize()) {
			throw new GeneralException ("PFPMRandomWalk::Predict : current particle state size differs from variance vector size.",__FILE__,__LINE__);
		}
		
		p_state = io_sample->GetState();
		
		if (!p_state) {
			throw new GeneralException ("PFPMRandomWalk::Predict : current particle has an uninitialized state.",__FILE__,__LINE__);
		}
		
		for (int i=0; i<m_stateSize; i++) {
			*p_state = *p_state * (1.f + (PFUTIL_randn()-0.5f)*(*m_noiseVariance)[i]);
			p_state++;
		}
	}
	catch (BaseException *e) {
		throw e->add(new GeneralException("Exception in PFPMRandomWalk::Predict:",__FILE__,__LINE__));
	}
}

void PFPMRandomWalk::Initialize(const Vector<float> *i_variance)
{
	try {
		m_stateSize = i_variance->size();
		m_noiseVariance = Vector<float>::alloc(m_stateSize);
		
		for (int i=0; i<m_stateSize; i++) {
			(*m_noiseVariance)[i] = (*i_variance)[i];
		}
		
		m_init = true;
	}
	catch (BaseException *e) {
		throw e->add(new GeneralException("Exception in PFPMRandomWalk::Initialize:",__FILE__,__LINE__));
	}
}

}

--- NEW FILE: DrawPFParticle.cc ---
/* Copyright (C) 2005 Pierre Moisan (Pie...@US...) 

   This library is free software; you can redistribute it and/or
   modify it under the terms of the GNU Lesser General Public
   License as published by the Free Software Foundation; either
   version 2.1 of the License, or (at your option) any later version.
   
   This library is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
   Lesser General Public License for more details.
   
   You should have received a copy of the GNU Lesser General Public
   License along with this library; if not, write to the Free Software
   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#ifndef _DRAWPFPARTICLE_CC_
#define _DRAWPFPARTICLE_CC_

#include "BufferedNode.h"
#include "VisualROI.h"
#include "Image.h"

using namespace std;
using namespace FD;

namespace RobotFlow {

class DrawPFParticle;
DECLARE_NODE(DrawPFParticle)

  /*Node
   *
   * @name DrawPFParticle
   * @category RobotFlow:Vision:Tracking
   * @description Draws particle state as a visual ROI in current image.
   *
   * @parameter_name FRAME_WIDTH
   * @parameter_type int
   * @parameter_value 320
   * @parameter_description Video frame width.
   *
   * @parameter_name FRAME_HEIGHT
   * @parameter_type int
   * @parameter_value 240
   * @parameter_description Video frame height.
   *
   * @parameter_name NUM_CHANNELS
   * @parameter_type int
   * @parameter_value 3
   * @parameter_description Number of channels in video frame.
   *
   * @input_name IN_IMAGE
   * @input_type Image
   * @input_description Current image to use.
   *
   * @input_name IN_ROI
   * @input_type VisualROI
   * @input_description Current particle to draw.
   *
   * @output_name OUT_IMAGE
   * @output_type Image
   * @output_description Resulting image with particles drawn.
   *
   * @output_name OUT_ROI
   * @output_type VisualROI
   * @output_description Current ROI.
   *
   * @output_name PREPROCESS_COMPLETED
   * @output_type int
   * @output_description Output to force preprocessing.
   *
   END*/

class DrawPFParticle : public BufferedNode
{
public:
	DrawPFParticle::DrawPFParticle(string nodeName, ParameterSet params)
	: BufferedNode(nodeName, params)
	{
		m_imageInID = addInput("IN_IMAGE");
		m_roiInID = addInput("IN_ROI");
		
		m_imageOutID = addOutput("OUT_IMAGE");
		m_roiOutID = addOutput("OUT_ROI");
		m_ppCompletedOutID = addOutput("PREPROCESS_COMPLETED");
		
		m_width = dereference_cast<int>(parameters.get("FRAME_WIDTH"));
		m_height = dereference_cast<int>(parameters.get("FRAME_HEIGHT"));
		m_numChannels = dereference_cast<int>(parameters.get("NUM_CHANNELS"));
		
		m_numPixels = m_width*m_height;
		m_numBytesInFrame = m_numPixels*m_numChannels;
		
		m_curImage = RCPtr<Image>(Image::alloc(m_width, m_height, m_numChannels));
		
		m_color = new unsigned char[m_numChannels];
		for (int c=0; c<m_numChannels; c++) {
			m_color[c] = 128;
		}
	}
	
	DrawPFParticle::~DrawPFParticle()
	{
		delete [] m_color;
	}
	
	// Modified BufferedNode request method to support cyclic node connection
	void DrawPFParticle::request(int output_id, const ParameterSet &req) 
	{
		if (req.exist("LOOKAHEAD")) {
			outputs[output_id].lookAhead = max(outputs[output_id].lookAhead,dereference_cast<int> (req.get("LOOKAHEAD")));
		}
		
		if (req.exist("LOOKBACK")) {
			outputs[output_id].lookBack = max(outputs[output_id].lookBack,dereference_cast<int> (req.get("LOOKBACK")));
		}
		
		if (req.exist("INORDER")) {
			inOrder = true;
		}
		
		int outputLookAhead=0, outputLookBack=0;
	
		outputLookAhead=max(outputLookAhead, outputs[output_id].lookAhead);
		outputLookBack =max(outputLookBack, outputs[output_id].lookBack);
		
		if (output_id == m_imageOutID) {
			return;
		}
		else if (output_id == m_roiOutID) {
			ParameterSet myReq;
			myReq.add("LOOKAHEAD", ObjectRef(Int::alloc(inputsCache[m_roiInID].lookAhead+outputLookAhead)));
			myReq.add("LOOKBACK", ObjectRef(Int::alloc(inputsCache[m_roiInID].lookBack+outputLookBack)));
			inputs[m_roiInID].node->request(inputs[m_roiInID].outputID,myReq);
		}
		else if (output_id == m_ppCompletedOutID) {
			ParameterSet myReq;
			myReq.add("LOOKAHEAD", ObjectRef(Int::alloc(inputsCache[m_imageInID].lookAhead+outputLookAhead)));
			myReq.add("LOOKBACK", ObjectRef(Int::alloc(inputsCache[m_imageInID].lookBack+outputLookBack)));
			inputs[m_imageInID].node->request(inputs[m_imageInID].outputID, myReq);
		}
		else {
			throw new GeneralException ("DrawPFParticle::request : unknown output ID.",__FILE__,__LINE__);
		}
	
	}
	
	void DrawPFParticle::calculate(int output_id, int count, Buffer &out)
	{
		try {
			if (output_id == m_roiOutID) {
				// Get current particle to convert
				ObjectRef roiObjRef = getInput(m_roiInID, count);
				
				if (!roiObjRef->isNil()) {
					RCPtr<VisualROI> roiRef = RCPtr<VisualROI>(roiObjRef);
					
					// Draw particle
					roiRef->DrawROI(m_width, m_height, m_numChannels, m_curImage->get_data(), m_color);
				}
				
				// Output the input ROI
				(*outputs[m_roiOutID].buffer)[count] = roiObjRef;
			}
			else if (output_id == m_ppCompletedOutID) {
				// Get a new copy of current frame
				RCPtr<Image> imageRef = getInput(m_imageInID, count);
		
				// Verify input image sanity
				if (imageRef->get_width() != m_width ||
					imageRef->get_height() != m_height ||
					imageRef->get_pixelsize() != m_numChannels) {
					throw new GeneralException ("DrawPFParticle::calculate : image parameters do not correspond to given input.",__FILE__,__LINE__);
				}
				
				// Copy input image
				memcpy(m_curImage->get_data(), imageRef->get_data(), m_numBytesInFrame);
				
				(*outputs[m_ppCompletedOutID].buffer)[count] = ObjectRef(Int::alloc(1));
			}
			else if (output_id == m_imageOutID) {
				// Allocate output image
				Image *imageOut = Image::alloc(m_width, m_height, m_numChannels);
			
				// Copy image
				memcpy(imageOut->get_data(), m_curImage->get_data(), m_numBytesInFrame);
				(*outputs[m_imageOutID].buffer)[count] = ObjectRef(imageOut);
			}
		}
		catch (BaseException *e) {
			throw e->add(new GeneralException("Exception in DrawPFParticle::calculate:",__FILE__,__LINE__));
		}
	}

private:
	// Input IDs
	int m_imageInID;
	int m_roiInID;
	
	// Output IDs
	int m_imageOutID;
	int m_ppCompletedOutID;
	int m_roiOutID;
	
	// Parameters
	int m_width;
	int m_height;
	int m_numChannels;
	int m_numPixels;
	int m_numBytesInFrame;
	
	unsigned char *m_color;
	
	RCPtr<Image> m_curImage;
};

}

#endif

--- NEW FILE: PFState2VisualROI.cc ---
/* Copyright (C) 2005 Pierre Moisan (Pie...@US...) 

   This library is free software; you can redistribute it and/or
   modify it under the terms of the GNU Lesser General Public
   License as published by the Free Software Foundation; either
   version 2.1 of the License, or (at your option) any later version.
   
   This library is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
   Lesser General Public License for more details.
   
   You should have received a copy of the GNU Lesser General Public
   License along with this library; if not, write to the Free Software
   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#ifndef _PFSTATE2VISUALROI_CC_
#define _PFSTATE2VISUALROI_CC_

#include "BufferedNode.h"
#include "VisualROI.h"
#include "PFGenericParticle.h"

using namespace std;
using namespace FD;

namespace RobotFlow {

class PFState2VisualROI;
DECLARE_NODE(PFState2VisualROI)

  /*Node
   *
   * @name PFState2VisualROI
   * @category RobotFlow:Vision:Tracking
   * @description Conversion from a particle filter sample (dynamic state) to a VisualROI object.
   *
   * @parameter_name USE_SCALE
   * @parameter_type bool
   * @parameter_value true
   * @parameter_description Flag indicating to use the ROI scale (width and height) in the particle's state.
   *
   * @parameter_name USE_ROTATION_ANGLE
   * @parameter_type bool
   * @parameter_value false
   * @parameter_description Flag indicating to use the ROI rotation angle in the particle's state.
   *
   * @input_name IN_PARTICLE
   * @input_type PFGenericParticle
   * @input_description Current ROI to convert.
   *
   * @output_name OUT_ROI
   * @output_type VisualROI
   * @output_description Resulting particle with appropriate state.
   *
   END*/

class PFState2VisualROI : public BufferedNode
{
public:
	PFState2VisualROI::PFState2VisualROI(string nodeName, ParameterSet params)
	: BufferedNode(nodeName, params)
	{
		m_particleInID = addInput("IN_PARTICLE");
		
		m_roiOutID = addOutput("OUT_ROI");
		
		m_useScale = dereference_cast<bool>(parameters.get("USE_SCALE"));
		m_useAngle = dereference_cast<bool>(parameters.get("USE_ROTATION_ANGLE"));
		
		// First 2 elements of state is center position x,y
		m_stateSize = 2;
		
		if (m_useScale) {
			// Add scale state which is described by the half-width and half-height of ROI
			m_stateSize += 2;
		}
		
		if (m_useAngle) {
			// Add rotation angle state
			m_stateSize += 1;
		}
		
		// Allocate ROI
		m_curROI = RCPtr<VisualROI>(new VisualROI());
		m_curROI->SetType(e_VISUALROI_elliptical);
	}
	
	PFState2VisualROI::~PFState2VisualROI()
	{
		
	}
	
	void PFState2VisualROI::calculate(int output_id, int count, Buffer &out)
	{
		try {
			// Get current particle to convert
			ObjectRef particleObjRef = getInput(m_particleInID, count);
			
			if (!particleObjRef->isNil()) {
				RCPtr<PFGenericParticle> particleRef = RCPtr<PFGenericParticle>(particleObjRef);
				
				// Set ROI according to particle state
				const float *p_state = particleRef->GetCstState();
				// Set position
				m_curROI->SetXCen((int)(*p_state++));
				m_curROI->SetYCen((int)(*p_state++));
				
				if (m_useScale) {
					// Set scale
					m_curROI->SetHSX((int)(*p_state++));
					m_curROI->SetHSY((int)(*p_state++));
				}
				
				if (m_useAngle) {
					// Set rotation angle state
					m_curROI->SetAngle((int)(*p_state++));
				}
				
				// Output particle
				(*outputs[m_roiOutID].buffer)[count] = ObjectRef(m_curROI);
			}
			else {
				throw new GeneralException ("PFState2VisualROI::calculate : invalid (nilObject) IN_PARTICLE input.",__FILE__,__LINE__);
			}
		}
		catch (BaseException *e) {
			throw e->add(new GeneralException("Exception in PFState2VisualROI::calculate:",__FILE__,__LINE__));
		}
	}

private:
	// Input IDs
	int m_particleInID;
	
	// Output IDs
	int m_roiOutID;
	
	// Parameters
	bool m_useScale;
	bool m_useAngle;
	
	unsigned int m_stateSize;
	
	RCPtr<VisualROI> m_curROI;
};

}

#endif