gtsam/tests/testOccupancyGrid.cpp

/**
 *      @file testOccupancyGrid.cpp
 *      @date May 14, 2012
 *      @author Brian Peasley
 *      @author Frank Dellaert
 */


#include <gtsam/discrete/DiscreteFactorGraph.h>
#include <gtsam/geometry/Pose2.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/random/mersenne_twister.hpp>
//#include <boost/random/uniform_int_distribution.hpp>  // FIXME: does not exist in boost 1.46
#include <boost/random/uniform_int.hpp> // Old header - should still exist

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

using namespace std;
using namespace gtsam;


/**
 * Laser Factor
 * @brief factor that encodes a laser measurements likelihood.
 */

class LaserFactor : public DiscreteFactor{
private:
	vector<Index> 	m_cells;	///cells in which laser passes through

public:

	///constructor
	LaserFactor(const vector<Index> &cells) : m_cells(cells) {}

	/**
	 * Find value for given assignment of values to variables
	 * return 1000 if any of the non-last cell is occupied and 1 otherwise
	 * Values contains all occupancy values (0 or 1)
	 */
	virtual double operator()(const Values &vals) const{

		// loops through all but the last cell and checks that they are all 0.  Otherwise return 1000.
		for(Index i = 0; i < m_cells.size() - 1; i++){
			if(vals.at(m_cells[i]) == 1)
				return 1000;
		}

		// check if the last cell hit by the laser is 1.  return 900 otherwise.
		if(vals.at(m_cells[m_cells.size() - 1]) == 0)
			return 900;

		return 1;

	}

	/// Multiply in a DecisionTreeFactor and return the result as DecisionTreeFactor
	virtual DecisionTreeFactor operator*(const DecisionTreeFactor&) const{
		throw runtime_error("operator * not implemented");
	}

	virtual DecisionTreeFactor toDecisionTreeFactor() const{
		throw runtime_error("DecisionTreeFactor toDecisionTreeFactor not implemented");
	}
};

/**
 * OccupancyGrid Class
 * An occupancy grid is just a factor graph.
 * Every cell in the occupancy grid is a variable in the factor graph.
 * Measurements will create factors, as well as the prior.
 */
class OccupancyGrid : public DiscreteFactorGraph {
private:
	size_t			width_;		//number of cells wide the grid is
	size_t			height_;	//number of cells tall the grid is
	double			res_;		//the resolution at which the grid is created

	vector<Index> 	cells_;			//list of keys of all cells in the grid
	vector<Index> 	laser_indices_; //indices of the laser factor in factors_


public:

	size_t width() const {
		return width_;
	}
	size_t height() const {
		return height_;
	}
	// should we just not typedef Values Occupancy; ?
	class Occupancy : public Values {
	private:
	public:
	};


	typedef std::vector<double> Marginals;
	///constructor
	///Creates a 2d grid of cells with the origin in the center of the grid
	OccupancyGrid(double width, double height, double resolution){
		width_ 		= 	width/resolution;
		height_ 	= 	height/resolution;
		res_		=	resolution;

		for(Index i = 0; i < cellCount(); i++)
			cells_.push_back(i);
	}

	/// Returns an empty occupancy grid of size width_ x height_
	Occupancy emptyOccupancy(){
		Occupancy		occupancy;		//mapping from Index to value (0 or 1)
		for(size_t i = 0; i < cellCount(); i++)
			occupancy.insert(pair<Index, size_t>((Index)i,0));

		return occupancy;
	}

	///add a prior
	void addPosePrior(Index cell, double prior){
		size_t numStates = 2;
		DiscreteKey key(cell, numStates);

		//add a factor
		vector<double> table(2);
		table[0] = 1-prior;
		table[1] = prior;
		add(key, table);
	}

	///add a laser measurement
	void addLaser(const Pose2 &pose, double range){
		//ray trace from pose to range t//a >= 1 accept new stateo find all cells the laser passes through
		double x = pose.x();		//start position of the laser
		double y = pose.y();
		double step = res_/8.0;	//amount to step in each iteration of laser traversal

		Index 			key;
		vector<Index> 	cells;		//list of keys of cells hit by the laser

		//traverse laser
		for(double i = 0; i < range; i += step){
			//get point on laser
			x = pose.x() + i*cos(pose.theta());
			y = pose.y() + i*sin(pose.theta());

			//printf("%lf %lf\n", x, y);
			//get the key of the cell that holds point (x,y)
			key = keyLookup(x,y);

			//add cell to list of cells if it is new
			if(i == 0 || key != cells[cells.size()-1])
				cells.push_back(key);
		}

//		for(size_t i = 0; i < cells.size(); i++)
//			printf("%ld ", cells[i]);
//		printf("\n");

		//add a factor that connects all those cells
		laser_indices_.push_back(factors_.size());
		push_back(boost::make_shared<LaserFactor>(cells));

	}

	/// returns the number of cells in the grid
	size_t cellCount() const {
		return width_*height_;
	}

	/// returns the key of the cell in which point (x,y) lies.
	Index keyLookup(double x, double y) const {
		//move (x,y) to the nearest resolution
		x *= (1.0/res_);
		y *= (1.0/res_);

		//round to nearest integer
		x = (double)((int)x);
		y = (double)((int)y);

		//determine index
		x += width_/2;
		y = height_/2 - y;

		//bounds checking
		size_t index = y*width_ + x;
		index = index >= width_*height_ ? -1 : index;

		return cells_[index];
	}

	/**
	 * @brief Computes the value of a laser factor
	 * @param index defines which laser is to be used
	 * @param occupancy defines the grid which the laser will be evaulated with
	 * @ret a double value that is the value of the specified laser factor for the grid
	 */
	double laserFactorValue(Index index, const Occupancy &occupancy) const{
		return (*factors_[ laser_indices_[index] ])(occupancy);
	}

	/// returns the sum of the laser factors for the current state of the grid
	double operator()(const Occupancy &occupancy) const {
		double value = 0;

		// loop over all laser factors in the graph
		//printf("%ld\n", (*this).size());

		for(Index i = 0; i < laser_indices_.size(); i++){
			value += laserFactorValue(i, occupancy);
		}

		return value;
	}

	/**
	 * @brief Run a metropolis sampler.
	 * @param iterations defines the number of iterations to run.
	 * @return  vector of marginal probabilities.
	 */
	Marginals runMetropolis(size_t iterations){
		Occupancy occupancy = emptyOccupancy();

		size_t size = cellCount();
		Marginals marginals(size);

		// NOTE: using older interface for boost.random due to interface changes after boost 1.46
		boost::mt19937 rng;
		boost::uniform_int<Index> random_cell(0,size-1);

		// run Metropolis for the requested number of operations
		// compute initial probability of occupancy grid, P(x_t)

		double Px = (*this)(occupancy);

		for(size_t it = 0; it < marginals.size(); it++)
			marginals[it] = 0;

		for(size_t it = 0; it < iterations; it++){
			//choose a random cell
			Index x = random_cell(rng);
			//printf("%ld:",x);
			//flip the state of a random cell, x
				 occupancy[x] = 1 - occupancy[x];

			//compute probability of new occupancy grid, P(x')
			//by summing over all LaserFactor::operator()
				 double Px_prime = (*this)(occupancy);

			//occupancy.print();
			//calculate acceptance ratio, a
				double a = Px_prime/Px;

			//if a <= 1 otherwise accept with probability a
			//if we accept the new state P(x_t) = P(x')
			//	printf(" %.3lf %.3lf\t", Px, Px_prime);
				if(a <= 1){
					Px = Px_prime;
					//printf("\taccept\n");
				}
				else{
					 occupancy[x] = 1 - occupancy[x];
					// printf("\treject\n");
				}

			//increment the number of iterations each cell has been on
				for(size_t i = 0; i < size; i++){
					if(occupancy[i] == 1)
						marginals[i]++;
				}
		}

		//compute the marginals
		for(size_t it = 0; it < size; it++)
			marginals[it] /= iterations;

		return marginals;
	}

};

/* ************************************************************************* */
TEST_UNSAFE( OccupancyGrid, Test1) {
	//Build a small grid and test optimization

	//Build small grid
	double width 		=	3; 		//meters
	double height 		= 	2; 		//meters
	double resolution 	= 	0.5; 	//meters
	OccupancyGrid occupancyGrid(width, height, resolution); //default center to middle

	//Add measurements
	Pose2 pose(0,0,0);
	double range = 1;

	occupancyGrid.addPosePrior(0, 0.7);
	EXPECT_LONGS_EQUAL(1, occupancyGrid.size());

	occupancyGrid.addLaser(pose, range);
	EXPECT_LONGS_EQUAL(2, occupancyGrid.size());

	OccupancyGrid::Occupancy occupancy = occupancyGrid.emptyOccupancy();
	EXPECT_LONGS_EQUAL(900, occupancyGrid.laserFactorValue(0,occupancy));


	occupancy[16] = 1;
	EXPECT_LONGS_EQUAL(1, occupancyGrid.laserFactorValue(0,occupancy));

	occupancy[15] = 1;
	EXPECT_LONGS_EQUAL(1000, occupancyGrid.laserFactorValue(0,occupancy));

	occupancy[16] = 0;
	EXPECT_LONGS_EQUAL(1000, occupancyGrid.laserFactorValue(0,occupancy));


	//run MCMC
	OccupancyGrid::Marginals occupancyMarginals = occupancyGrid.runMetropolis(50000);
	EXPECT_LONGS_EQUAL( (width*height)/pow(resolution,2), occupancyMarginals.size());



}

/* ************************************************************************* */
int main() {
	TestResult tr;
	return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */