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

Added code to run MCMC with real data.

release/4.3a0
bpeasle 2012-05-21 19:06:26 +00:00
parent 8938c659b6
commit d4a70b66d9
1 changed files with 165 additions and 95 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

256
tests/testOccupancyGrid.cpp
View File

@ -12,12 +12,14 @@
#include <boost/random/mersenne_twister.hpp> #include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_int_distribution.hpp> #include <boost/random/uniform_int_distribution.hpp>
#include <vector>
#include <stdlib.h> #include <stdlib.h>
#include <math.h> #include <math.h>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
/** /**
* Laser Factor * Laser Factor
* @brief factor that encodes a laser measurements likelihood. * @brief factor that encodes a laser measurements likelihood.
@ -25,14 +27,12 @@ using namespace gtsam;
class LaserFactor : public DiscreteFactor{ class LaserFactor : public DiscreteFactor{
private: private:
//FIX ME vector<Index> m_cells; ///cells in which laser passes through
//m_cells changed to vector<Index>
DiscreteKeys m_cells; ///cells in which laser passes through
public: public:
///constructor ///constructor
LaserFactor(const DiscreteKeys &cells) : m_cells(cells) {} LaserFactor(const vector<Index> &cells) : m_cells(cells) {}
/** /**
* Find value for given assignment of values to variables * Find value for given assignment of values to variables
@ -43,13 +43,13 @@ public:
// loops through all but the last cell and checks that they are all 0. Otherwise return 1000. // 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++){ for(Index i = 0; i < m_cells.size() - 1; i++){
if(vals.at(m_cells[i].first) == 1) if(vals.at(m_cells[i]) == 1)
return 1000; return 1000;
} }
// check if the last cell hit by the laser is 1. return 1000 otherwise. // check if the last cell hit by the laser is 1. return 900 otherwise.
if(vals.at(m_cells[m_cells.size() - 1].first) == 0) if(vals.at(m_cells[m_cells.size() - 1]) == 0)
return 1000; return 900;
return 1; return 1;
@ -73,34 +73,42 @@ public:
*/ */
class OccupancyGrid : public DiscreteFactorGraph { class OccupancyGrid : public DiscreteFactorGraph {
private: private:
size_t m_width; //number of cells wide the grid is size_t width_; //number of cells wide the grid is
size_t m_height; //number of cells tall the grid is size_t height_; //number of cells tall the grid is
double m_res; //the resolution at which the grid is created double res_; //the resolution at which the grid is created
DiscreteKeys m_cells; //list of keys of all cells in the grid vector<Index> cells_; //list of keys of all cells in the grid
vector<Index> laser_indices_; //indices of the laser factor in factors_
public: public:
size_t width() const {
return width_;
}
size_t height() const {
return height_;
}
// should we just not typedef Values Occupancy; ?
class Occupancy : public Values { class Occupancy : public Values {
private: private:
public: public:
}; };
typedef std::vector<double> Marginals; typedef std::vector<double> Marginals;
///constructor ///constructor
///Creates a 2d grid of cells with the origin in the center of the grid ///Creates a 2d grid of cells with the origin in the center of the grid
OccupancyGrid(double width, double height, double resolution){ OccupancyGrid(double width, double height, double resolution){
m_width = width/resolution; width_ = width/resolution;
m_height = height/resolution; height_ = height/resolution;
m_res = resolution; res_ = resolution;
for(size_t i = 0; i < cellCount(); i++) for(Index i = 0; i < cellCount(); i++)
m_cells.push_back(DiscreteKey(i,2)); cells_.push_back(i);
} }
/// Returns an empty occupancy grid of size width_ x height_
Occupancy emptyOccupancy(){ Occupancy emptyOccupancy(){
Occupancy occupancy; //mapping from Index to value (0 or 1) Occupancy occupancy; //mapping from Index to value (0 or 1)
for(size_t i = 0; i < cellCount(); i++) for(size_t i = 0; i < cellCount(); i++)
@ -123,12 +131,13 @@ public:
///add a laser measurement ///add a laser measurement
void addLaser(const Pose2 &pose, double range){ void addLaser(const Pose2 &pose, double range){
//ray trace from pose to range to find all cells the laser passes through //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 x = pose.x(); //start position of the laser
double y = pose.y(); double y = pose.y();
double step = m_res/8.0; //amount to step in each iteration of laser traversal double step = res_/8.0; //amount to step in each iteration of laser traversal
DiscreteKey key;
DiscreteKeys cells; //list of keys of cells hit by the laser Index key;
vector<Index> cells; //list of keys of cells hit by the laser
//traverse laser //traverse laser
for(double i = 0; i < range; i += step){ for(double i = 0; i < range; i += step){
@ -136,6 +145,7 @@ public:
x = pose.x() + i*cos(pose.theta()); x = pose.x() + i*cos(pose.theta());
y = pose.y() + i*sin(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) //get the key of the cell that holds point (x,y)
key = keyLookup(x,y); key = keyLookup(x,y);
@ -144,70 +154,64 @@ public:
cells.push_back(key); cells.push_back(key);
} }
for(Index i = 0; i < cells.size(); i++) // for(size_t i = 0; i < cells.size(); i++)
printf("%d,", (int)cells[i].first); // printf("%ld ", cells[i]);
// printf("\n");
//add a factor that connects all those cells //add a factor that connects all those cells
laser_indices_.push_back(factors_.size());
push_back(boost::make_shared<LaserFactor>(cells)); push_back(boost::make_shared<LaserFactor>(cells));
} }
/// returns the number of cells in the grid /// returns the number of cells in the grid
size_t cellCount() const { size_t cellCount() const {
return m_width*m_height; return width_*height_;
} }
/// returns the key of the cell in which point (x,y) lies. /// returns the key of the cell in which point (x,y) lies.
DiscreteKey keyLookup(double x, double y) const { Index keyLookup(double x, double y) const {
//move (x,y) to the nearest resolution //move (x,y) to the nearest resolution
x *= (1.0/m_res); x *= (1.0/res_);
y *= (1.0/m_res); y *= (1.0/res_);
//round to nearest integer //round to nearest integer
x = (double)((int)x); x = (double)((int)x);
y = (double)((int)y); y = (double)((int)y);
//determine index //determine index
x += m_width/2; x += width_/2;
y = m_height/2 - y; y = height_/2 - y;
//bounds checking //bounds checking
size_t index = y*m_width + x; size_t index = y*width_ + x;
index = index >= m_width*m_height ? -1 : index; index = index >= width_*height_ ? -1 : index;
return m_cells[index]; return cells_[index];
} }
/**
* @brief Computes the value of a laser factor
/// access a cell in the grid via its row and column * @param index defines which laser is to be used
/*size_t &cell(int row, int col){ * @param occupancy defines the grid which the laser will be evaulated with
Index index = (Index)(row*m_width + col); * @ret a double value that is the value of the specified laser factor for the grid
return m_vals[index]; */
double laserFactorValue(Index index, const Occupancy &occupancy) const{
return (*factors_[ laser_indices_[index] ])(occupancy);
} }
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 /// returns the sum of the laser factors for the current state of the grid
// void print() const { double operator()(const Occupancy &occupancy) const {
// Index index; double value = 0;
// 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 // loop over all laser factors in the graph
//better name //printf("%ld\n", (*this).size());
double laserFactorValue(int index, const Occupancy &occupancy) const{
return (*factors_[index + 1])(occupancy); for(Index i = 0; i < laser_indices_.size(); i++){
value += laserFactorValue(i, occupancy);
}
return value;
} }
/** /**
@ -222,35 +226,55 @@ public:
Marginals marginals(size); Marginals marginals(size);
boost::random::mt19937 rng; boost::random::mt19937 rng;
boost::random::uniform_int_distribution<Index> six(0,size-1); boost::random::uniform_int_distribution<Index> random_cell(0,size-1);
// run Metropolis for the requested number of operations // run Metropolis for the requested number of operations
// compute initial probability of occupancy grid, P(x_t) // 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);
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 //flip the state of a random cell, x
occupancy[x] = 1 - occupancy[x]; occupancy[x] = 1 - occupancy[x];
//compute probability of new occupancy grid, P(x') //compute probability of new occupancy grid, P(x')
// sum over all LaserFactor::operator() //by summing over all LaserFactor::operator()
double Px_prime = (*this)(occupancy); double Px_prime = (*this)(occupancy);
//occupancy.print();
//calculate acceptance ratio, a //calculate acceptance ratio, a
double a = Px_prime/Px; double a = Px_prime/Px;
//if a <= 1 otherwise accept with probability a
//if a >= 1 otherwise accept with probability a
//if we accept the new state P(x_t) = P(x') //if we accept the new state P(x_t) = P(x')
if(a >= 1){ // printf(" %.3lf %.3lf\t", Px, Px_prime);
if(a <= 1){
Px = Px_prime; Px = Px_prime;
}else{ //printf("\taccept\n");
}
else{
occupancy[x] = 1 - occupancy[x]; 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; return marginals;
} }
@ -261,41 +285,87 @@ TEST_UNSAFE( OccupancyGrid, Test1) {
//Build a small grid and test optimization //Build a small grid and test optimization
//Build small grid //Build small grid
double width = 3; //meters double width = 20; //meters
double height = 2; //meters double height = 20; //meters
double resolution = 0.5; //meters double resolution = 0.2; //meters
OccupancyGrid occupancyGrid(width, height, resolution); //default center to middle OccupancyGrid occupancyGrid(width, height, resolution); //default center to middle
//Add measurements //Add measurements
Pose2 pose(0,0,0); // Pose2 pose(0,0,0);
double range = 1.0; // double range = 4.499765;
//
// occupancyGrid.addPrior(0, 0.7);
// EXPECT_LONGS_EQUAL(1, occupancyGrid.size());
//
// occupancyGrid.addLaser(pose, range);
// EXPECT_LONGS_EQUAL(2, occupancyGrid.size());
occupancyGrid.addPrior(0, 0.7); //add lasers
EXPECT_LONGS_EQUAL(1, occupancyGrid.size()); int n_frames = 1;
int n_lasers_per_frame = 640;
char laser_list_file[1000];
for(int i = 0; i < n_frames; i++){
sprintf(laser_list_file, "/home/brian/Desktop/research/user/bpeasle/code/KinectInterface/Data/ScanLinesAsLasers/KinectRecording9/laser_list%.4d", i);
FILE *fptr = fopen(laser_list_file,"r");
double x,y, theta;
double range, angle;
fscanf(fptr, "%lf %lf %lf", &x, &y, &theta);
for(int j = 0; j < n_lasers_per_frame; j++){
fscanf(fptr, "%lf %lf", &range, &angle);
//if(j == 159){
Pose2 pose(x,y, theta+angle);
occupancyGrid.addLaser(pose, range); occupancyGrid.addLaser(pose, range);
EXPECT_LONGS_EQUAL(2, occupancyGrid.size()); //}
}
fclose(fptr);
}
OccupancyGrid::Occupancy occupancy = occupancyGrid.emptyOccupancy(); // OccupancyGrid::Occupancy occupancy = occupancyGrid.emptyOccupancy();
EXPECT_LONGS_EQUAL(1000, occupancyGrid.laserFactorValue(0,occupancy)); // EXPECT_LONGS_EQUAL(900, occupancyGrid.laserFactorValue(0,occupancy));
//
//
occupancy[16] = 1; // occupancy[16] = 1;
EXPECT_LONGS_EQUAL(1, occupancyGrid.laserFactorValue(0,occupancy)); // EXPECT_LONGS_EQUAL(1, occupancyGrid.laserFactorValue(0,occupancy));
//
occupancy[15] = 1; // occupancy[15] = 1;
EXPECT_LONGS_EQUAL(1000, occupancyGrid.laserFactorValue(0,occupancy)); // EXPECT_LONGS_EQUAL(1000, occupancyGrid.laserFactorValue(0,occupancy));
//
occupancy[16] = 0; // occupancy[16] = 0;
EXPECT_LONGS_EQUAL(1000, occupancyGrid.laserFactorValue(0,occupancy)); // EXPECT_LONGS_EQUAL(1000, occupancyGrid.laserFactorValue(0,occupancy));
//run MCMC //run MCMC
OccupancyGrid::Marginals occupancyMarginals = occupancyGrid.runMetropolis(5); OccupancyGrid::Marginals occupancyMarginals = occupancyGrid.runMetropolis(50000);
EXPECT_LONGS_EQUAL( (width*height)/pow(resolution,2), occupancyMarginals.size()); //EXPECT_LONGS_EQUAL( (width*height)/pow(resolution,2), occupancyMarginals.size());
//select a cell at a random to flip //select a cell at a random to flip
printf("\n");
for(size_t i = 0, it = 0; i < occupancyGrid.height(); i++){
for(size_t j = 0; j < occupancyGrid.width(); j++, it++){
printf("%.2lf ", occupancyMarginals[it]);
}
printf("\n");
}
char marginalsOutput[1000];
sprintf(marginalsOutput, "/home/brian/Desktop/research/user/bpeasle/code/KinectInterface/marginals.txt");
FILE *fptr = fopen(marginalsOutput, "w");
fprintf(fptr, "%d %d\n", occupancyGrid.width(), occupancyGrid.height());
for(int i = 0; i < occupancyMarginals.size(); i++){
fprintf(fptr, "%lf ", occupancyMarginals[i]);
}
fclose(fptr);
} }
/* ************************************************************************* */ /* ************************************************************************* */