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
25namespace 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 */
41OccupancyGrid::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 */
60int
62{
63 return cell_width_;
64}
65
66/** Get the cell height
67 * @return the height of the cells in cm
68 */
69int
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 */
78int
80{
81 return width_;
82}
83
84/** Get the height of the grid
85 * @return the height of the grid in # cells
86 */
87int
89{
90 return height_;
91}
92
93/** Resets the cell width
94 * @param width the width of the cells in cm
95 */
96void
98{
99 cell_width_ = width;
100}
101
102/** Resets the cell height
103 * @param height the height of the cells in cm
104 */
105void
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 */
114void
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 */
124void
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 */
136void
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 */
146void
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 */
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 */
179OccupancyGrid::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 */
185void
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
int cell_height_
Cell height in cm.
Definition: occupancygrid.h:85
void set_width(int width)
Resets the width of the grid and constructs a new empty grid.
void init_grid()
Init a new empty grid with the predefined parameters *‍/.
Probability get_prob(int x, int y)
Get the occupancy probability of a cell.
void set_height(int height)
Resets the height of the grid and constructs a new empty grid.
int get_cell_width()
Get the cell width (in cm)
Probability & operator()(const int x, const int y)
Get the occupancy probability of a cell.
int height_
Height of the grid in # cells.
Definition: occupancygrid.h:87
int get_height()
Get the height of the grid.
int get_width()
Get the width of the grid.
std::vector< std::vector< Probability > > occupancy_probs_
The occupancy probability of the cells in a 2D array.
Definition: occupancygrid.h:81
void set_cell_width(int cell_width)
Resets the cell width (in cm)
OccupancyGrid(int width, int height, int cell_width=5, int cell_height=5)
Constructs an empty occupancy grid.
int width_
Width of the grid in # cells.
Definition: occupancygrid.h:86
void set_cell_height(int cell_height)
Resets the cell height (in cm)
int get_cell_height()
Get the cell height (in cm)
virtual void set_prob(int x, int y, Probability prob)
Reset the occupancy probability of a cell.
int cell_width_
Cell width in cm.
Definition: occupancygrid.h:84
virtual ~OccupancyGrid()
Destructor.
void fill(Probability prob)
Resets all occupancy probabilities.
Fawkes library namespace.
bool isProb(Probability p)
Check if the probability value is valid.
Definition: probability.h:36
float Probability
A probability type.
Definition: probability.h:29