Fawkes API  Fawkes Development Version
occupancygrid.cpp
1 
2 /***************************************************************************
3  * occupancygrid.cpp - An occupancy-grid
4  *
5  * Created: Fri Oct 18 15:16:23 2013
6  * Copyright 2002 AllemaniACs
7  * 2013-2014 Bahram Maleki-Fard
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "occupancygrid.h"
24 
25 namespace fawkes
26 {
27 #if 0 /* just to make Emacs auto-indent happy */
28 }
29 #endif
30 
31 /** @class OccupancyGrid <plugins/colli/utils/occupancygrid/occupancygrid.h>
32  * Occupancy Grid class for general use. Many derivated classes
33  * exist, which are usually used instead of this general class.
34  * Note: the coord system is assumed to map x onto width an y onto
35  * height, with x being the first coordinate !
36  */
37 
38 /** Constructs an empty occupancy grid
39  *
40  * @param width the width of the grid in # of cells
41  * @param height the height of the cells in # of cells
42  * @param cell_width the cell width in cm
43  * @param cell_height the cell height in cm
44  */
45 OccupancyGrid::OccupancyGrid(int width, int height, int cell_width, int cell_height)
46 {
47  width_ = width;
48  height_ = height;
49  cell_width_ = cell_width;
50  cell_height_ = cell_height;
51 
52  init_grid();
53 }
54 
55 /** Destructor */
57 {
58  occupancy_probs_.clear();
59 }
60 
61 /** Get the cell width
62  * @return the cell width in cm
63  */
64 int
66 {
67  return cell_width_;
68 }
69 
70 /** Get the cell height
71  * @return the height of the cells in cm
72  */
73 int
75 {
76  return cell_height_;
77 }
78 
79 /** Get the width of the grid
80  * @return the width of the grid in # of cells
81  */
82 int
84 {
85  return width_;
86 }
87 
88 /** Get the height of the grid
89  * @return the height of the grid in # cells
90  */
91 int
93 {
94  return height_;
95 }
96 
97 /** Resets the cell width
98  * @param width the width of the cells in cm
99  */
100 void
102 {
103  cell_width_ = width;
104 }
105 
106 /** Resets the cell height
107  * @param height the height of the cells in cm
108  */
109 void
111 {
112  cell_height_ = height;
113 }
114 
115 /** Resets the width of the grid and constructs a new empty grid
116  * @param width the cell width in cm
117  */
118 void
120 {
121  width_ = width;
122  init_grid();
123 }
124 
125 /** Resets the height of the grid and constructs a new empty grid
126  * @param height the height of the grid in # of cells
127  */
128 void
130 {
131  height_ = height;
132  init_grid();
133 }
134 
135 
136 /** Reset the occupancy probability of a cell
137  * @param x the x-position of the cell
138  * @param y the y-position of the cell
139  * @param prob the occupancy probability of cell (x,y)
140  */
141 void
143 {
144  if( (x < width_) && (y < height_) && ((isProb(prob)) || (prob == 2.f)) )
145  occupancy_probs_[x][y] = prob;
146 }
147 
148 /** Resets all occupancy probabilities
149  * @param prob the occupancy probability the grid will become filled with
150  */
151 void
153 {
154  if((isProb(prob)) || (prob == -1.f)) {
155  for(int x = 0; x < width_; x++) {
156  for(int y = 0; y < height_; y++) {
157  occupancy_probs_[x][y] = prob;
158  }
159  }
160  }
161 }
162 
163 /** Get the occupancy probability of a cell
164  * @param x the x-position of the cell
165  * @param y the y-position of the cell
166  * @return the occupancy probability of cell (x,y)
167  */
170 {
171  if( (x >= 0) && (x < width_) && (y >= 0) && (y < height_) ) {
172  return occupancy_probs_[x][y];
173  } else {
174  return 1;
175  }
176 }
177 
178 /** Operator (), get occupancy probability of a cell
179  * @param x the x-position of the cell
180  * @param y the y-position of the cell
181  * @return the occupancy probability of cell (x,y)
182  */
184 OccupancyGrid::operator () (const int x, const int y)
185 {
186  return occupancy_probs_[x][y];
187 }
188 
189 /** Init a new empty grid with the predefined parameters */
190 void
192 {
193  occupancy_probs_.clear();
194  std::vector<Probability> column;
195  column.resize(height_, 0.f);
196  occupancy_probs_.resize(width_, column);
197  fill( 0.f );
198 }
199 
200 } // namespace fawkes
void fill(Probability prob)
Resets all occupancy probabilities.
float Probability
A probability type.
Definition: probability.h:33
int get_width()
Get the width of the grid.
int get_height()
Get the height of the grid.
Probability & operator()(const int x, const int y)
Get the occupancy probability of a cell.
int get_cell_width()
Get the cell width (in cm)
Fawkes library namespace.
void set_cell_height(int cell_height)
Resets the cell height (in cm)
int get_cell_height()
Get the cell height (in cm)
Probability get_prob(int x, int y)
Get the occupancy probability of a cell.
int cell_height_
Cell height in cm.
Definition: occupancygrid.h:89
int width_
Width of the grid in # cells.
Definition: occupancygrid.h:90
bool isProb(Probability p)
Check if the probability value is valid.
Definition: probability.h:40
virtual ~OccupancyGrid()
Destructor.
int cell_width_
Cell width in cm.
Definition: occupancygrid.h:88
int height_
Height of the grid in # cells.
Definition: occupancygrid.h:91
void set_width(int width)
Resets the width of the grid and constructs a new empty grid.
virtual void set_prob(int x, int y, Probability prob)
Reset the occupancy probability of a cell.
void set_height(int height)
Resets the height of the grid and constructs a new empty grid.
void init_grid()
Init a new empty grid with the predefined parameters */.
std::vector< std::vector< Probability > > occupancy_probs_
The occupancy probability of the cells in a 2D array.
Definition: occupancygrid.h:85
OccupancyGrid(int width, int height, int cell_width=5, int cell_height=5)
Constructs an empty occupancy grid.
void set_cell_width(int cell_width)
Resets the cell width (in cm)