From 9682745b81833262e3378ce09ab7d0aa3f05645e Mon Sep 17 00:00:00 2001 From: bpeasle Date: Fri, 18 May 2012 20:12:08 +0000 Subject: [PATCH] Added ray tracing and implemented LaserFactor::operator()(const Values &) --- tests/testOccupancyGrid.cpp | 138 +++++++++++++++++++++++++----------- 1 file changed, 96 insertions(+), 42 deletions(-) diff --git a/tests/testOccupancyGrid.cpp b/tests/testOccupancyGrid.cpp index 3aad53829..55b256712 100644 --- a/tests/testOccupancyGrid.cpp +++ b/tests/testOccupancyGrid.cpp @@ -15,37 +15,44 @@ using namespace std; using namespace gtsam; - -/** - * Point Class - * @brief simple class that holds x,y,z coordinates - */ -/*class Point{ -public: - double x,y,z; -};*/ - /** * Laser Factor * @brief factor that encodes a laser measurements likelihood. */ + class LaserFactor : public DiscreteFactor{ +private: + DiscreteKeys m_cells; public: ///constructor - LaserFactor(){ + LaserFactor(const DiscreteKeys &cells) { + m_cells.resize(cells.size()); + for(unsigned int i = 0; i < cells.size(); i++) + m_cells[i] = cells[i]; } /// 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&) const{ - return 0; + virtual double operator()(const Values &vals) const{ + + for(unsigned int i = 0; i < m_cells.size() - 1; i++){ + if(vals.at(m_cells[i].first) == 1) + return 1000; + } + + 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 virtual DecisionTreeFactor operator*(const DecisionTreeFactor&) const{ - throw runtime_error("operator DecisionTreeFactor not implemented"); + throw runtime_error("operator * not implemented"); } virtual operator DecisionTreeFactor() const{ @@ -61,11 +68,12 @@ public: */ class OccupancyGrid : public DiscreteFactorGraph { private: - //bool *m_grid; //array of boolean that denotes if cell is occupied or free - //Point *m_location; //location of each cell - 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 + 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 + + DiscreteKeys m_cells; //list of keys of all cells in the grid + Values m_vals; //mapping from Index to value (0 or 1) public: @@ -76,14 +84,12 @@ public: m_height = height/resolution; m_res = resolution; - /*m_grid = (bool *)malloc(cellCount()*sizeof(bool)); - m_location = (Point *)malloc(cellCount()*sizeof(Point)); - for(int i = 0; i < cellCount(); i++){ - m_grid[i] = false; - m_location[i].x = (i%m_width)*resolution - width/2.0; - m_location[i].y = (i/m_width)*resolution - height/2.0; - }*/ + m_cells.push_back(DiscreteKey(i,2)); + m_vals.insert(pair((Index)i,0)); + } + m_vals[0]; + } ///add a prior @@ -104,8 +110,8 @@ public: 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 - int index; - vector cells; //ordered vector that contain the indicis of all cells hit by the laser + DiscreteKey key; + DiscreteKeys cells; //list of keys of cells hit by the laser //traverse laser for(double i = 0; i < range; i += step){ @@ -113,19 +119,20 @@ public: x = pose.x() + i*cos(pose.theta()); y = pose.y() + i*sin(pose.theta()); - //get the index of the cell that holds point (x,y) - index = cellLookup(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 || index != cells[cells.size()-1]) - cells.push_back(index); + if(i == 0 || key != cells[cells.size()-1]) + cells.push_back(key); } - for(int i = 0; i < cells.size(); i++) - printf("%d,",cells[i]); + for(unsigned int i = 0; i < cells.size(); i++) + printf("%d,", (int)cells[i].first); //add a factor that connects all those cells - push_back(boost::make_shared()); + push_back(boost::make_shared(cells)); + } /// returns the number of cells in the grid @@ -133,8 +140,8 @@ public: return m_width*m_height; } - /// returns the index of the cell in which point (x,y) lies. - int cellLookup(double x, double y) const { + /// returns the key of the cell in which point (x,y) lies. + DiscreteKey keyLookup(double x, double y) const { //move (x,y) to the nearest resolution x *= (1.0/m_res); y *= (1.0/m_res); @@ -149,10 +156,48 @@ public: y = m_height/2 - y; //bounds checking - int index = (int)(y*m_width + x); + int index = y*m_width + x; index = index >= m_width*m_height ? -1 : index; - return 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){ + Index index = (Index)(row*m_width + col); + return m_vals[index]; + } + const size_t operator()(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 assignments()const { + m_vals.print(); } }; @@ -176,12 +221,21 @@ 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[16] = 0; + EXPECT_LONGS_EQUAL(1000, occupancyGrid(0)); + + //run MCMC - } - - /* ************************************************************************* */ int main() { TestResult tr;