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



Page generated by Doxygen 1.8.14 for MRPT 2.0.1 Git: 0fef1a6d7 Fri Apr 3 23:00:21 2020 +0200 at vie abr 3 23:20:28 CEST 2020