From ba6439dbb146d5235d5eaffd9557eaed8ea5dfaf Mon Sep 17 00:00:00 2001 From: bpeasle Date: Fri, 18 May 2012 22:09:20 +0000 Subject: [PATCH] Implemented a very slow Metropolis algorithm --- tests/testOccupancyGrid.cpp | 170 ++++++++++++++++++++++++------------ 1 file changed, 116 insertions(+), 54 deletions(-) diff --git a/tests/testOccupancyGrid.cpp b/tests/testOccupancyGrid.cpp index 55b256712..2289e444b 100644 --- a/tests/testOccupancyGrid.cpp +++ b/tests/testOccupancyGrid.cpp @@ -9,6 +9,9 @@ #include #include #include +#include +#include + #include #include @@ -22,32 +25,34 @@ using namespace gtsam; class LaserFactor : public DiscreteFactor{ private: - DiscreteKeys m_cells; + //FIX ME + //m_cells changed to vector + DiscreteKeys m_cells; ///cells in which laser passes through + public: ///constructor - LaserFactor(const DiscreteKeys &cells) { - m_cells.resize(cells.size()); - for(unsigned int i = 0; i < cells.size(); i++) - m_cells[i] = cells[i]; - } + LaserFactor(const DiscreteKeys &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) + /** + * 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{ - 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 - int m_height; //number of cells tall the grid is - double m_res; //the resolution at which the grid is created + size_t m_width; //number of cells wide 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 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 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)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)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 { - Index index; - printf("\n"); - for(int i = 0; i < m_height; i++){ - for(int j = 0; j < m_width; j++){ - printf("%ld ", m_vals.at(index)); - index++; - } - printf("\n"); - } - } - double operator()(int index) const{ - return (*factors_[index + 1])(m_vals); +// void print() const { +// Index index; +// printf("\n"); +// for(size_t i = 0; i < m_height; i++){ +// for(size_t j = 0; j < m_width; j++){ +// printf("%ld ", m_vals.at(index)); +// index++; +// } +// printf("\n"); +// } +// } + + //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 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; - EXPECT_LONGS_EQUAL(1000, occupancyGrid(0)); + OccupancyGrid::Occupancy occupancy = occupancyGrid.emptyOccupancy(); + 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 + } /* ************************************************************************* */