Main MRPT website > C++ reference for MRPT 1.5.7
CDynamicGrid3D.h
Go to the documentation of this file.
1 
2 #pragma once
3 
4 #include <mrpt/utils/core_defs.h>
5 #include <mrpt/utils/round.h>
6 #include <mrpt/utils/CStream.h>
7 #include <vector>
8 #include <string>
9 #define _USE_MATH_DEFINES // (For VS to define M_PI, etc. in cmath)
10 #include <cmath>
11 
12 namespace mrpt
13 {
14  namespace utils
15  {
16  /** A 3D rectangular grid of dynamic size which stores any kind of data at each voxel.
17  * \tparam T The type of each voxel in the grid.
18  */
19  template <class T>
20  class CDynamicGrid3D
21  {
22  public:
23  /** Constructor */
24  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) :
25  m_map()
26  {
27  setSize(x_min,x_max,y_min,y_max,z_min, z_max, resolution_xy, resolution_z);
28  }
29 
30  /** Changes the size of the grid, maintaining previous contents.
31  * \sa setSize
32  */
33  virtual void resize(
34  double new_x_min, double new_x_max,
35  double new_y_min, double new_y_max,
36  double new_z_min, double new_z_max,
37  const T& defaultValueNewCells, double additionalMarginMeters = 2.0)
38  {
39  // Is resize really necesary?
40  if (new_x_min >= m_x_min &&
41  new_y_min >= m_y_min &&
42  new_z_min >= m_z_min &&
43  new_x_max <= m_x_max &&
44  new_y_max <= m_y_max &&
45  new_z_max <= m_z_max)
46  return;
47 
48  if (new_x_min>m_x_min) new_x_min = m_x_min;
49  if (new_x_max<m_x_max) new_x_max = m_x_max;
50  if (new_y_min>m_y_min) new_y_min = m_y_min;
51  if (new_y_max<m_y_max) new_y_max = m_y_max;
52  if (new_z_min>m_z_min) new_z_min = m_z_min;
53  if (new_z_max<m_z_max) new_z_max = m_z_max;
54 
55  // Additional margin:
56  if (additionalMarginMeters>0)
57  {
58  if (new_x_min<m_x_min) new_x_min = floor(new_x_min - additionalMarginMeters);
59  if (new_x_max>m_x_max) new_x_max = ceil(new_x_max + additionalMarginMeters);
60  if (new_y_min<m_y_min) new_y_min = floor(new_y_min - additionalMarginMeters);
61  if (new_y_max>m_y_max) new_y_max = ceil(new_y_max + additionalMarginMeters);
62  if (new_z_min<m_z_min) new_z_min = floor(new_z_min - additionalMarginMeters);
63  if (new_z_max>m_z_max) new_z_max = ceil(new_z_max + additionalMarginMeters);
64  }
65 
66  // Adjust sizes to adapt them to full sized cells acording to the resolution:
67  if (fabs(new_x_min / m_resolution_xy - round(new_x_min / m_resolution_xy))>0.05)
68  new_x_min = m_resolution_xy*round(new_x_min / m_resolution_xy);
69  if (fabs(new_y_min / m_resolution_xy - round(new_y_min / m_resolution_xy))>0.05)
70  new_y_min = m_resolution_xy*round(new_y_min / m_resolution_xy);
71  if (fabs(new_z_min / m_resolution_z - round(new_z_min / m_resolution_z))>0.05)
72  new_z_min = m_resolution_z*round(new_z_min / m_resolution_z);
73  if (fabs(new_x_max / m_resolution_xy - round(new_x_max / m_resolution_xy))>0.05)
74  new_x_max = m_resolution_xy*round(new_x_max / m_resolution_xy);
75  if (fabs(new_y_max / m_resolution_xy - round(new_y_max / m_resolution_xy))>0.05)
76  new_y_max = m_resolution_xy*round(new_y_max / m_resolution_xy);
77  if (fabs(new_z_max / m_resolution_z - round(new_z_max / m_resolution_z))>0.05)
78  new_z_max = m_resolution_z*round(new_z_max / m_resolution_z);
79 
80  // Change the map size: Extensions at each side:
81  size_t extra_x_izq = round((m_x_min - new_x_min) / m_resolution_xy);
82  size_t extra_y_arr = round((m_y_min - new_y_min) / m_resolution_xy);
83  size_t extra_z_top = round((m_z_min - new_z_min) / m_resolution_z);
84 
85  size_t new_size_x = round((new_x_max - new_x_min) / m_resolution_xy);
86  size_t new_size_y = round((new_y_max - new_y_min) / m_resolution_xy);
87  size_t new_size_z = round((new_z_max - new_z_min) / m_resolution_z);
88  size_t new_size_x_times_y = new_size_x*new_size_y;
89 
90  // Reserve new memory:
91  typename std::vector<T> new_map;
92  new_map.resize(new_size_x*new_size_y*new_size_z, defaultValueNewCells);
93 
94  // Copy previous rows:
95  size_t x, y, z;
96  typename std::vector<T>::iterator itSrc, itDst;
97  for (z = 0; z < m_size_z; z++)
98  {
99  for (y = 0; y < m_size_y; y++)
100  {
101  for (x = 0, itSrc = (m_map.begin() + y*m_size_x+z*m_size_x_times_y), itDst = (new_map.begin() + extra_x_izq + (y + extra_y_arr)*new_size_x + (z+extra_z_top)*new_size_x_times_y );
102  x < m_size_x;
103  ++x, ++itSrc, ++itDst)
104  {
105  *itDst = *itSrc;
106  }
107  }
108  }
109 
110  // Update the new map limits:
111  m_x_min = new_x_min;
112  m_x_max = new_x_max;
113  m_y_min = new_y_min;
114  m_y_max = new_y_max;
115  m_z_min = new_z_min;
116  m_z_max = new_z_max;
117 
118  m_size_x = new_size_x;
119  m_size_y = new_size_y;
120  m_size_z = new_size_z;
121  m_size_x_times_y = new_size_x_times_y;
122 
123  // Keep the new map only:
124  m_map.swap(new_map);
125  }
126 
127  /** Changes the size of the grid, ERASING all previous contents.
128  * If \a fill_value is left as NULL, the contents of cells may be undefined (some will remain with
129  * their old values, the new ones will have the default voxel value, but the location of old values
130  * may change wrt their old places).
131  * If \a fill_value is not NULL, it is assured that all cells will have a copy of that value after resizing.
132  * If `resolution_z`<0, the same resolution will be used for all dimensions x,y,z as given in `resolution_xy`
133  * \sa resize, fill
134  */
135  virtual void setSize(
136  const double x_min, const double x_max,
137  const double y_min, const double y_max,
138  const double z_min, const double z_max,
139  const double resolution_xy, const double resolution_z_=-1.0,
140  const T * fill_value = NULL)
141  {
142  const double resolution_z = resolution_z_ > 0.0 ? resolution_z_ : resolution_xy;
143 
144  // Adjust sizes to adapt them to full sized cells acording to the resolution:
145  m_x_min = x_min;
146  m_y_min = y_min;
147  m_z_min = z_min;
148 
149  m_x_max = x_min + resolution_xy*round((x_max-x_min) / resolution_xy);
150  m_y_max = y_min + resolution_xy*round((y_max-y_min)/ resolution_xy);
151  m_z_max = z_min + resolution_z*round((z_max-z_min) / resolution_z);
152 
153  // Res:
154  m_resolution_xy = resolution_xy;
155  m_resolution_z = resolution_z;
156 
157  // Now the number of cells should be integers:
158  m_size_x = round((m_x_max-m_x_min)/m_resolution_xy);
159  m_size_y = round((m_y_max - m_y_min) / m_resolution_xy);
160  m_size_x_times_y = m_size_x * m_size_y;
161  m_size_z = round((m_z_max-m_z_min)/m_resolution_z);
162 
163  // Cells memory:
164  if (fill_value)
165  m_map.assign(m_size_x*m_size_y*m_size_z, *fill_value);
166  else m_map.resize(m_size_x*m_size_y*m_size_z);
167  }
168 
169  /** Erase the contents of all the cells, setting them to their default values (default ctor). */
170  virtual void clear() {
171  m_map.clear();
172  m_map.resize(m_size_x*m_size_y*m_size_z);
173  }
174 
175  /** Fills all the cells with the same value
176  */
177  inline void fill( const T& value ) {
178  for (typename std::vector<T>::iterator it=m_map.begin();it!=m_map.end();++it)
179  *it=value;
180  }
181 
182  static const size_t INVALID_VOXEL_IDX = size_t(-1);
183 
184  inline bool isOutOfBounds(const int cx, const int cy, const int cz) const {
185  return (cx<0 || cx >= static_cast<int>(m_size_x)) ||
186  (cy<0 || cy >= static_cast<int>(m_size_y)) ||
187  (cz<0 || cz >= static_cast<int>(m_size_z));
188  }
189 
190  /** 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). \sa x2idx(), y2idx(), z2idx() */
191  inline size_t cellAbsIndexFromCXCYCZ(const int cx, const int cy, const int cz) const {
192  if (isOutOfBounds(cx,cy,cz)) return INVALID_VOXEL_IDX;
193  return cx + cy*m_size_x + cz * m_size_x_times_y;
194  }
195 
196  /** Returns a pointer to the contents of a voxel given by its coordinates, or NULL if it is out of the map extensions.
197  */
198  inline T* cellByPos( double x, double y, double z )
199  {
200  const size_t cidx = cellAbsIndexFromCXCYCZ(x2idx(x), y2idx(y), z2idx(z));
201  if (cidx == INVALID_VOXEL_IDX) return NULL;
202  return &m_map[cidx];
203  }
204  /** \overload */
205  inline const T* cellByPos( double x, double y, double z ) const
206  {
207  const size_t cidx = cellAbsIndexFromCXCYCZ(x2idx(x), y2idx(y), z2idx(z));
208  if (cidx == INVALID_VOXEL_IDX) return NULL;
209  return &m_map[cidx];
210  }
211 
212  /** Returns a pointer to the contents of a voxel given by its voxel indexes, or NULL if it is out of the map extensions.
213  */
214  inline T* cellByIndex( unsigned int cx, unsigned int cy, unsigned int cz )
215  {
216  const size_t cidx = cellAbsIndexFromCXCYCZ(cx, cy, cz);
217  if (cidx == INVALID_VOXEL_IDX) return NULL;
218  return &m_map[cidx];
219  }
220 
221  /** Returns a pointer to the contents of a voxel given by its voxel indexes, or NULL if it is out of the map extensions.
222  */
223  inline const T* cellByIndex( unsigned int cx, unsigned int cy, unsigned int cz ) const
224  {
225  const size_t cidx = cellAbsIndexFromCXCYCZ(cx, cy, cz);
226  if (cidx == INVALID_VOXEL_IDX) return NULL;
227  return &m_map[cidx];
228  }
229 
230  inline size_t getSizeX() const { return m_size_x; }
231  inline size_t getSizeY() const { return m_size_y; }
232  inline size_t getSizeZ() const { return m_size_z; }
233  inline size_t getVoxelCount() const { return m_size_x_times_y * m_size_z; }
234 
235  inline double getXMin()const { return m_x_min; }
236  inline double getXMax()const { return m_x_max; }
237  inline double getYMin()const { return m_y_min; }
238  inline double getYMax()const { return m_y_max; }
239  inline double getZMin()const { return m_z_min; }
240  inline double getZMax()const { return m_z_max; }
241 
242  inline double getResolutionXY()const { return m_resolution_xy; }
243  inline double getResolutionZ()const { return m_resolution_z; }
244 
245  /** Transform a coordinate values into voxel indexes */
246  inline int x2idx(double x) const { return static_cast<int>( (x-m_x_min)/m_resolution_xy ); }
247  inline int y2idx(double y) const { return static_cast<int>((y - m_y_min) / m_resolution_xy); }
248  inline int z2idx(double z) const { return static_cast<int>( (z-m_z_min)/m_resolution_z ); }
249 
250  /** Transform a voxel index into a coordinate value of the voxel central point */
251  inline double idx2x(int cx) const { return m_x_min+(cx)*m_resolution_xy; }
252  inline double idx2y(int cy) const { return m_y_min+(cy)*m_resolution_xy; }
253  inline double idx2z(int cz) const { return m_z_min + (cz)*m_resolution_z; }
254 
255  protected:
256  mutable std::vector<T> m_map; //!< The cells
257  /** Used only from logically const method that really need to modify the object */
258  inline std::vector<T> & m_map_castaway_const() const { return m_map; }
259 
260  double m_x_min, m_x_max, m_y_min, m_y_max, m_z_min, m_z_max, m_resolution_xy, m_resolution_z;
261  size_t m_size_x, m_size_y, m_size_z, m_size_x_times_y;
262  /** Serialization of all parameters, except the contents of each voxel (responsability of the derived class) */
263  void dyngridcommon_writeToStream(mrpt::utils::CStream &out) const {
264  out << m_x_min << m_x_max << m_y_min << m_y_max << m_z_min << m_z_max;
265  out << m_resolution_xy << m_resolution_z;
266  out << static_cast<uint32_t>(m_size_x) << static_cast<uint32_t>(m_size_y) << static_cast<uint32_t>(m_size_z);
267  }
268  /** Serialization of all parameters, except the contents of each voxel (responsability of the derived class) */
269  void dyngridcommon_readFromStream(mrpt::utils::CStream &in) {
270  in >> m_x_min >> m_x_max >> m_y_min >> m_y_max >> m_z_min >> m_z_max;
271  in >> m_resolution_xy >> m_resolution_z;
272 
273  uint32_t nX,nY,nZ;
274  in >> nX >> nY >> nZ;
275  m_size_x = nX; m_size_y = nY; m_size_z = nZ;
276  m_map.resize(nX*nY*nZ);
277  }
278 
279  }; // end of CDynamicGrid3D<>
280 
281  } // End of namespace
282 } // end of namespace
283 
mrpt::utils::CDynamicGrid3D::z2idx
int z2idx(double z) const
Definition: CDynamicGrid3D.h:248
mrpt::utils::CDynamicGrid3D::cellByPos
T * cellByPos(double x, double y, double z)
Returns a pointer to the contents of a voxel given by its coordinates, or NULL if it is out of the ma...
Definition: CDynamicGrid3D.h:198
z
GLdouble GLdouble z
Definition: glext.h:3734
mrpt::utils::CDynamicGrid3D::m_map
std::vector< T > m_map
The cells.
Definition: CDynamicGrid3D.h:256
mrpt::utils::CDynamicGrid3D::cellAbsIndexFromCXCYCZ
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).
Definition: CDynamicGrid3D.h:191
mrpt::utils::CDynamicGrid3D::cellByPos
const T * cellByPos(double x, double y, double z) const
Definition: CDynamicGrid3D.h:205
mrpt::utils::CDynamicGrid3D::CDynamicGrid3D
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.
Definition: CDynamicGrid3D.h:24
mrpt::utils::CDynamicGrid3D::idx2z
double idx2z(int cz) const
Definition: CDynamicGrid3D.h:253
mrpt::utils::CDynamicGrid3D::INVALID_VOXEL_IDX
static const size_t INVALID_VOXEL_IDX
Definition: CDynamicGrid3D.h:182
iterator
Scalar * iterator
Definition: eigen_plugins.h:23
mrpt::utils::CDynamicGrid3D::isOutOfBounds
bool isOutOfBounds(const int cx, const int cy, const int cz) const
Definition: CDynamicGrid3D.h:184
mrpt::utils::CDynamicGrid3D::dyngridcommon_writeToStream
void dyngridcommon_writeToStream(mrpt::utils::CStream &out) const
Serialization of all parameters, except the contents of each voxel (responsability of the derived cla...
Definition: CDynamicGrid3D.h:263
mrpt::utils::CDynamicGrid3D::clear
virtual void clear()
Erase the contents of all the cells, setting them to their default values (default ctor)...
Definition: CDynamicGrid3D.h:170
mrpt::utils::CDynamicGrid3D
A 3D rectangular grid of dynamic size which stores any kind of data at each voxel.
Definition: CDynamicGrid3D.h:20
CStream.h
mrpt::utils::CDynamicGrid3D::cellByIndex
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 NULL if it is out of the ...
Definition: CDynamicGrid3D.h:223
mrpt::utils::CStream
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
mrpt::utils::CDynamicGrid3D::setSize
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=NULL)
Changes the size of the grid, ERASING all previous contents.
Definition: CDynamicGrid3D.h:135
mrpt::utils::CDynamicGrid3D::cellByIndex
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 NULL if it is out of the ...
Definition: CDynamicGrid3D.h:214
mrpt::utils::CDynamicGrid3D::resize
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.
Definition: CDynamicGrid3D.h:33
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:16
mrpt::utils::CDynamicGrid3D::x2idx
int x2idx(double x) const
Transform a coordinate values into voxel indexes.
Definition: CDynamicGrid3D.h:246
mrpt::utils::CDynamicGrid3D::fill
void fill(const T &value)
Fills all the cells with the same value.
Definition: CDynamicGrid3D.h:177
mrpt::utils::CDynamicGrid3D::idx2x
double idx2x(int cx) const
Transform a voxel index into a coordinate value of the voxel central point.
Definition: CDynamicGrid3D.h:251
mrpt::utils::CDynamicGrid3D::idx2y
double idx2y(int cy) const
Definition: CDynamicGrid3D.h:252
mrpt::utils::CDynamicGrid3D::dyngridcommon_readFromStream
void dyngridcommon_readFromStream(mrpt::utils::CStream &in)
Serialization of all parameters, except the contents of each voxel (responsability of the derived cla...
Definition: CDynamicGrid3D.h:269
in
GLuint in
Definition: glext.h:6301
mrpt::utils::round
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:26
y
GLenum GLint GLint y
Definition: glext.h:3516
value
GLsizei const GLfloat * value
Definition: glext.h:3929
mrpt::utils::CDynamicGrid3D::m_map_castaway_const
std::vector< T > & m_map_castaway_const() const
Used only from logically const method that really need to modify the object.
Definition: CDynamicGrid3D.h:258
x
GLenum GLint x
Definition: glext.h:3516
uint32_t
unsigned __int32 uint32_t
Definition: rptypes.h:49
mrpt::utils::CDynamicGrid3D::y2idx
int y2idx(double y) const
Definition: CDynamicGrid3D.h:247
core_defs.h


Page generated by Doxygen 1.8.14 for MRPT 1.5.7 Git: 5902e14cc Wed Apr 24 15:04:01 2019 +0200 at lun oct 28 01:39:17 CET 2019