Added code to run MCMC with real data.
bpeasle
parent
8938c659b6
commit
d4a70b66d9
|
|
@ -12,12 +12,14 @@
|
|||
#include <boost/random/mersenne_twister.hpp>
|
||||
#include <boost/random/uniform_int_distribution.hpp>
|
||||
|
||||
#include <vector>
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
|
||||
/**
|
||||
* Laser Factor
|
||||
* @brief factor that encodes a laser measurements likelihood.
|
||||
|
|
@ -25,14 +27,12 @@ using namespace gtsam;
|
|||
|
||||
class LaserFactor : public DiscreteFactor{
|
||||
private:
|
||||
//FIX ME
|
||||
//m_cells changed to vector<Index>
|
||||
DiscreteKeys m_cells; ///cells in which laser passes through
|
||||
vector<Index> m_cells; ///cells in which laser passes through
|
||||
|
||||
public:
|
||||
|
||||
///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
|
||||
|
|
@ -43,13 +43,13 @@ public:
|
|||
|
||||
// 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)
|
||||
if(vals.at(m_cells[i]) == 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;
|
||||
// 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;
|
||||
|
||||
|
|
@ -73,34 +73,42 @@ public:
|
|||
*/
|
||||
class OccupancyGrid : public DiscreteFactorGraph {
|
||||
private:
|
||||
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
|
||||
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
|
||||
|
||||
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:
|
||||
|
||||
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){
|
||||
m_width = width/resolution;
|
||||
m_height = height/resolution;
|
||||
m_res = resolution;
|
||||
width_ = width/resolution;
|
||||
height_ = height/resolution;
|
||||
res_ = resolution;
|
||||
|
||||
for(size_t i = 0; i < cellCount(); i++)
|
||||
m_cells.push_back(DiscreteKey(i,2));
|
||||
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++)
|
||||
|
|
@ -123,12 +131,13 @@ public:
|
|||
|
||||
///add a laser measurement
|
||||
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 y = pose.y();
|
||||
double step = m_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
|
||||
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){
|
||||
|
|
@ -136,6 +145,7 @@ public:
|
|||
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);
|
||||
|
||||
|
|
@ -144,70 +154,64 @@ public:
|
|||
cells.push_back(key);
|
||||
}
|
||||
|
||||
for(Index i = 0; i < cells.size(); i++)
|
||||
printf("%d,", (int)cells[i].first);
|
||||
// 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 m_width*m_height;
|
||||
return width_*height_;
|
||||
}
|
||||
|
||||
/// 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
|
||||
x *= (1.0/m_res);
|
||||
y *= (1.0/m_res);
|
||||
x *= (1.0/res_);
|
||||
y *= (1.0/res_);
|
||||
|
||||
//round to nearest integer
|
||||
x = (double)((int)x);
|
||||
y = (double)((int)y);
|
||||
|
||||
|
||||
//determine index
|
||||
x += m_width/2;
|
||||
y = m_height/2 - y;
|
||||
x += width_/2;
|
||||
y = height_/2 - y;
|
||||
|
||||
//bounds checking
|
||||
size_t index = y*m_width + x;
|
||||
index = index >= m_width*m_height ? -1 : index;
|
||||
size_t index = y*width_ + x;
|
||||
index = index >= width_*height_ ? -1 : index;
|
||||
|
||||
return m_cells[index];
|
||||
return cells_[index];
|
||||
}
|
||||
|
||||
|
||||
|
||||
/// access a cell in the grid via its row and column
|
||||
/*size_t &cell(int row, int col){
|
||||
Index index = (Index)(row*m_width + col);
|
||||
return m_vals[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);
|
||||
}
|
||||
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(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");
|
||||
// }
|
||||
// }
|
||||
/// returns the sum of the laser factors for the current state of the grid
|
||||
double operator()(const Occupancy &occupancy) const {
|
||||
double value = 0;
|
||||
|
||||
//FIX ME
|
||||
//better name
|
||||
double laserFactorValue(int index, const Occupancy &occupancy) const{
|
||||
return (*factors_[index + 1])(occupancy);
|
||||
// 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;
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -222,35 +226,55 @@ public:
|
|||
Marginals marginals(size);
|
||||
|
||||
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
|
||||
// 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
|
||||
occupancy[x] = 1 - occupancy[x];
|
||||
|
||||
//compute probability of new occupancy grid, P(x')
|
||||
// sum over all LaserFactor::operator()
|
||||
double Px_prime = (*this)(occupancy);
|
||||
//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 a <= 1 otherwise accept with probability a
|
||||
//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;
|
||||
}else{
|
||||
//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;
|
||||
}
|
||||
|
||||
|
|
@ -261,41 +285,87 @@ 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
|
||||
double width = 20; //meters
|
||||
double height = 20; //meters
|
||||
double resolution = 0.2; //meters
|
||||
OccupancyGrid occupancyGrid(width, height, resolution); //default center to middle
|
||||
|
||||
//Add measurements
|
||||
Pose2 pose(0,0,0);
|
||||
double range = 1.0;
|
||||
// Pose2 pose(0,0,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);
|
||||
EXPECT_LONGS_EQUAL(1, occupancyGrid.size());
|
||||
|
||||
occupancyGrid.addLaser(pose, range);
|
||||
EXPECT_LONGS_EQUAL(2, occupancyGrid.size());
|
||||
//add lasers
|
||||
int n_frames = 1;
|
||||
int n_lasers_per_frame = 640;
|
||||
char laser_list_file[1000];
|
||||
|
||||
|
||||
OccupancyGrid::Occupancy occupancy = occupancyGrid.emptyOccupancy();
|
||||
EXPECT_LONGS_EQUAL(1000, occupancyGrid.laserFactorValue(0,occupancy));
|
||||
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);
|
||||
//}
|
||||
}
|
||||
fclose(fptr);
|
||||
|
||||
}
|
||||
|
||||
|
||||
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));
|
||||
// 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(5);
|
||||
EXPECT_LONGS_EQUAL( (width*height)/pow(resolution,2), occupancyMarginals.size());
|
||||
OccupancyGrid::Marginals occupancyMarginals = occupancyGrid.runMetropolis(50000);
|
||||
//EXPECT_LONGS_EQUAL( (width*height)/pow(resolution,2), occupancyMarginals.size());
|
||||
//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);
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue