12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Implemented a very slow Metropolis algorithm

release/4.3a0
bpeasle 2012-05-18 22:09:20 +00:00
parent 9682745b81
commit ba6439dbb1
1 changed files with 116 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

170
tests/testOccupancyGrid.cpp
View File

@ -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) {
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<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 {
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<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;
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
}
/* ************************************************************************* */