Main MRPT website > C++ reference for MRPT 1.5.5
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  /** 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() */
185  inline size_t cellAbsIndexFromCXCYCZ(const int cx, const int cy, const int cz) const {
186  if (cx<0 || cx >= static_cast<int>(m_size_x)) return INVALID_VOXEL_IDX;
187  if (cy<0 || cy >= static_cast<int>(m_size_y)) return INVALID_VOXEL_IDX;
188  if (cz<0 || cz >= static_cast<int>(m_size_z)) return INVALID_VOXEL_IDX;
189  return cx + cy*m_size_x + cz * m_size_x_times_y;
190  }
191 
192 
193  /** Returns a pointer to the contents of a voxel given by its coordinates, or NULL if it is out of the map extensions.
194  */
195  inline T* cellByPos( double x, double y, double z )
196  {
197  const size_t cidx = cellAbsIndexFromCXCYCZ(x2idx(x), y2idx(y), z2idx(z));
198  if (cidx == INVALID_VOXEL_IDX) return NULL;
199  return &m_map[cidx];
200  }
201  /** \overload */
202  inline const T* cellByPos( double x, double y, double z ) const
203  {
204  const size_t cidx = cellAbsIndexFromCXCYCZ(x2idx(x), y2idx(y), z2idx(z));
205  if (cidx == INVALID_VOXEL_IDX) return NULL;
206  return &m_map[cidx];
207  }
208 
209  /** Returns a pointer to the contents of a voxel given by its voxel indexes, or NULL if it is out of the map extensions.
210  */
211  inline T* cellByIndex( unsigned int cx, unsigned int cy, unsigned int cz )
212  {
213  const size_t cidx = cellAbsIndexFromCXCYCZ(cx, cy, cz);
214  if (cidx == INVALID_VOXEL_IDX) return NULL;
215  return &m_map[cidx];
216  }
217 
218  /** Returns a pointer to the contents of a voxel given by its voxel indexes, or NULL if it is out of the map extensions.
219  */
220  inline const T* cellByIndex( unsigned int cx, unsigned int cy, unsigned int cz ) const
221  {
222  const size_t cidx = cellAbsIndexFromCXCYCZ(cx, cy, cz);
223  if (cidx == INVALID_VOXEL_IDX) return NULL;
224  return &m_map[cidx];
225  }
226 
227  inline size_t getSizeX() const { return m_size_x; }
228  inline size_t getSizeY() const { return m_size_y; }
229  inline size_t getSizeZ() const { return m_size_z; }
230  inline size_t getVoxelCount() const { return m_size_x_times_y * m_size_z; }
231 
232  inline double getXMin()const { return m_x_min; }
233  inline double getXMax()const { return m_x_max; }
234  inline double getYMin()const { return m_y_min; }
235  inline double getYMax()const { return m_y_max; }
236  inline double getZMin()const { return m_z_min; }
237  inline double getZMax()const { return m_z_max; }
238 
239  inline double getResolutionXY()const { return m_resolution_xy; }
240  inline double getResolutionZ()const { return m_resolution_z; }
241 
242  /** Transform a coordinate values into voxel indexes */
243  inline int x2idx(double x) const { return static_cast<int>( (x-m_x_min)/m_resolution_xy ); }
244  inline int y2idx(double y) const { return static_cast<int>((y - m_y_min) / m_resolution_xy); }
245  inline int z2idx(double z) const { return static_cast<int>( (z-m_z_min)/m_resolution_z ); }
246 
247  /** Transform a voxel index into a coordinate value of the voxel central point */
248  inline double idx2x(int cx) const { return m_x_min+(cx)*m_resolution_xy; }
249  inline double idx2y(int cy) const { return m_y_min+(cy)*m_resolution_xy; }
250  inline double idx2z(int cz) const { return m_z_min + (cz)*m_resolution_z; }
251 
252  protected:
253  mutable std::vector<T> m_map; //!< The cells
254  /** Used only from logically const method that really need to modify the object */
255  inline std::vector<T> & m_map_castaway_const() const { return m_map; }
256 
259  /** Serialization of all parameters, except the contents of each voxel (responsability of the derived class) */
261  out << m_x_min << m_x_max << m_y_min << m_y_max << m_z_min << m_z_max;
263  out << static_cast<uint32_t>(m_size_x) << static_cast<uint32_t>(m_size_y) << static_cast<uint32_t>(m_size_z);
264  }
265  /** Serialization of all parameters, except the contents of each voxel (responsability of the derived class) */
267  in >> m_x_min >> m_x_max >> m_y_min >> m_y_max >> m_z_min >> m_z_max;
269 
270  uint32_t nX,nY,nZ;
271  in >> nX >> nY >> nZ;
272  m_size_x = nX; m_size_y = nY; m_size_z = nZ;
273  m_map.resize(nX*nY*nZ);
274  }
275 
276  }; // end of CDynamicGrid3D<>
277 
278  } // End of namespace
279 } // end of namespace
280 
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
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.5 Git: e06b63dbf Fri Dec 1 14:41:11 2017 +0100 at lun oct 28 01:31:35 CET 2019