testOccupancyGrid.cpp
Go to the documentation of this file.
1 
9 
10 #if 0
11 
13 #include <gtsam/geometry/Pose2.h>
14 
15 #include <vector>
16 #include <cmath.>
17 #include <random>
18 
19 #include <stdlib.h>
20 
21 using namespace std;
22 using namespace gtsam;
23 
24 
30 class LaserFactor : public DiscreteFactor{
31 private:
32  vector<Index> m_cells;
33 
34 public:
35 
37  LaserFactor(const vector<Index> &cells) : m_cells(cells) {}
38 
44  virtual double operator()(const Values &vals) const{
45 
46  // loops through all but the last cell and checks that they are all 0. Otherwise return 1000.
47  for(Index i = 0; i < m_cells.size() - 1; i++){
48  if(vals.at(m_cells[i]) == 1)
49  return 1000;
50  }
51 
52  // check if the last cell hit by the laser is 1. return 900 otherwise.
53  if(vals.at(m_cells[m_cells.size() - 1]) == 0)
54  return 900;
55 
56  return 1;
57 
58  }
59 
61  virtual DecisionTreeFactor operator*(const DecisionTreeFactor&) const{
62  throw runtime_error("operator * not implemented");
63  }
64 
65  virtual DecisionTreeFactor toDecisionTreeFactor() const{
66  throw runtime_error("DecisionTreeFactor toDecisionTreeFactor not implemented");
67  }
68 };
69 
76 class OccupancyGrid : public DiscreteFactorGraph {
77 private:
78  size_t width_; //number of cells wide the grid is
79  size_t height_; //number of cells tall the grid is
80  double res_; //the resolution at which the grid is created
81 
82  vector<Index> cells_; //list of keys of all cells in the grid
83  vector<Index> laser_indices_; //indices of the laser factor in factors_
84 
85 
86 public:
87 
88  size_t width() const {
89  return width_;
90  }
91  size_t height() const {
92  return height_;
93  }
94  // should we just not typedef Values Occupancy; ?
95  class Occupancy : public Values {
96  private:
97  public:
98  };
99 
100 
101  typedef std::vector<double> Marginals;
104  OccupancyGrid(double width, double height, double resolution){
105  width_ = width/resolution;
106  height_ = height/resolution;
107  res_ = resolution;
108 
109  for(Index i = 0; i < cellCount(); i++)
110  cells_.push_back(i);
111  }
112 
114  Occupancy emptyOccupancy(){
115  Occupancy occupancy; //mapping from Index to value (0 or 1)
116  for(size_t i = 0; i < cellCount(); i++)
117  occupancy.insert(pair<Index, size_t>((Index)i,0));
118 
119  return occupancy;
120  }
121 
123  void addPosePrior(Index cell, double prior){
124  size_t numStates = 2;
125  DiscreteKey key(cell, numStates);
126 
127  //add a factor
128  vector<double> table(2);
129  table[0] = 1-prior;
130  table[1] = prior;
131  add(key, table);
132  }
133 
135  void addLaser(const Pose2 &pose, double range){
136  //ray trace from pose to range t//a >= 1 accept new stateo find all cells the laser passes through
137  double x = pose.x(); //start position of the laser
138  double y = pose.y();
139  double step = res_/8.0; //amount to step in each iteration of laser traversal
140 
141  Index key;
142  vector<Index> cells; //list of keys of cells hit by the laser
143 
144  //traverse laser
145  for(double i = 0; i < range; i += step){
146  //get point on laser
147  x = pose.x() + i*cos(pose.theta());
148  y = pose.y() + i*sin(pose.theta());
149 
150  //printf("%lf %lf\n", x, y);
151  //get the key of the cell that holds point (x,y)
152  key = keyLookup(x,y);
153 
154  //add cell to list of cells if it is new
155  if(i == 0 || key != cells[cells.size()-1])
156  cells.push_back(key);
157  }
158 
159 // for(size_t i = 0; i < cells.size(); i++)
160 // printf("%ld ", cells[i]);
161 // printf("\n");
162 
163  //add a factor that connects all those cells
164  laser_indices_.push_back(factors_.size());
165  push_back(std::make_shared<LaserFactor>(cells));
166 
167  }
168 
170  size_t cellCount() const {
171  return width_*height_;
172  }
173 
175  Index keyLookup(double x, double y) const {
176  //move (x,y) to the nearest resolution
177  x *= (1.0/res_);
178  y *= (1.0/res_);
179 
180  //round to nearest integer
181  x = (double)((int)x);
182  y = (double)((int)y);
183 
184  //determine index
185  x += width_/2;
186  y = height_/2 - y;
187 
188  //bounds checking
189  size_t index = y*width_ + x;
190  index = index >= width_*height_ ? -1 : index;
191 
192  return cells_[index];
193  }
194 
201  double laserFactorValue(Index index, const Occupancy &occupancy) const{
202  return (*factors_[ laser_indices_[index] ])(occupancy);
203  }
204 
206  double operator()(const Occupancy &occupancy) const {
207  double value = 0;
208 
209  // loop over all laser factors in the graph
210  //printf("%ld\n", (*this).size());
211 
212  for(Index i = 0; i < laser_indices_.size(); i++){
213  value += laserFactorValue(i, occupancy);
214  }
215 
216  return value;
217  }
218 
224  Marginals runMetropolis(size_t iterations){
225  Occupancy occupancy = emptyOccupancy();
226 
227  size_t size = cellCount();
228  Marginals marginals(size);
229 
230  // NOTE: using older interface for boost.random due to interface changes after boost 1.46
231  std::mt19937 rng;
232  std::uniform_int_distribution<> random_cell(0, size - 1);
233 
234  // run Metropolis for the requested number of operations
235  // compute initial probability of occupancy grid, P(x_t)
236 
237  double Px = (*this)(occupancy);
238 
239  for(size_t it = 0; it < marginals.size(); it++)
240  marginals[it] = 0;
241 
242  for(size_t it = 0; it < iterations; it++){
243  //choose a random cell
244  Index x = random_cell(rng);
245  //printf("%ld:",x);
246  //flip the state of a random cell, x
247  occupancy[x] = 1 - occupancy[x];
248 
249  //compute probability of new occupancy grid, P(x')
250  //by summing over all LaserFactor::operator()
251  double Px_prime = (*this)(occupancy);
252 
253  //occupancy.print();
254  //calculate acceptance ratio, a
255  double a = Px_prime/Px;
256 
257  //if a <= 1 otherwise accept with probability a
258  //if we accept the new state P(x_t) = P(x')
259  // printf(" %.3lf %.3lf\t", Px, Px_prime);
260  if(a <= 1){
261  Px = Px_prime;
262  //printf("\taccept\n");
263  }
264  else{
265  occupancy[x] = 1 - occupancy[x];
266  // printf("\treject\n");
267  }
268 
269  //increment the number of iterations each cell has been on
270  for(size_t i = 0; i < size; i++){
271  if(occupancy[i] == 1)
272  marginals[i]++;
273  }
274  }
275 
276  //compute the marginals
277  for(size_t it = 0; it < size; it++)
278  marginals[it] /= iterations;
279 
280  return marginals;
281  }
282 
283 };
284 
285 /* ************************************************************************* */
286 TEST( OccupancyGrid, Test1) {
287  //Build a small grid and test optimization
288 
289  //Build small grid
290  double width = 3; //meters
291  double height = 2; //meters
292  double resolution = 0.5; //meters
293  OccupancyGrid occupancyGrid(width, height, resolution); //default center to middle
294 
295  //Add measurements
296  Pose2 pose(0,0,0);
297  double range = 1;
298 
299  occupancyGrid.addPosePrior(0, 0.7);
300  EXPECT_LONGS_EQUAL(1, occupancyGrid.size());
301 
302  occupancyGrid.addLaser(pose, range);
303  EXPECT_LONGS_EQUAL(2, occupancyGrid.size());
304 
305  OccupancyGrid::Occupancy occupancy = occupancyGrid.emptyOccupancy();
306  EXPECT_LONGS_EQUAL(900, occupancyGrid.laserFactorValue(0,occupancy));
307 
308 
309  occupancy[16] = 1;
310  EXPECT_LONGS_EQUAL(1, occupancyGrid.laserFactorValue(0,occupancy));
311 
312  occupancy[15] = 1;
313  EXPECT_LONGS_EQUAL(1000, occupancyGrid.laserFactorValue(0,occupancy));
314 
315  occupancy[16] = 0;
316  EXPECT_LONGS_EQUAL(1000, occupancyGrid.laserFactorValue(0,occupancy));
317 
318 
319  //run MCMC
320  OccupancyGrid::Marginals occupancyMarginals = occupancyGrid.runMetropolis(50000);
321  EXPECT_LONGS_EQUAL( (width*height)/pow(resolution,2), occupancyMarginals.size());
322 
323 
324 
325 }
326 
327 #endif
328 
329 /* ************************************************************************* */
330 int main() {
331  TestResult tr;
332  return TestRegistry::runAllTests(tr);
333 }
334 /* ************************************************************************* */
335 
const gtsam::Symbol key('X', 0)
Vector3_ operator*(const Double_ &s, const Vector3_ &v)
def step(data, isam, result, truth, currPoseIndex)
Definition: visual_isam.py:82
Jet< T, N > cos(const Jet< T, N > &f)
Definition: jet.h:426
Scalar * y
static int runAllTests(TestResult &result)
Point2 prior(const Point2 &x)
Prior on a single pose.
Definition: simulated2D.h:88
const ValueType at(Key j) const
Definition: Values-inl.h:204
static std::mt19937 rng
Jet< T, N > sin(const Jet< T, N > &f)
Definition: jet.h:439
Definition: BFloat16.h:88
double y() const
get y
Definition: Pose2.h:249
Scalar Scalar int size
Definition: benchVecAdd.cpp:17
EIGEN_DEFAULT_DENSE_INDEX_TYPE Index
The Index type as used for the API.
Definition: Meta.h:74
double theta() const
get theta
Definition: Pose2.h:252
int main()
traits
Definition: chartTesting.h:28
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()), Point3(0, 0, 0.5))
#define EXPECT_LONGS_EQUAL(expected, actual)
Definition: Test.h:154
ArrayXXf table(10, 4)
Double_ range(const Point2_ &p, const Point2_ &q)
std::pair< Key, size_t > DiscreteKey
Definition: DiscreteKey.h:38
graph add(PriorFactor< Pose2 >(1, priorMean, priorNoise))
Jet< T, N > pow(const Jet< T, N > &f, double g)
Definition: jet.h:570
2D Pose
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
internal::enable_if< internal::valid_indexed_view_overload< RowIndices, ColIndices >::value &&internal::traits< typename EIGEN_INDEXED_VIEW_METHOD_TYPE< RowIndices, ColIndices >::type >::ReturnAsIndexedView, typename EIGEN_INDEXED_VIEW_METHOD_TYPE< RowIndices, ColIndices >::type >::type operator()(const RowIndices &rowIndices, const ColIndices &colIndices) EIGEN_INDEXED_VIEW_METHOD_CONST
double x() const
get x
Definition: Pose2.h:246
#define TEST(testGroup, testName)
Definition: Test.h:63
Marginals marginals(graph, result)


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:38:52