From d4a70b66d96fd415a2676444481892503d8d2f9e Mon Sep 17 00:00:00 2001 From: bpeasle Date: Mon, 21 May 2012 19:06:26 +0000 Subject: [PATCH] Added code to run MCMC with real data. --- tests/testOccupancyGrid.cpp | 260 +++++++++++++++++++++++------------- 1 file changed, 165 insertions(+), 95 deletions(-) diff --git a/tests/testOccupancyGrid.cpp b/tests/testOccupancyGrid.cpp index 2289e444b..542c144f4 100644 --- a/tests/testOccupancyGrid.cpp +++ b/tests/testOccupancyGrid.cpp @@ -12,12 +12,14 @@ #include #include +#include #include #include 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 - DiscreteKeys m_cells; ///cells in which laser passes through + vector m_cells; ///cells in which laser passes through public: ///constructor - LaserFactor(const DiscreteKeys &cells) : m_cells(cells) {} + LaserFactor(const vector &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 cells_; //list of keys of all cells in the grid + vector 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 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 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(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 six(0,size-1); + boost::random::uniform_int_distribution 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); + + + } /* ************************************************************************* */