12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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

release/4.3a0
bpeasle 2012-05-18 20:12:08 +00:00
parent b571a2a7ee
commit 9682745b81
1 changed files with 96 additions and 42 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

138
tests/testOccupancyGrid.cpp
View File

@ -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, size_t>((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<int> 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<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.
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;