peopletracking_test.cpp

Particle filtering implementation and application to people tracking.

#include <stdlib.h>
#include <iostream>
#include <fstream>
#include <map>
#include <particlefilter/particlefilter.h>
#include <sensor/sensor_range/rangesensor.h>
#include <sensor/sensor_range/rangereading.h>
#include <utils/point.h>
#include <utils/math.h>
#include <utils/commandline.h>

using namespace std;
using namespace GMapping;

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

struct Particle: Point
{
	double w;
	double oldw;
	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->x << " " << it->y << " " << it->oldw << endl;
	}
	return os;
}

struct EvolutionModel
{
	Particle evolve(const Particle& p)
	{
		Particle pn(p);
		pn.x += randrange(.5);
		pn.y += randrange(.5);
		return pn;
	}
};

struct CamObs: Point
{
	double sx;
	double sy;
};

double sigmaRho = 0.04;

struct LikelyhoodModel
{
	double likelyhood(const Particle& p, const CamObs& c) const
	{
		double dx = c.x - p.x,
					 dy = c.y - p.y;

		double gamma = exp(-square(dx)/c.sx)*exp(-square(dy)/c.sy);
		gamma = gamma < 0.01 ? 0.01 : gamma;
		return gamma;
	}

	double likelyhood(const Particle& p, double rho) const
	{
		double prho = sqrt(square(p.x) + square(p.y));

		if (prho < rho)
		{
			return 0.01;
		}
		return 1;
	}
};

int main (int argc, const char * const * argv)
{
	int nparticles = 1000;
	bool laserobs = false;
	Point people(5.,2.);

	CMD_PARSE_BEGIN(1,argc)
	{
		parseInt("-p",nparticles);
		parseFlag("-l",laserobs);
	}
	CMD_PARSE_END

	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;
		it->x=randrange(MAXRANGE);
		it->y=randrange(MAXRANGE);
	}

	vector<Particle> sirparticles(particles);

	/*sir step*/
	bool batch=false;
	CamObs cobs;

	/* Laser */
	int nbeams = 361;
	double res = deg2rad(0.5);
	double maxrange = 15;
	RangeSensor rs("laser",
									nbeams,
									res,
									OrientedPoint(0.,0.,0.),
									0,
									maxrange);
	vector<RangeSensor::Beam> beams = rs.beams();
	Point wall0(4.5,50),
				wall1(4.5,-50);
	double d[nbeams];
	for (int i=0; i<nbeams; ++i)
	{
		Point ps, pe;
		if (clip(ps,pe,
						 Point(0.,0.),Point(maxrange*(beams[i].c),maxrange*(beams[i].s)),
						 wall0, wall1))
		{
			d[i] = sqrt(square((pe-ps).x) + square((pe-ps).y));
		}
		else
		{
			d[i] = maxrange;
		}
	}
	RangeReading rr(nbeams, d, &rs);
	double anglerange = -.5*res*nbeams;

	int fn = 0;
	while (true)
	{
		/* People move */
		people.y -= 0.05;

		/* Camera observation */
		cobs.sx = randrange(1.,5.);
		cobs.sy = randrange(.5,2.);
		cobs.x = people.x + randrange(cobs.sx);
		cobs.y = people.y + randrange(cobs.sy);

		/* Laser observation */

		/* Estimation */
		vector<Particle> newgeneration;
		for (vector<Particle>::iterator it=sirparticles.begin(); it!=sirparticles.end(); it++)
		{
			double angle = atan2(it->y, it->x);
			double rho = d[int((angle-anglerange)/res + .5)];
			*it = evolutionModel.evolve(*it);
			double w = 1;
			w *= likelyhoodModel.likelyhood(*it,cobs);
			if (laserobs)
			{
				w *= likelyhoodModel.likelyhood(*it,rho);
			}
			it->setWeight(w);
			it->oldw = it->w;
		}
		newgeneration=resampler.resample(sirparticles);
		sirparticles=newgeneration;

		cout << "set mouse" << endl;
		cout << "set title 'Frame Number " << fn++ << "'" << endl;
		cout << "set ticslevel 0" << endl;
		cout << "set xrange[0:" << MAXRANGE << "]" << endl;
		cout << "set yrange[" << -MAXRANGE << ":" << MAXRANGE << "]" << endl;
		cout << "splot "
				 << "'-' title 'Particle' w p pt 7 lt 1 ps 1"
				 << ", '-' title 'True Pose' w p pt 6 lt 3 ps 4"
				 << ", '-' title 'Scan' w p pt 1 lt 2 ps 1"
				 << ", '-' title 'Obs' w p lt 4 ps 1"
				 << ", '-' notitle w l lt 4";
		cout << endl;

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

		// true pose
		cout << people.x << " " << people.y << " " << 0 << endl;
		cout << "e" << endl;

		// scan
		vector<Point> p=rr.cartesianForm(maxrange+1);
		for (unsigned int i=0; i<p.size(); ++i)
		{
			cout << p[i].x << " " << p[i].y << " " << 0 << endl;
		}
		cout << "e" << endl;

		// camera obs
		cout << cobs.x << " " << cobs.y << " " << 0 << endl;
		cout << "e" << endl;
		cout << (cobs.x - cobs.sx) << " " << (cobs.y - cobs.sy) << " " << 0 << endl;
		cout << (cobs.x + cobs.sx) << " " << (cobs.y - cobs.sy) << " " << 0 << endl;
		cout << (cobs.x + cobs.sx) << " " << (cobs.y + cobs.sy) << " " << 0 << endl;
		cout << (cobs.x - cobs.sx) << " " << (cobs.y + cobs.sy) << " " << 0 << endl;
		cout << (cobs.x - cobs.sx) << " " << (cobs.y - cobs.sy) << " " << 0 << 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;
			}
		}
	}
}