1/**
2 * @file testOccupancyGrid.cpp
3 * @date May 14, 2012
4 * @author Brian Peasley
5 * @author Frank Dellaert
6 */
7
8#include <CppUnitLite/TestHarness.h>
9
10#if 0
11
12#include <gtsam/discrete/DiscreteFactorGraph.h>
13#include <gtsam/geometry/Pose2.h>
14
15#include <vector>
16#include <cmath.>
17#include <random>
18
19#include <stdlib.h>
20
21using namespace std;
22using namespace gtsam;
23
24
25/**
26 * Laser Factor
27 * @brief factor that encodes a laser measurements likelihood.
28 */
29
30class LaserFactor : public DiscreteFactor{
31private:
32 vector<Index> m_cells; ///cells in which laser passes through
33
34public:
35
36 ///constructor
37 LaserFactor(const vector<Index> &cells) : m_cells(cells) {}
38
39 /**
40 * Find value for given assignment of values to variables
41 * return 1000 if any of the non-last cell is occupied and 1 otherwise
42 * Values contains all occupancy values (0 or 1)
43 */
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
60 /// Multiply in a DecisionTreeFactor and return the result as DecisionTreeFactor
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
70/**
71 * OccupancyGrid Class
72 * An occupancy grid is just a factor graph.
73 * Every cell in the occupancy grid is a variable in the factor graph.
74 * Measurements will create factors, as well as the prior.
75 */
76class OccupancyGrid : public DiscreteFactorGraph {
77private:
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
86public:
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;
102 ///constructor
103 ///Creates a 2d grid of cells with the origin in the center of the grid
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
113 /// Returns an empty occupancy grid of size width_ x height_
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
122 ///add a prior
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
134 ///add a laser measurement
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
169 /// returns the number of cells in the grid
170 size_t cellCount() const {
171 return width_*height_;
172 }
173
174 /// returns the key of the cell in which point (x,y) lies.
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
195 /**
196 * @brief Computes the value of a laser factor
197 * @param index defines which laser is to be used
198 * @param occupancy defines the grid which the laser will be evaulated with
199 * @ret a double value that is the value of the specified laser factor for the grid
200 */
201 double laserFactorValue(Index index, const Occupancy &occupancy) const{
202 return (*factors_[ laser_indices_[index] ])(occupancy);
203 }
204
205 /// returns the sum of the laser factors for the current state of the grid
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
219 /**
220 * @brief Run a metropolis sampler.
221 * @param iterations defines the number of iterations to run.
222 * @return vector of marginal probabilities.
223 */
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/* ************************************************************************* */
286TEST( 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/* ************************************************************************* */
330int main() {
331 TestResult tr;
332 return TestRegistry::runAllTests(result&: tr);
333}
334/* ************************************************************************* */
335
336