particlefilter3d_test.cpp

Particle filtering implementation and application to people tracking.

#include <stdlib.h>
#include <iostream>
#include <fstream>
#include <map>
#include "particlefilter.h"
#include "prioritybuffer.h"

#include <utils/point3d.h>
#include <utils/Pose3D.h>
#include <utils/math.h>
#include <utils/commandline.h>

using namespace std;

#define test(s) {cout << s <<  " " << flush;}
#define testOk() {cout << "OK" << endl;}
#define MAXRANGE 10000
#define MINRANGE -500

struct Particle: Pose3D
{
	double w;
	inline operator double() const {return w;}
	inline void setWeight(double _w) {w=_w;}
	Particle* next;
};

ostream& printParticles(ostream& os, const vector<Particle>& p)
{
	for (vector<Particle>::const_iterator it=p.begin(); it!=p.end(); ++it)
	{
		os << it->translation.x << " " << it->translation.y << " " << it->translation.z << endl;
	}
	return os;
}

ostream& operator << (ostream& os, const Pose3D& p)
{
	os << p.translation.x << " " << p.translation.y << " " << p.translation.z;
	os << " " << p.rotation.getXAngle() << " " << p.rotation.getYAngle() << " " << p.rotation.getZAngle();
	return os;
}

struct EvolutionModel
{
	Particle evolve(const Particle& p, const Pose3D& op)
	{
		Particle pn(p);
		pn.conc(op);
		pn.translate(randrange(2.5*op.translation.abs()),
				randrange(2.5*op.translation.abs()),
				randrange(2.5*op.translation.abs()));
		return pn;
	}
};

double sigmaW = 0.04;
double sigmaH = 0.04;


struct Landmark: Point3d
{
	double w;

	inline operator double() const {return w;}
};

struct LikelyhoodModel
{
	double likelyhood(const Particle& p, const prioritybuffer<Landmark>& landmark, const Pose3D& pose) const
	{
		double gamma=1;
		for (prioritybuffer<Landmark>::const_iterator it=landmark.begin(); it!=landmark.end(); ++it)
		{
			Pose3D landmarkFromParticle(
				it->x - p.translation.x,
				it->y - p.translation.y,
				it->z - p.translation.z);
			landmarkFromParticle.rotate(p.rotation);
			double landmarkFromParticleW = atan2(landmarkFromParticle.translation.y,landmarkFromParticle.translation.x);
			double landmarkFromParticleH = atan2(landmarkFromParticle.translation.z,landmarkFromParticle.translation.x);

			Pose3D landmarkFromRealPose(
				it->x - pose.translation.x,
				it->y - pose.translation.y,
				it->z - pose.translation.z);
			landmarkFromRealPose.rotate(pose.rotation);
			double landmarkFromRealPoseW = atan2(landmarkFromRealPose.translation.y,landmarkFromRealPose.translation.x);
			double landmarkFromRealPoseH = atan2(landmarkFromRealPose.translation.z,landmarkFromRealPose.translation.x);
			
			gamma *= exp(-(square(landmarkFromRealPoseW - landmarkFromParticleW)/sigmaW));
			gamma *= exp(-(square(landmarkFromRealPoseH - landmarkFromParticleH)/sigmaH));
		}
		//cerr << gamma << endl;
		gamma = gamma < 0.01 ? 0.01 : gamma;
		return gamma;
	}
};

Pose3D evolvePose(Pose3D& pose)
{
	Pose3D newodometry;
	newodometry.translate(randrange(100,200),randrange(-10,10),randrange(-50,50));
	newodometry.rotateX(randrange(deg2rad(-2),deg2rad(2)));
	newodometry.rotateY(randrange(deg2rad(-2),deg2rad(2)));
	newodometry.rotateZ(randrange(deg2rad(-5),deg2rad(10)));
	//cerr << "# Odometry: " << newodometry << endl;
	return newodometry;
}

void calcFOVPoint(vector<Pose3D>& fovp,
		const Pose3D& pose,
		const double& spanHeight,
		const double& spanWidth)
{
	double r=10000;
	IntPoint sH[4], sW[4];
	sH[0] = IntPoint( 1, 1);
	sW[0] = IntPoint( 1,-1);
	sH[1] = IntPoint(-1,-1);
	sW[1] = IntPoint(-1, 1);
	sH[2] = IntPoint(-1, 1);
	sW[2] = IntPoint( 1, 1);
	sH[3] = IntPoint( 1,-1);
	sW[3] = IntPoint(-1,-1);
	Pose3D p[3];
	p[0] = pose;
		
	for (int i=0; i<4; ++i)
	{
		p[1] = pose;
		p[1].rotateZ(sW[i].x*spanWidth).rotateY(sH[i].x*spanHeight).conc(Pose3D(r,0.,0.));
		p[2] = pose;
		p[2].rotateZ(sW[i].y*spanWidth).rotateY(sH[i].y*spanHeight).conc(Pose3D(r,0.,0.));

		fovp.push_back(p[0]);
		fovp.push_back(p[1]);
		fovp.push_back(p[2]);
	}
}

bool landmarkInsideFOV(
		Landmark& point,
		const vector<Pose3D>& fovp)
{
	double A,B,C,D;
	int c = 0;
	double s;

	for (unsigned int i=0; i<fovp.size(); i+=3)
	{
		calculateplane(A,B,C,D,
									Point3d(fovp[i+0].translation.x,fovp[i+0].translation.y,fovp[i+0].translation.z),
									Point3d(fovp[i+1].translation.x,fovp[i+1].translation.y,fovp[i+1].translation.z),
									Point3d(fovp[i+2].translation.x,fovp[i+2].translation.y,fovp[i+2].translation.z));
		s = A*point.x + B*point.y + C*point.z + D;
		c += s<0 ? 1 : 0;
	}
	point.w = drand48();
	return c == 4;
}

Pose3D clusterize(vector<Particle>& v,
		const double& d, const double& boundx, const double& boundy, const double& boundz,
		double& sigma)
{
	Pose3D pose;
	sigma=0;
	map<int, vector<int> > grid;

	for (unsigned int i=0; i<v.size(); ++i)
	{
		int x = int(v[i].translation.x/d+.5),
				y = int(v[i].translation.y/d+.5),
				z = int(v[i].translation.z/d+.5);
		
		int index = x*100+y*10+z;
		grid[index].push_back(i);
	}

	unsigned int countmax=0;
	int gidx;

	for (map<int, vector<int> >::const_iterator it=grid.begin(); it!=grid.end(); ++it)
	{
		const vector<int>& vit = (it->second);
		if (vit.size() > countmax)
		{
			countmax = vit.size();
			gidx = it->first;
		}
	}
	if (countmax>0)
	{
		double xS=0,
					 yS=0,
					 zS=0;
		double cx=0,
					 sx=0,
					 cy=0,
					 sy=0,
					 cz=0,
					 sz=0;

		double a;
		const vector<int>& vit = grid[gidx];
		for (vector<int>::const_iterator it=vit.begin(); it!=vit.end(); ++it)
		{
			xS += v[*it].translation.x;
			yS += v[*it].translation.y;
			zS += v[*it].translation.z;

			a = v[*it].rotation.getXAngle();
			cx += cos(a);
			sx += sin(a);
			a = v[*it].rotation.getYAngle();
			cy += cos(a);
			sy += sin(a);
			a = v[*it].rotation.getZAngle();
			cz += cos(a);
			sz += sin(a);
		}
		pose.translation.x = xS/countmax;
		pose.translation.y = yS/countmax;
		pose.translation.z = zS/countmax;
		
		pose.rotation.rotateX(atan2(sx,cx)).rotateY(atan2(sy,cy)).rotateZ(atan2(sz,cz));
		sigma = countmax/v.size();
	}
	return pose;
}

int main (int argc, const char * const * argv)
{
	int nparticles = 1000,
			nlandmarks = 100;
	double spanWidth = 0.37,
				 spanHeight = 0.30;
	double dpose = 100;
	bool trackmode = false;
	Pose3D pose;
	pose.translate(0.,100.,1000.);
	pose.rotateX(0.);
	pose.rotateY(0.);
	pose.rotateZ(deg2rad(-40.));


	double mbX = MINRANGE,
				 MbX = 2*MAXRANGE,
				 mbY = -MAXRANGE,
				 MbY = MAXRANGE,
				 mbZ = 0,
				 MbZ = 4000;

	CMD_PARSE_BEGIN(1,argc)
	{
		parseInt("-p",nparticles);
		parseInt("-l",nlandmarks);
		parseDouble("-w",spanWidth);
		parseDouble("-h",spanHeight);
		parseDouble("-d",dpose);
		parseFlag("-t",trackmode);
	}
	CMD_PARSE_END

	vector<Landmark> landmarkModel(nlandmarks);
	for (int i=0; i<nlandmarks;  ++i)
	{
		landmarkModel[i].x = randrange(mbX,MbX);
		landmarkModel[i].y = randrange(mbY,MbY);
		landmarkModel[i].z = randrange(mbZ,MbZ);
		landmarkModel[i].w = 1;
	}

	vector<Particle> particles(nparticles);
	LikelyhoodModel likelyhoodModel;
	uniform_resampler<Particle, double> resampler;
	EvolutionModel evolutionModel;

	for (vector<Particle>::iterator it=particles.begin(); it!=particles.end(); it++)
	{
		it->w=1;
		if (trackmode)
		{
			double linearRange = 50;
			it->translation = pose.translation;
			it->rotation = pose.rotation;
			it->conc(Pose3D(randrange(-linearRange,linearRange),randrange(-linearRange,linearRange),randrange(-linearRange,linearRange)));
		}
		else
		{
			it->translation.x=randrange(mbX,MbX);
			it->translation.y=randrange(mbY,MbY);
			it->translation.z=randrange(mbZ,MbZ);
			it->rotateX(randrange(-1.57,1.57));
			it->rotateY(randrange(-1.57,1.57));
			it->rotateZ(randrange(-1.57,1.57));
		}
	}

	vector<Particle> sirparticles(particles);
	vector<Landmark> landmarkVisible;
	vector<Landmark> landmarkOutside;

	/*sir step*/
	bool batch=false;
	while (true)
	{
		landmarkVisible.clear();
		landmarkOutside.clear();
		
		Pose3D odometry = evolvePose(pose);
		pose.conc(odometry);
		//cerr << "# Pose: " << pose << endl;

		vector<Particle> newgeneration;
		cout << "# SIR step" << endl;
		//evolver.evolve(sirparticles,pose);

		// calculate field of view and landmark visible
		prioritybuffer<Landmark> landmark(5);
		vector<Pose3D> vp;
		calcFOVPoint(vp,pose,spanHeight, spanWidth);
		for (vector<Landmark>::iterator it=landmarkModel.begin(); it!=landmarkModel.end(); ++it)
		{
			if (landmarkInsideFOV(*it, vp))
			{
				landmarkVisible.push_back(*it);
				landmark.push(*it);
			}
			else
			{
				landmarkOutside.push_back(*it);
			}
		}
		
		for (vector<Particle>::iterator it=sirparticles.begin(); it!=sirparticles.end(); it++)
		{
			*it = evolutionModel.evolve(*it,odometry);
			it->setWeight(likelyhoodModel.likelyhood(*it,landmark,pose));
		}
		newgeneration=resampler.resample(sirparticles);
		sirparticles=newgeneration;

		double sigma;
		Pose3D epose = clusterize(sirparticles, dpose, MbX-mbX, MbY-mbY, MbZ-mbZ,sigma);

		cout << "set mouse" << endl;
		cout << "set xlabel 'X front'" << endl;
		cout << "set ylabel 'Y side'" << endl;
		cout << "set zlabel 'Z height'" << endl;
		cout << "splot "
				 << "[" << mbX << ":" << MbX << "]"
				 << "[" << mbY << ":" << MbY << "]" 
				 << "[" << mbZ << ":" << MbZ << "]" 
				 << "'-' using 1:2:3 title 'Particle' w p pt 7 lt 1 ps 1"
				 << ", '-' title 'Landmark Inside' w p pt 6 lt 3 ps 2"
				 << ", '-' title 'Landmark Outside' w p pt 6 lt 2 ps 2"
				 << ", '-' title 'Landmark Weighter' w p pt 6 lt 1 ps 2"
				 << ", '-' title 'True Pose' w p pt 6 lt 3 ps 4"
				 << ", '-' notitle w l lt 3"
				 << ", '-' title 'Estimated Pose " << sigma <<  "' w p pt 4 lt 6 ps 4"
				 << ", '-' notitle w l lt 6"
				 << ", '-' notitle w l lt 5";
		cout << endl;

		// particles
		printParticles(cout,sirparticles);
		cout << "e" << endl;

		// landmark
		for (vector<Landmark>::const_iterator it=landmarkVisible.begin(); it!=landmarkVisible.end(); ++it)
		{
			cout << it->x << " " << it->y << " " << it->z << endl;
		}
		cout << "e" << endl;
		
		for (vector<Landmark>::const_iterator it=landmarkOutside.begin(); it!=landmarkOutside.end(); ++it)
		{
			cout << it->x << " " << it->y << " " << it->z << endl;
		}
		cout << "e" << endl;

		for (prioritybuffer<Landmark>::const_iterator it=landmark.begin(); it!=landmark.end(); ++it)
		{
			cout << it->x << " " << it->y << " " << it->z << endl;
		}
		cout << "e" << endl;

		// true pose
		Pose3D v = pose;
		v.conc(Pose3D(5000.,0.,0.));
		cout << pose.translation.x << " " << pose.translation.y << " " << pose.translation.z << endl;
		cout << "e" << endl;
		cout << pose.translation.x << " " << pose.translation.y << " " << pose.translation.z << endl;
		cout << v.translation.x << " " << v.translation.y << " " << v.translation.z << endl;
		cout << "e" << endl;

		// estimated pose
		v = epose;
		v.conc(Pose3D(5000.,0.,0.));
		cout << epose.translation.x << " " << epose.translation.y << " " << epose.translation.z << endl;
		cout << "e" << endl;
		cout << epose.translation.x << " " << epose.translation.y << " " << epose.translation.z << endl;
		cout << v.translation.x << " " << v.translation.y << " " << v.translation.z << endl;
		cout << "e" << endl;

		// camera fov
		for (vector<Pose3D>::const_iterator it=vp.begin(); it!=vp.end(); ++it)
		{
			cout << it->translation.x << " " << it->translation.y << " " << it->translation.z << endl << endl;
		}
		cout << "e" << endl;
		
		if (! batch)
		{
			char buf[2];
			cin.getline(buf,2);
			if (buf[0] == 'q')
			{
				return 0;
			}
			else if (buf[0] == 'b')
			{
				batch = true;
			}
		}
	}
}