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 /** @class OccupancyGrid <plugins/colli/utils/occupancygrid/occupancygrid.h>
28  * Occupancy Grid class for general use. Many derivated classes
29  * exist, which are usually used instead of this general class.
30  * Note: the coord system is assumed to map x onto width an y onto
31  * height, with x being the first coordinate !
32  */
33 
34 /** Constructs an empty occupancy grid
35  *
36  * @param width the width of the grid in # of cells
37  * @param height the height of the cells in # of cells
38  * @param cell_width the cell width in cm
39  * @param cell_height the cell height in cm
40  */
41 OccupancyGrid::OccupancyGrid(int width, int height, int cell_width, int cell_height)
42 {
43  width_ = width;
44  height_ = height;
45  cell_width_ = cell_width;
46  cell_height_ = cell_height;
47 
48  init_grid();
49 }
50 
51 /** Destructor */
53 {
54  occupancy_probs_.clear();
55 }
56 
57 /** Get the cell width
58  * @return the cell width in cm
59  */
60 int
62 {
63  return cell_width_;
64 }
65 
66 /** Get the cell height
67  * @return the height of the cells in cm
68  */
69 int
71 {
72  return cell_height_;
73 }
74 
75 /** Get the width of the grid
76  * @return the width of the grid in # of cells
77  */
78 int
80 {
81  return width_;
82 }
83 
84 /** Get the height of the grid
85  * @return the height of the grid in # cells
86  */
87 int
89 {
90  return height_;
91 }
92 
93 /** Resets the cell width
94  * @param width the width of the cells in cm
95  */
96 void
98 {
99  cell_width_ = width;
100 }
101 
102 /** Resets the cell height
103  * @param height the height of the cells in cm
104  */
105 void
107 {
108  cell_height_ = height;
109 }
110 
111 /** Resets the width of the grid and constructs a new empty grid
112  * @param width the cell width in cm
113  */
114 void
115 OccupancyGrid::set_width(int width)
116 {
117  width_ = width;
118  init_grid();
119 }
120 
121 /** Resets the height of the grid and constructs a new empty grid
122  * @param height the height of the grid in # of cells
123  */
124 void
125 OccupancyGrid::set_height(int height)
126 {
127  height_ = height;
128  init_grid();
129 }
130 
131 /** Reset the occupancy probability of a cell
132  * @param x the x-position of the cell
133  * @param y the y-position of the cell
134  * @param prob the occupancy probability of cell (x,y)
135  */
136 void
137 OccupancyGrid::set_prob(int x, int y, Probability prob)
138 {
139  if ((x < width_) && (y < height_) && ((isProb(prob)) || (prob == 2.f)))
140  occupancy_probs_[x][y] = prob;
141 }
142 
143 /** Resets all occupancy probabilities
144  * @param prob the occupancy probability the grid will become filled with
145  */
146 void
148 {
149  if ((isProb(prob)) || (prob == -1.f)) {
150  for (int x = 0; x < width_; x++) {
151  for (int y = 0; y < height_; y++) {
152  occupancy_probs_[x][y] = prob;
153  }
154  }
155  }
156 }
157 
158 /** Get the occupancy probability of a cell
159  * @param x the x-position of the cell
160  * @param y the y-position of the cell
161  * @return the occupancy probability of cell (x,y)
162  */
164 OccupancyGrid::get_prob(int x, int y)
165 {
166  if ((x >= 0) && (x < width_) && (y >= 0) && (y < height_)) {
167  return occupancy_probs_[x][y];
168  } else {
169  return 1;
170  }
171 }
172 
173 /** Operator (), get occupancy probability of a cell
174  * @param x the x-position of the cell
175  * @param y the y-position of the cell
176  * @return the occupancy probability of cell (x,y)
177  */
178 Probability &
179 OccupancyGrid::operator()(const int x, const int y)
180 {
181  return occupancy_probs_[x][y];
182 }
183 
184 /** Init a new empty grid with the predefined parameters */
185 void
187 {
188  occupancy_probs_.clear();
189  std::vector<Probability> column;
190  column.resize(height_, 0.f);
191  occupancy_probs_.resize(width_, column);
192  fill(0.f);
193 }
194 
195 } // namespace fawkes
fawkes::OccupancyGrid::set_cell_width
void set_cell_width(int cell_width)
Resets the cell width (in cm)
Definition: occupancygrid.cpp:101
fawkes::OccupancyGrid::~OccupancyGrid
virtual ~OccupancyGrid()
Destructor.
Definition: occupancygrid.cpp:56
fawkes::OccupancyGrid::get_width
int get_width()
Get the width of the grid.
Definition: occupancygrid.cpp:83
fawkes::OccupancyGrid::height_
int height_
Height of the grid in # cells.
Definition: occupancygrid.h:91
fawkes::OccupancyGrid::width_
int width_
Width of the grid in # cells.
Definition: occupancygrid.h:90
fawkes::OccupancyGrid::fill
void fill(Probability prob)
Resets all occupancy probabilities.
Definition: occupancygrid.cpp:151
fawkes::isProb
bool isProb(Probability p)
Check if the probability value is valid.
Definition: probability.h:40
fawkes::OccupancyGrid::operator()
Probability & operator()(const int x, const int y)
Get the occupancy probability of a cell.
Definition: occupancygrid.cpp:183
fawkes::OccupancyGrid::cell_width_
int cell_width_
Cell width in cm.
Definition: occupancygrid.h:88
fawkes::OccupancyGrid::set_prob
virtual void set_prob(int x, int y, Probability prob)
Reset the occupancy probability of a cell.
Definition: occupancygrid.cpp:141
fawkes::OccupancyGrid::set_cell_height
void set_cell_height(int cell_height)
Resets the cell height (in cm)
Definition: occupancygrid.cpp:110
fawkes::OccupancyGrid::get_prob
Probability get_prob(int x, int y)
Get the occupancy probability of a cell.
Definition: occupancygrid.cpp:168
fawkes::OccupancyGrid::cell_height_
int cell_height_
Cell height in cm.
Definition: occupancygrid.h:89
fawkes
fawkes::Probability
float Probability
A probability type.
Definition: probability.h:33
fawkes::OccupancyGrid::init_grid
void init_grid()
Init a new empty grid with the predefined parameters *‍/.
Definition: occupancygrid.cpp:190
fawkes::OccupancyGrid::occupancy_probs_
std::vector< std::vector< Probability > > occupancy_probs_
The occupancy probability of the cells in a 2D array.
Definition: occupancygrid.h:85
fawkes::OccupancyGrid::OccupancyGrid
OccupancyGrid(int width, int height, int cell_width=5, int cell_height=5)
Constructs an empty occupancy grid.
Definition: occupancygrid.cpp:45
fawkes::OccupancyGrid::set_width
void set_width(int width)
Resets the width of the grid and constructs a new empty grid.
Definition: occupancygrid.cpp:119
fawkes::OccupancyGrid::set_height
void set_height(int height)
Resets the height of the grid and constructs a new empty grid.
Definition: occupancygrid.cpp:129
fawkes::OccupancyGrid::get_cell_width
int get_cell_width()
Get the cell width (in cm)
Definition: occupancygrid.cpp:65
fawkes::OccupancyGrid::get_height
int get_height()
Get the height of the grid.
Definition: occupancygrid.cpp:92
fawkes::OccupancyGrid::get_cell_height
int get_cell_height()
Get the cell height (in cm)
Definition: occupancygrid.cpp:74