[Robotflow-cvs] RobotFlow/Vision/include PFGenericParticle.h,NONE,1.1 PFGenericParticleFilter.h,NONE,1.1 PFPMRandomWalk.h,NONE,1.1 PFParticle.h,NONE,1.1 PFParticleFilter.h,NONE,1.1 PFPredictionModel.h,NONE,1.1 PFUtilityFct.h,NONE,1.1

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

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

Added Files:
	PFGenericParticle.h PFGenericParticleFilter.h PFPMRandomWalk.h 
	PFParticle.h PFParticleFilter.h PFPredictionModel.h 
	PFUtilityFct.h 
Log Message:
Generic particle filter implementation for visual tracking purposes

--- NEW FILE: PFParticle.h ---
/* 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 _PFPARTICLE_H_
#define _PFPARTICLE_H_

#include "Object.h"
#include <iostream>

namespace RobotFlow {

typedef enum
{
	e_PFP_Generic = 0,
	e_PFP_Unknown
} e_PFP_type;

//
// This should be an abstract base class. Since many containers (like Vector)
// do not allow abstract functions, each abstract function throws an exception
// to avoid the direct use of the base class "abstract" routines.
//
class PFParticle : public FD::Object
{
	friend std::ostream &operator<<(std::ostream &o_out, const PFParticle &i_ref)
	{
		try {
			i_ref.printOn(o_out);
			// Enable cascading
			return o_out;
		}
		catch (FD::BaseException *e) {
			throw e->add(new FD::GeneralException("Exception in PFParticle::operator<<:",__FILE__,__LINE__));
		}
	}
	
	friend std::istream &operator>>(std::istream &i_in, PFParticle &o_ref)
	{
		try {
			o_ref.readFrom(i_in);
			// Enable cascading
			return i_in;
		}
		catch (FD::BaseException *e) {
			throw e->add(new FD::GeneralException("Exception in PFParticle::operator>>:",__FILE__,__LINE__));
		}
	}
	
public:
	PFParticle()
	: m_pfpType(e_PFP_Unknown)
	{ 
	}
	
	PFParticle(e_PFP_type i_pfpType) 
	: m_pfpType(i_pfpType)
	{ 
	}
	
	PFParticle(const PFParticle &i_ref) 
	{ 
		m_pfpType = i_ref.m_pfpType;
	}

	virtual ~PFParticle()
	{
	
	}

	virtual PFParticle & operator =(const PFParticle &i_ref)
	{
		// Avoid self assignment
		if (&i_ref != this) {
			this->m_pfpType = i_ref.m_pfpType;
		}
		
		return *this;
	}
	
	virtual PFParticle* clone()  const
	{
		return new PFParticle(*this);
	}

	// Default routine to print a PFParticle object to an output stream
	virtual void printOn(std::ostream &out) const
	{
		throw new FD::GeneralException("Exception in PFParticle::printOn: cannot use base class routine.",__FILE__,__LINE__);
	}

	// Default routine to read a PFParticle object from an input stream
	virtual void readFrom(std::istream &in)
	{
		throw new FD::GeneralException("Exception in PFParticle::readFrom: cannot use base class routine.",__FILE__,__LINE__);
	}
	
	e_PFP_type GetType() const 
	{ 
		return m_pfpType; 
	}
	
	void SetType(e_PFP_type i_type)
	{ 
		m_pfpType = i_type; 
	}
	
	virtual unsigned int GetStateSize() const
	{
		throw new FD::GeneralException("Exception in PFParticle::GetStateSize: cannot use base class routine.",__FILE__,__LINE__);
	}
	
	virtual float *GetState()
	{
		throw new FD::GeneralException("Exception in PFParticle::GetState: cannot use base class routine.",__FILE__,__LINE__);
	}
	
	virtual const float *GetCstState() const 
	{
		throw new FD::GeneralException("Exception in PFParticle::GetCstState: cannot use base class routine.",__FILE__,__LINE__);
	}
	
	virtual float GetStateIdx(int i_idx) const
	{
		throw new FD::GeneralException("Exception in PFParticle::GetStateIdx: cannot use base class routine.",__FILE__,__LINE__);
	}
	
	virtual float GetWeight() const
	{
		throw new FD::GeneralException("Exception in PFParticle::GetWeight: cannot use base class routine.",__FILE__,__LINE__);
	}
	
	virtual void SetStateSize(unsigned int i_size) 
	{
		throw new FD::GeneralException("Exception in PFParticle::SetStateSize: cannot use base class routine.",__FILE__,__LINE__);
	}
	
	virtual void SetState(const float *i_state) 
	{
		throw new FD::GeneralException("Exception in PFParticle::SetState: cannot use base class routine.",__FILE__,__LINE__);
	}
	
	virtual void SetStateIdx(int i_idx, float i_val)
	{
		throw new FD::GeneralException("Exception in PFParticle::SetStateIdx: cannot use base class routine.",__FILE__,__LINE__);
	}
	
	virtual void SetWeight(float i_weight) 
	{
		throw new FD::GeneralException("Exception in PFParticle::SetWeight: cannot use base class routine.",__FILE__,__LINE__);
	}

private:
	e_PFP_type m_pfpType;
};

}

#endif

--- NEW FILE: PFPMRandomWalk.h ---
/* 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 _PFPMRANDOMWALK_H_
#define _PFPMRANDOMWALK_H_

#include "PFPredictionModel.h"
#include "PFGenericParticle.h"
#include "PFUtilityFct.h"
#include "Vector.h"

namespace RobotFlow {
//
// Random walk prediction model for particle filters
//
class PFPMRandomWalk : public PFPredictionModel
{
public:
	PFPMRandomWalk();
	
	PFPMRandomWalk(const FD::Vector<float> *i_variance);
	
	PFPMRandomWalk(std::string nodeName, FD::ParameterSet params);

	virtual ~PFPMRandomWalk();
	
	virtual void calculate(int output_id, int count, FD::Buffer &out);
	
	virtual void Predict(PFParticle *io_sample);
	
	void Initialize(const FD::Vector<float> *i_variance);
	
private:
	// BufferedNode inputs
	int m_varianceInID;
	int m_particleInID;
	
	// BufferedNode outputs
	int m_completedOutID;
	
	bool m_init;
	unsigned int m_stateSize;
	FD::Vector<float> *m_noiseVariance;
};

}

#endif

--- NEW FILE: PFPredictionModel.h ---
/* 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 _PFPREDICTIONMODEL_H_
#define _PFPREDICTIONMODEL_H_

#include "BufferedNode.h"
#include "PFParticle.h"

namespace RobotFlow {

typedef enum
{
	e_PFPM_RandomWalk = 0,
	e_PFPM_Unknown
} e_PFPM_type;

//
// Abstract base class for particle filter prediction models
//
class PFPredictionModel : public FD::BufferedNode
{
public:
	PFPredictionModel()
	: m_modelType(e_PFPM_Unknown)
	{ 
	}
	
	PFPredictionModel(e_PFPM_type i_modelType) 
	: m_modelType(i_modelType)
	{ 
	}
	
	PFPredictionModel(std::string nodeName, FD::ParameterSet params)
	: FD::BufferedNode(nodeName, params)
	{
	
	}

	virtual ~PFPredictionModel()
	{
	
	}
	
	e_PFPM_type GetType() const
	{ 
		return m_modelType; 
	}
	
	void SetType(e_PFPM_type i_type)
	{
		m_modelType = i_type;
	}
	
	virtual void calculate(int output_id, int count, FD::Buffer &out) = 0;
	
	virtual void Predict(PFParticle *io_sample) = 0;
	
private:
	e_PFPM_type m_modelType;
};

}

#endif

--- NEW FILE: PFGenericParticle.h ---
/* 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 _PFGENERICPARTICLE_H_
#define _PFGENERICPARTICLE_H_

#include "PFParticle.h"
#include <iostream>

namespace RobotFlow {

class PFGenericParticle : public PFParticle
{
public:
	PFGenericParticle();
	
	PFGenericParticle(unsigned int i_stateSize);
	
	PFGenericParticle(const PFGenericParticle &i_ref);

	virtual ~PFGenericParticle();

	virtual PFGenericParticle & operator =(const PFGenericParticle &i_ref);
	
	virtual PFGenericParticle* clone()  const;

	// Default routine to print a PFGenericParticle object to an output stream
	virtual void printOn(std::ostream &out) const;

	// Default routine to read a PFGenericParticle object from an input stream
	virtual void readFrom(std::istream &in);
	
	virtual unsigned int GetStateSize() const;
	
	virtual float *GetState();
	
	virtual float GetStateIdx(int i_idx) const;
	
	virtual const float *GetCstState() const;
	
	virtual float GetWeight() const;
	
	virtual void SetStateSize(unsigned int i_size);
	
	virtual void SetState(const float *i_state);
	
	virtual void SetStateIdx(int i_idx, float i_val);
	
	virtual void SetWeight(float i_weight);

private:
	int m_stateSize;
	float *m_state;
	float m_weight;
};

}

#endif

--- NEW FILE: PFGenericParticleFilter.h ---
/* 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 _PFGENERICPARTICLEFILTER_H_
#define _PFGENERICPARTICLEFILTER_H_

#include "PFParticleFilter.h"
#include "PFGenericParticle.h"

namespace RobotFlow {
//
// Generic implementation of a particle filter
// For more information, refer to the following publication
// M. S. Arulampalam, S. Maskell, N. Gordon, T. Clapp.
// "A Tutorial on Particle Filters for On-line Nonlinear/Non-Gaussian 
// Bayesian Tracking"  (2001) 
//
class PFGenericParticleFilter : public PFParticleFilter
{
public:
	PFGenericParticleFilter();
	
	PFGenericParticleFilter(unsigned int i_numSamples, 
		unsigned int i_sampleStateSize, const FD::Vector<float> *i_initVariance);
	
	PFGenericParticleFilter(std::string nodeName, FD::ParameterSet params);

	virtual ~PFGenericParticleFilter();

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

	// Default routine to read a PFGenericParticleFilter object from an input stream
	void readFrom(std::istream &in)
	{
		throw new FD::GeneralException("Exception in PFGenericParticleFilter::printOn: method not yet implemented.",__FILE__,__LINE__);
	}
	
	virtual void request(int output_id, const FD::ParameterSet &req);
	
	void calculate(int output_id, int count, FD::Buffer &out);
	
	void Initialize(const FD::Vector<float> *i_variance);
	
	void InitSamples();
	
	void Update();
	
	void ComputeMeanState();
	
	void Resample();

private:
	void CopyTmpSamples();

private:
	// Input IDs (for BufferedNode)
	int m_initVarianceInID;
	int m_refMeanStateInID;
	int m_predictInID;
	int m_likelihoodInID;
	
	// Output IDs (for BufferedNode)
	int m_finishedOutID;
	int m_particleOutID;
	int m_meanStateOutID;
	
	bool m_init;
	bool m_initPF;
	bool m_initSamples;
	bool m_finished;
	
	int m_curSampleIdx;
	unsigned int m_numSamples;
	unsigned int m_sampleStateSize;
	FD::RCPtr<PFGenericParticle> *m_samples;
	FD::RCPtr<PFGenericParticle> *m_tmpSamples;
	float m_likelihoodsSum;
	float *m_cumulWeight;
	FD::Vector<float> *m_initVariance;
	
	FD::RCPtr<PFGenericParticle> m_curSample;
	FD::RCPtr<PFGenericParticle> m_refMeanState;
	FD::RCPtr<PFGenericParticle> m_outMeanState;
};

}

#endif

--- NEW FILE: PFParticleFilter.h ---
/* 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 _PFPARTICLEFILTER_H_
#define _PFPARTICLEFILTER_H_

#include "VisualTracker.h"
#include "PFParticle.h"
#include "PFUtilityFct.h"

namespace RobotFlow {

typedef enum
{
	e_PF_GenericFilter = 0,
	e_PF_Unknown
} e_PF_type;

//
// Abstract base class for particle filter
//

class PFParticleFilter : public VisualTracker
{
public:
	PFParticleFilter()
	: m_filterType(e_PF_Unknown)
	{
	
	}
	
	PFParticleFilter(e_PF_type i_modelType) 
	: m_filterType(i_modelType)
	{ 
	}
	
	PFParticleFilter(std::string nodeName, FD::ParameterSet params)
	: VisualTracker(nodeName, params)
	{
	
	}

	virtual ~PFParticleFilter()
	{
	
	}

	// Default routine to print a PFParticleFilter object to an output stream
	void printOn(std::ostream &out) const = 0;

	// Default routine to read a PFParticleFilter object from an input stream
	void readFrom(std::istream &in) = 0;
	
	void calculate(int output_id, int count, FD::Buffer &out) = 0;
	
	e_PF_type GetType() const
	{ 
		return m_filterType; 
	}
	
	void SetType(e_PF_type i_type)
	{
		m_filterType = i_type;
	}

private:
	e_PF_type m_filterType;
};

}

#endif

--- NEW FILE: PFUtilityFct.h ---
#ifndef _PFUTILITYFCT_H_
#define _PFUTILITYFCT_H_

#include <stdlib.h>
#include <math.h>

namespace RobotFlow {
//
// Utility functions mainly for random numbers generation
//  Copyright (C) 2004 Jean-Marc Valin
//
#define PFUTIL_FLOGLOOKUP2SIZE 256
#define PFUTIL_FLOGLOOKUP2SHIFT 15

union PFUTIL_FloatManip {
	float f;
	unsigned int i;
};

void PFUTIL_build_flog_table();

float PFUTIL_flog(const float in);

float PFUTIL_randn();

float PFUTIL_ran();

int PFUTIL_find_range(float i_x, const float *i_cumul, int i_cumulSize);

}

#endif