333 lines
9.2 KiB
C++
333 lines
9.2 KiB
C++
/**
|
|
* @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);
|
|
}
|
|
/* ************************************************************************* */
|
|
|