Added ray tracing and implemented LaserFactor::operator()(const Values &)

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{
+	virtual double operator()(const Values &vals) const{
-		return 0;
+
+		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
+	int		m_width;			//number of cells wide the grid is
-	//Point 	*m_location;	//location of each cell
+	int		m_height;			//number of cells tall the grid is
-	int		m_width;		//number of cells wide the grid is
+	double	m_res;				//the resolution at which the grid is created
-	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_cells.push_back(DiscreteKey(i,2));
-			m_location[i].x = (i%m_width)*resolution - width/2.0;
+			m_vals.insert(pair<Index, size_t>((Index)i,0));
-			m_location[i].y = (i/m_width)*resolution - height/2.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;
+		DiscreteKey 	key;
-		vector<int> cells;			//ordered vector that contain the indicis of all cells hit by the laser
+		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)
+			//get the key of the cell that holds point (x,y)
-			index = cellLookup(x,y);
+			key = keyLookup(x,y);
 
 			//add cell to list of cells if it is new
-			if(i == 0 || index != cells[cells.size()-1])
+			if(i == 0 || key != cells[cells.size()-1])
-				cells.push_back(index);
+				cells.push_back(key);
 		}
 
-		for(int i = 0; i < cells.size(); i++)
+		for(unsigned int i = 0; i < cells.size(); i++)
-			printf("%d,",cells[i]);
+			printf("%d,", (int)cells[i].first);
 
 		//add a factor that connects all those cells
-		push_back(boost::make_shared<LaserFactor>());
+		push_back(boost::make_shared<LaserFactor>(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.
+	/// returns the key of the cell in which point (x,y) lies.
-	int cellLookup(double x, double y) const {
+	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;