Implemented a very slow Metropolis algorithm

tests/testOccupancyGrid.cpp

@@ -9,6 +9,9 @@
 #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>
+
 #include <stdlib.h>
 #include <math.h>
 
@@ -22,32 +25,34 @@ using namespace gtsam;
 
 class LaserFactor : public DiscreteFactor{
 private:
-	DiscreteKeys 	m_cells;
+	//FIX ME
+	//m_cells changed to vector<Index>
+	DiscreteKeys 	m_cells;	///cells in which laser passes through
+
 public:
 
 	///constructor
-	LaserFactor(const DiscreteKeys &cells) {
+	LaserFactor(const DiscreteKeys &cells) : m_cells(cells) {}
-		m_cells.resize(cells.size());
-		for(unsigned int i = 0; i < cells.size(); i++)
-			m_cells[i] = cells[i];
-	}
 
-	/// Find value for given assignment of values to variables
+	/**
-	/// return 1000 if any of the non-last cell is occupied and 1 otherwise
+	 * Find value for given assignment of values to variables
-	/// Values contains all occupancy values (0 or 1)
+	 * 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{
 
-		for(unsigned int i = 0; i < m_cells.size() - 1; i++){
+		// 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].first) == 1)
 				return 1000;
 		}
 
+		// check if the last cell hit by the laser is 1.  return 1000 otherwise.
 		if(vals.at(m_cells[m_cells.size() - 1].first) == 0)
 			return 1000;
 
 		return 1;
 
-
 	}
 
 	/// Multiply in a DecisionTreeFactor and return the result as DecisionTreeFactor
@@ -68,15 +73,23 @@ public:
  */
 class OccupancyGrid : public DiscreteFactorGraph {
 private:
-	int		m_width;			//number of cells wide the grid is
+	size_t		m_width;			//number of cells wide the grid is
-	int		m_height;			//number of cells tall the grid is
+	size_t		m_height;			//number of cells tall the grid is
-	double	m_res;				//the resolution at which the grid is created
+	double		m_res;				//the resolution at which the grid is created
 
 	DiscreteKeys 	m_cells;	//list of keys of all cells in the grid
-	Values			m_vals;		//mapping from Index to value (0 or 1)
+
 
 public:
 
+	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){
@@ -84,12 +97,16 @@ public:
 		m_height 	= 	height/resolution;
 		m_res		=	resolution;
 
-		for(int i = 0; i < cellCount(); i++){
+		for(size_t i = 0; i < cellCount(); i++)
 			m_cells.push_back(DiscreteKey(i,2));
-			m_vals.insert(pair<Index, size_t>((Index)i,0));
+	}
-		}
-		m_vals[0];
 
+	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
@@ -127,7 +144,7 @@ public:
 				cells.push_back(key);
 		}
 
-		for(unsigned int i = 0; i < cells.size(); i++)
+		for(Index i = 0; i < cells.size(); i++)
 			printf("%d,", (int)cells[i].first);
 
 		//add a factor that connects all those cells
@@ -136,7 +153,7 @@ public:
 	}
 
 	/// returns the number of cells in the grid
-	int cellCount() const {
+	size_t cellCount() const {
 		return m_width*m_height;
 	}
 
@@ -156,48 +173,85 @@ public:
 		y = m_height/2 - y;
 
 		//bounds checking
-		int index = y*m_width + x;
+		size_t index = y*m_width + x;
 		index = index >= m_width*m_height ? -1 : index;
 
 		return m_cells[index];
 	}
 
-	/// access a cell in the grid via its index
+
-		size_t &operator[](Index index){
-			return m_vals[index];
-		}
-		const size_t operator[](Index index) const{
-			return m_vals.at(index);
-		}
 
 	/// access a cell in the grid via its row and column
-	size_t &operator()(int row, int col){
+	/*size_t &cell(int row, int col){
 		Index index = (Index)(row*m_width + col);
 		return m_vals[index];
 	}
-	const size_t operator()(int row, int col) const{
+	const size_t cell(int row, int col) const{
 		Index index = (Index)(row*m_width + col);
 		return m_vals.at(index);
-	}
+	}*/
 
 	/// prints an ASCII grid to the console
-	void print() const {
+//	void print() const {
-		Index index;
+//		Index index;
-		printf("\n");
+//		printf("\n");
-		for(int i = 0; i < m_height; i++){
+//		for(size_t i = 0; i < m_height; i++){
-			for(int j = 0; j < m_width; j++){
+//			for(size_t j = 0; j < m_width; j++){
-				printf("%ld ", m_vals.at(index));
+//				printf("%ld ", m_vals.at(index));
-				index++;
+//				index++;
-			}
+//			}
-			printf("\n");
+//			printf("\n");
-		}
+//		}
-	}
+//	}
-	double operator()(int index) const{
+
-		return (*factors_[index + 1])(m_vals);
+	//FIX ME
+	//better name
+	double laserFactorValue(int index, const Occupancy &occupancy) const{
+		return (*factors_[index + 1])(occupancy);
 	}
 
-	void assignments()const {
+	/**
-		m_vals.print();
+	 * @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);
+
+		boost::random::mt19937 rng;
+		boost::random::uniform_int_distribution<Index> six(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; it < iterations; it++){
+			//choose a random cell
+			Index x = six(rng);
+
+			//flip the state of a random cell, x
+				 occupancy[x] = 1 - occupancy[x];
+
+				//compute probability of new occupancy grid, P(x')
+					// sum over all LaserFactor::operator()
+					double Px_prime = (*this)(occupancy);
+
+			//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')
+				if(a >= 1){
+					Px = Px_prime;
+				}else{
+					 occupancy[x] = 1 - occupancy[x];
+				}
+		}
+
+		return marginals;
 	}
 
 };
@@ -221,19 +275,27 @@ TEST_UNSAFE( OccupancyGrid, Test1) {
 
 	occupancyGrid.addLaser(pose, range);
 	EXPECT_LONGS_EQUAL(2, occupancyGrid.size());
-	EXPECT_LONGS_EQUAL(1000, occupancyGrid(0));
 
-	occupancyGrid[16] = 1;
-	EXPECT_LONGS_EQUAL(1, occupancyGrid(0));
 
-	occupancyGrid[15] = 1;
+	OccupancyGrid::Occupancy occupancy = occupancyGrid.emptyOccupancy();
-	EXPECT_LONGS_EQUAL(1000, occupancyGrid(0));
+	EXPECT_LONGS_EQUAL(1000, occupancyGrid.laserFactorValue(0,occupancy));
 
-	occupancyGrid[16] = 0;
+
-	EXPECT_LONGS_EQUAL(1000, occupancyGrid(0));
+	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(5);
+	EXPECT_LONGS_EQUAL( (width*height)/pow(resolution,2), occupancyMarginals.size());
+	//select a cell at a random to flip
+
 }
 
 /* ************************************************************************* */