tests/testOccupancyGrid.cpp

@@ -0,0 +1,191 @@
+/**
+ *      @file testOccupancyGrid.cpp
+ *      @date May 14, 2012
+ *      @author Brian Peasley
+ *      @author Frank Dellaert
+ */
+
+
+#include <gtsam/discrete/DiscreteFactorGraph.h>
+#include <gtsam/geometry/Pose2.h>
+#include <CppUnitLite/TestHarness.h>
+#include <stdlib.h>
+#include <math.h>
+
+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{
+public:
+
+	///constructor
+	LaserFactor(){
+	}
+
+	/// 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;
+	}
+
+	/// Multiply in a DecisionTreeFactor and return the result as DecisionTreeFactor
+	virtual DecisionTreeFactor operator*(const DecisionTreeFactor&) const{
+		throw runtime_error("operator DecisionTreeFactor not implemented");
+	}
+
+	virtual operator DecisionTreeFactor() const{
+		throw runtime_error("operator DecisionTreeFactor not implemented");
+	}
+};
+
+/**
+ * OccupancyGrid Class
+ * An occupancy grid is just a factor graph.
+ * Every cell in the occupancy grid is a variable in the factor graph.
+ * Measurements will create factors, as well as the prior.
+ */
+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
+
+public:
+
+	///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;
+
+		/*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;
+		}*/
+	}
+
+	///add a prior
+	void addPrior(Index cell, double prior){
+		size_t numStates = 2;
+		DiscreteKey key(cell, numStates);
+
+		//add a factor
+		vector<double> table(2);
+		table[0] = 1-prior;
+		table[1] = prior;
+		add(key, table);
+	}
+
+	///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
+		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<int> cells;			//ordered vector that contain the indicis of all cells hit by the laser
+
+		//traverse laser
+		for(double i = 0; i < range; i += step){
+			//get point on laser
+			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);
+
+			//add cell to list of cells if it is new
+			if(i == 0 || index != cells[cells.size()-1])
+				cells.push_back(index);
+		}
+
+		for(int i = 0; i < cells.size(); i++)
+			printf("%d,",cells[i]);
+
+		//add a factor that connects all those cells
+		push_back(boost::make_shared<LaserFactor>());
+	}
+
+	/// returns the number of cells in the grid
+	int cellCount() const {
+		return m_width*m_height;
+	}
+
+	/// returns the index of the cell in which point (x,y) lies.
+	int cellLookup(double x, double y) const {
+		//move (x,y) to the nearest resolution
+		x *= (1.0/m_res);
+		y *= (1.0/m_res);
+
+		//round to nearest integer
+		x = (double)((int)x);
+		y = (double)((int)y);
+
+
+		//determine index
+		x += m_width/2;
+		y = m_height/2 - y;
+
+		//bounds checking
+		int index = (int)(y*m_width + x);
+		index = index >= m_width*m_height ? -1 : index;
+
+		return index;
+	}
+
+};
+
+/* ************************************************************************* */
+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
+	OccupancyGrid occupancyGrid(width, height, resolution); //default center to middle
+
+	//Add measurements
+	Pose2 pose(0,0,0);
+	double range = 1.0;
+
+	occupancyGrid.addPrior(0, 0.7);
+	EXPECT_LONGS_EQUAL(1, occupancyGrid.size());
+
+	occupancyGrid.addLaser(pose, range);
+	EXPECT_LONGS_EQUAL(2, occupancyGrid.size());
+	//run MCMC
+
+}
+
+
+
+/* ************************************************************************* */
+int main() {
+	TestResult tr;
+	return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */
+