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>
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:
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();
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 
262  /** Serialization of all parameters, except the contents of each voxel (responsability of the derived class) */
264  out << m_x_min << m_x_max << m_y_min << m_y_max << m_z_min << m_z_max;
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) */
270  in >> m_x_min >> m_x_max >> m_y_min >> m_y_max >> m_z_min >> m_z_max;
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 
int z2idx(double z) const
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...
GLdouble GLdouble z
Definition: glext.h:3734
std::vector< 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 T * cellByPos(double x, double y, double z) const
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.
double idx2z(int cz) const
static const size_t INVALID_VOXEL_IDX
Scalar * iterator
Definition: eigen_plugins.h:23
bool isOutOfBounds(const int cx, const int cy, const int cz) const
void dyngridcommon_writeToStream(mrpt::utils::CStream &out) const
Serialization of all parameters, except the contents of each voxel (responsability of the derived cla...
virtual void clear()
Erase the contents of all the cells, setting them to their default values (default ctor)...
A 3D rectangular grid of dynamic size which stores any kind of data at each voxel.
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 ...
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
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.
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 ...
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.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
int x2idx(double x) const
Transform a coordinate values into voxel indexes.
void fill(const T &value)
Fills all the cells with the same value.
double idx2x(int cx) const
Transform a voxel index into a coordinate value of the voxel central point.
double idx2y(int cy) const
void dyngridcommon_readFromStream(mrpt::utils::CStream &in)
Serialization of all parameters, except the contents of each voxel (responsability of the derived cla...
GLuint in
Definition: glext.h:6301
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:26
GLenum GLint GLint y
Definition: glext.h:3516
GLsizei const GLfloat * value
Definition: glext.h:3929
std::vector< T > & m_map_castaway_const() const
Used only from logically const method that really need to modify the object.
GLenum GLint x
Definition: glext.h:3516
unsigned __int32 uint32_t
Definition: rptypes.h:49
int y2idx(double y) const



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