Main MRPT website > C++ reference for MRPT 1.9.9
CDynamicGrid3D.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #pragma once
10 
11 #include <mrpt/core/round.h>
12 #include <vector>
13 #include <cstdint>
14 #include <cmath>
15 #include <cstddef>
16 
17 namespace mrpt
18 {
19 namespace containers
20 {
21 /** A 3D rectangular grid of dynamic size which stores any kind of data at each
22  * voxel.
23  * \tparam T The type of each voxel in the grid.
24  * \ingroup mrpt_containers_grp
25  */
26 template <class T>
28 {
29  public:
30  /** Constructor */
32  double x_min = -1.0, double x_max = 1.0, double y_min = -1.0,
33  double y_max = +1.0, double z_min = -1.0, double z_max = 1.0,
34  double resolution_xy = 0.5, double resolution_z = 0.5)
35  : m_map()
36  {
37  setSize(
38  x_min, x_max, y_min, y_max, z_min, z_max, resolution_xy,
39  resolution_z);
40  }
41 
42  /** Changes the size of the grid, maintaining previous contents.
43  * \sa setSize
44  */
45  virtual void resize(
46  double new_x_min, double new_x_max, double new_y_min, double new_y_max,
47  double new_z_min, double new_z_max, const T& defaultValueNewCells,
48  double additionalMarginMeters = 2.0)
49  {
50  // Is resize really necesary?
51  if (new_x_min >= m_x_min && new_y_min >= m_y_min &&
52  new_z_min >= m_z_min && new_x_max <= m_x_max &&
53  new_y_max <= m_y_max && new_z_max <= m_z_max)
54  return;
55 
56  if (new_x_min > m_x_min) new_x_min = m_x_min;
57  if (new_x_max < m_x_max) new_x_max = m_x_max;
58  if (new_y_min > m_y_min) new_y_min = m_y_min;
59  if (new_y_max < m_y_max) new_y_max = m_y_max;
60  if (new_z_min > m_z_min) new_z_min = m_z_min;
61  if (new_z_max < m_z_max) new_z_max = m_z_max;
62 
63  // Additional margin:
64  if (additionalMarginMeters > 0)
65  {
66  if (new_x_min < m_x_min)
67  new_x_min = floor(new_x_min - additionalMarginMeters);
68  if (new_x_max > m_x_max)
69  new_x_max = ceil(new_x_max + additionalMarginMeters);
70  if (new_y_min < m_y_min)
71  new_y_min = floor(new_y_min - additionalMarginMeters);
72  if (new_y_max > m_y_max)
73  new_y_max = ceil(new_y_max + additionalMarginMeters);
74  if (new_z_min < m_z_min)
75  new_z_min = floor(new_z_min - additionalMarginMeters);
76  if (new_z_max > m_z_max)
77  new_z_max = ceil(new_z_max + additionalMarginMeters);
78  }
79 
80  // Adjust sizes to adapt them to full sized cells acording to the
81  // resolution:
82  if (fabs(
83  new_x_min / m_resolution_xy -
84  round(new_x_min / m_resolution_xy)) > 0.05)
85  new_x_min = m_resolution_xy * round(new_x_min / m_resolution_xy);
86  if (fabs(
87  new_y_min / m_resolution_xy -
88  round(new_y_min / m_resolution_xy)) > 0.05)
89  new_y_min = m_resolution_xy * round(new_y_min / m_resolution_xy);
90  if (fabs(
91  new_z_min / m_resolution_z -
92  round(new_z_min / m_resolution_z)) > 0.05)
93  new_z_min = m_resolution_z * round(new_z_min / m_resolution_z);
94  if (fabs(
95  new_x_max / m_resolution_xy -
96  round(new_x_max / m_resolution_xy)) > 0.05)
97  new_x_max = m_resolution_xy * round(new_x_max / m_resolution_xy);
98  if (fabs(
99  new_y_max / m_resolution_xy -
100  round(new_y_max / m_resolution_xy)) > 0.05)
101  new_y_max = m_resolution_xy * round(new_y_max / m_resolution_xy);
102  if (fabs(
103  new_z_max / m_resolution_z -
104  round(new_z_max / m_resolution_z)) > 0.05)
105  new_z_max = m_resolution_z * round(new_z_max / m_resolution_z);
106 
107  // Change the map size: Extensions at each side:
108  size_t extra_x_izq = round((m_x_min - new_x_min) / m_resolution_xy);
109  size_t extra_y_arr = round((m_y_min - new_y_min) / m_resolution_xy);
110  size_t extra_z_top = round((m_z_min - new_z_min) / m_resolution_z);
111 
112  size_t new_size_x = round((new_x_max - new_x_min) / m_resolution_xy);
113  size_t new_size_y = round((new_y_max - new_y_min) / m_resolution_xy);
114  size_t new_size_z = round((new_z_max - new_z_min) / m_resolution_z);
115  size_t new_size_x_times_y = new_size_x * new_size_y;
116 
117  // Reserve new memory:
118  typename std::vector<T> new_map;
119  new_map.resize(
120  new_size_x * new_size_y * new_size_z, defaultValueNewCells);
121 
122  // Copy previous rows:
123  size_t x, y, z;
124  typename std::vector<T>::iterator itSrc, itDst;
125  for (z = 0; z < m_size_z; z++)
126  {
127  for (y = 0; y < m_size_y; y++)
128  {
129  for (x = 0,
130  itSrc =
131  (m_map.begin() + y * m_size_x + z * m_size_x_times_y),
132  itDst =
133  (new_map.begin() + extra_x_izq +
134  (y + extra_y_arr) * new_size_x +
135  (z + extra_z_top) * new_size_x_times_y);
136  x < m_size_x; ++x, ++itSrc, ++itDst)
137  {
138  *itDst = *itSrc;
139  }
140  }
141  }
142 
143  // Update the new map limits:
144  m_x_min = new_x_min;
145  m_x_max = new_x_max;
146  m_y_min = new_y_min;
147  m_y_max = new_y_max;
148  m_z_min = new_z_min;
149  m_z_max = new_z_max;
150 
151  m_size_x = new_size_x;
152  m_size_y = new_size_y;
153  m_size_z = new_size_z;
154  m_size_x_times_y = new_size_x_times_y;
155 
156  // Keep the new map only:
157  m_map.swap(new_map);
158  }
159 
160  /** Changes the size of the grid, ERASING all previous contents.
161  * If \a fill_value is left as nullptr, the contents of cells may be
162  * undefined (some will remain with
163  * their old values, the new ones will have the default voxel value, but
164  * the location of old values
165  * may change wrt their old places).
166  * If \a fill_value is not nullptr, it is assured that all cells will have
167  * a copy of that value after resizing.
168  * If `resolution_z`<0, the same resolution will be used for all dimensions
169  * x,y,z as given in `resolution_xy`
170  * \sa resize, fill
171  */
172  virtual void setSize(
173  const double x_min, const double x_max, const double y_min,
174  const double y_max, const double z_min, const double z_max,
175  const double resolution_xy, const double resolution_z_ = -1.0,
176  const T* fill_value = nullptr)
177  {
178  const double resolution_z =
179  resolution_z_ > 0.0 ? resolution_z_ : resolution_xy;
180 
181  // Adjust sizes to adapt them to full sized cells acording to the
182  // resolution:
183  m_x_min = x_min;
184  m_y_min = y_min;
185  m_z_min = z_min;
186 
187  m_x_max =
188  x_min + resolution_xy * round((x_max - x_min) / resolution_xy);
189  m_y_max =
190  y_min + resolution_xy * round((y_max - y_min) / resolution_xy);
191  m_z_max = z_min + resolution_z * round((z_max - z_min) / resolution_z);
192 
193  // Res:
194  m_resolution_xy = resolution_xy;
195  m_resolution_z = resolution_z;
196 
197  // Now the number of cells should be integers:
202 
203  // Cells memory:
204  if (fill_value)
205  m_map.assign(m_size_x * m_size_y * m_size_z, *fill_value);
206  else
207  m_map.resize(m_size_x * m_size_y * m_size_z);
208  }
209 
210  /** Erase the contents of all the cells, setting them to their default
211  * values (default ctor). */
212  virtual void clear()
213  {
214  m_map.clear();
215  m_map.resize(m_size_x * m_size_y * m_size_z);
216  }
217 
218  /** Fills all the cells with the same value
219  */
220  inline void fill(const T& value)
221  {
222  for (typename std::vector<T>::iterator it = m_map.begin();
223  it != m_map.end(); ++it)
224  *it = value;
225  }
226 
227  static const size_t INVALID_VOXEL_IDX = size_t(-1);
228 
229  /** Gets the absolute index of a voxel in the linear container m_map[] from
230  * its cx,cy,cz indices, or -1 if out of map bounds (in any dimension). \sa
231  * x2idx(), y2idx(), z2idx() */
232  inline size_t cellAbsIndexFromCXCYCZ(
233  const int cx, const int cy, const int cz) const
234  {
235  if (cx < 0 || cx >= static_cast<int>(m_size_x))
236  return INVALID_VOXEL_IDX;
237  if (cy < 0 || cy >= static_cast<int>(m_size_y))
238  return INVALID_VOXEL_IDX;
239  if (cz < 0 || cz >= static_cast<int>(m_size_z))
240  return INVALID_VOXEL_IDX;
241  return cx + cy * m_size_x + cz * m_size_x_times_y;
242  }
243 
244  /** Returns a pointer to the contents of a voxel given by its coordinates,
245  * or nullptr if it is out of the map extensions.
246  */
247  inline T* cellByPos(double x, double y, double z)
248  {
249  const size_t cidx =
251  if (cidx == INVALID_VOXEL_IDX) return nullptr;
252  return &m_map[cidx];
253  }
254  /** \overload */
255  inline const T* cellByPos(double x, double y, double z) const
256  {
257  const size_t cidx =
259  if (cidx == INVALID_VOXEL_IDX) return nullptr;
260  return &m_map[cidx];
261  }
262 
263  /** Returns a pointer to the contents of a voxel given by its voxel indexes,
264  * or nullptr if it is out of the map extensions.
265  */
266  inline T* cellByIndex(unsigned int cx, unsigned int cy, unsigned int cz)
267  {
268  const size_t cidx = cellAbsIndexFromCXCYCZ(cx, cy, cz);
269  if (cidx == INVALID_VOXEL_IDX) return nullptr;
270  return &m_map[cidx];
271  }
272 
273  /** Returns a pointer to the contents of a voxel given by its voxel indexes,
274  * or nullptr if it is out of the map extensions.
275  */
276  inline const T* cellByIndex(
277  unsigned int cx, unsigned int cy, unsigned int cz) const
278  {
279  const size_t cidx = cellAbsIndexFromCXCYCZ(cx, cy, cz);
280  if (cidx == INVALID_VOXEL_IDX) return nullptr;
281  return &m_map[cidx];
282  }
283 
284  inline size_t getSizeX() const { return m_size_x; }
285  inline size_t getSizeY() const { return m_size_y; }
286  inline size_t getSizeZ() const { return m_size_z; }
287  inline size_t getVoxelCount() const { return m_size_x_times_y * m_size_z; }
288  inline double getXMin() const { return m_x_min; }
289  inline double getXMax() const { return m_x_max; }
290  inline double getYMin() const { return m_y_min; }
291  inline double getYMax() const { return m_y_max; }
292  inline double getZMin() const { return m_z_min; }
293  inline double getZMax() const { return m_z_max; }
294  inline double getResolutionXY() const { return m_resolution_xy; }
295  inline double getResolutionZ() const { return m_resolution_z; }
296  /** Transform a coordinate values into voxel indexes */
297  inline int x2idx(double x) const
298  {
299  return static_cast<int>((x - m_x_min) / m_resolution_xy);
300  }
301  inline int y2idx(double y) const
302  {
303  return static_cast<int>((y - m_y_min) / m_resolution_xy);
304  }
305  inline int z2idx(double z) const
306  {
307  return static_cast<int>((z - m_z_min) / m_resolution_z);
308  }
309 
310  /** Transform a voxel index into a coordinate value of the voxel central
311  * point */
312  inline double idx2x(int cx) const { return m_x_min + (cx)*m_resolution_xy; }
313  inline double idx2y(int cy) const { return m_y_min + (cy)*m_resolution_xy; }
314  inline double idx2z(int cz) const { return m_z_min + (cz)*m_resolution_z; }
315  protected:
316  /** The cells */
317  mutable std::vector<T> m_map;
318  /** Used only from logically const method that really need to modify the
319  * object */
320  inline std::vector<T>& m_map_castaway_const() const { return m_map; }
324  /** Serialization of all parameters, except the contents of each voxel
325  * (responsability of the derived class) */
326  template <class ARCHIVE>
327  void dyngridcommon_writeToStream(ARCHIVE& out) const
328  {
329  out << m_x_min << m_x_max << m_y_min << m_y_max << m_z_min << m_z_max;
331  out << static_cast<uint32_t>(m_size_x)
332  << static_cast<uint32_t>(m_size_y)
333  << static_cast<uint32_t>(m_size_z);
334  }
335  /** Serialization of all parameters, except the contents of each voxel
336  * (responsability of the derived class) */
337  template <class ARCHIVE>
339  {
340  in >> m_x_min >> m_x_max >> m_y_min >> m_y_max >> m_z_min >> m_z_max;
342 
343  uint32_t nX, nY, nZ;
344  in >> nX >> nY >> nZ;
345  m_size_x = nX;
346  m_size_y = nY;
347  m_size_z = nZ;
348  m_map.resize(nX * nY * nZ);
349  }
350 
351 }; // end of CDynamicGrid3D<>
352 
353 } // namespace containers
354 } // namespace mrpt
Scalar * iterator
Definition: eigen_plugins.h:26
virtual void setSize(const double x_min, const double x_max, const double y_min, const double y_max, const double z_min, const double z_max, const double resolution_xy, const double resolution_z_=-1.0, const T *fill_value=nullptr)
Changes the size of the grid, ERASING all previous contents.
GLdouble GLdouble z
Definition: glext.h:3872
virtual void resize(double new_x_min, double new_x_max, double new_y_min, double new_y_max, double new_z_min, double new_z_max, const T &defaultValueNewCells, double additionalMarginMeters=2.0)
Changes the size of the grid, maintaining previous contents.
size_t cellAbsIndexFromCXCYCZ(const int cx, const int cy, const int cz) const
Gets the absolute index of a voxel in the linear container m_map[] from its cx,cy,cz indices, or -1 if out of map bounds (in any dimension).
const T * cellByIndex(unsigned int cx, unsigned int cy, unsigned int cz) const
Returns a pointer to the contents of a voxel given by its voxel indexes, or nullptr if it is out of t...
T * cellByPos(double x, double y, double z)
Returns a pointer to the contents of a voxel given by its coordinates, or nullptr if it is out of the...
CDynamicGrid3D(double x_min=-1.0, double x_max=1.0, double y_min=-1.0, double y_max=+1.0, double z_min=-1.0, double z_max=1.0, double resolution_xy=0.5, double resolution_z=0.5)
Constructor.
T * cellByIndex(unsigned int cx, unsigned int cy, unsigned int cz)
Returns a pointer to the contents of a voxel given by its voxel indexes, or nullptr if it is out of t...
A 3D rectangular grid of dynamic size which stores any kind of data at each voxel.
int x2idx(double x) const
Transform a coordinate values into voxel indexes.
void dyngridcommon_readFromStream(ARCHIVE &in)
Serialization of all parameters, except the contents of each voxel (responsability of the derived cla...
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
std::vector< T > m_map
The cells.
const T * cellByPos(double x, double y, double z) const
void fill(const T &value)
Fills all the cells with the same value.
GLuint in
Definition: glext.h:7274
void dyngridcommon_writeToStream(ARCHIVE &out) const
Serialization of all parameters, except the contents of each voxel (responsability of the derived cla...
GLenum GLint GLint y
Definition: glext.h:3538
virtual void clear()
Erase the contents of all the cells, setting them to their default values (default ctor)...
double idx2x(int cx) const
Transform a voxel index into a coordinate value of the voxel central point.
std::vector< T > & m_map_castaway_const() const
Used only from logically const method that really need to modify the object.
GLsizei const GLfloat * value
Definition: glext.h:4117
GLenum GLint x
Definition: glext.h:3538
static const size_t INVALID_VOXEL_IDX
unsigned __int32 uint32_t
Definition: rptypes.h:47
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:23



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: ad3a9d8ae Tue May 1 23:10:22 2018 -0700 at lun oct 28 00:14:14 CET 2019