Main MRPT website > C++ reference for MRPT 1.9.9
COctoMapVoxels.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 
13 
14 namespace mrpt
15 {
16 namespace opengl
17 {
19 {
22 };
23 
24 /** A flexible renderer of voxels, typically from a 3D octo map (see
25  *mrpt::maps::COctoMap).
26  * This class is sort of equivalent to octovis::OcTreeDrawer from the octomap
27  *package, but
28  * relying on MRPT's CRenderizableDisplayList so there's no need to manually
29  *cache the rendering of OpenGL primitives.
30  *
31  * Normally users call mrpt::maps::COctoMap::getAs3DObject() to obtain a
32  *generic mrpt::opengl::CSetOfObjects which insides holds an instance of
33  *COctoMapVoxels.
34  * You can also alternativelly call COctoMapVoxels::setFromOctoMap(), so you
35  *can tune the display parameters, colors, etc.
36  * As with any other mrpt::opengl class, all object coordinates refer to some
37  *frame of reference which is relative to the object parent and can be changed
38  *with mrpt::opengl::CRenderizable::setPose()
39  *
40  * This class draws these separate elements to represent an OctoMap:
41  * - A grid representation of all cubes, as simple lines (occupied/free,
42  *leafs/nodes,... whatever). See:
43  * - showGridLines()
44  * - setGridLinesColor()
45  * - setGridLinesWidth()
46  * - push_back_GridCube()
47  * - A number of <b>voxel collections</b>, drawn as cubes each having a
48  *different color (e.g. depending on the color scheme in the original
49  *mrpt::maps::COctoMap object).
50  * The meanning of each collection is user-defined, but you can use the
51  *constants VOXEL_SET_OCCUPIED, VOXEL_SET_FREESPACE for predefined meanings.
52  * - showVoxels()
53  * - push_back_Voxel()
54  *
55  * Several coloring schemes can be selected with setVisualizationMode(). See
56  *COctoMapVoxels::visualization_mode_t
57  *
58  * <div align="center">
59  * <table border="0" cellspan="4" cellspacing="4" style="border-width: 1px;
60  *border-style: solid;">
61  * <tr> <td> mrpt::opengl::COctoMapVoxels </td> <td> \image html
62  *preview_COctoMapVoxels.png </td> </tr>
63  * </table>
64  * </div>
65  *
66  * \sa opengl::COpenGLScene
67  * \ingroup mrpt_opengl_grp
68  */
70 {
72  public:
73  /** The different coloring schemes, which modulate the generic
74  * mrpt::opengl::CRenderizable object color. Set with setVisualizationMode()
75  */
77  {
78  /** Color goes from black (at the bottom) to the chosen color (at the
79  top) */
81  /** Color goes from black (occupied voxel) to the chosen color (free
82  voxel) */
84  /** Transparency goes from opaque (occupied voxel) to transparent (free
85  voxel). */
87  /** Color goes from black (occupaid voxel) to the chosen color (free
88  voxel) and they are transparent */
90  /** Combination of COLOR_FROM_HEIGHT and TRANSPARENCY_FROM_OCCUPANCY */
92  /** All cubes are of identical color. */
94  };
95 
96  /** The info of each of the voxels */
97  struct TVoxel
98  {
100  double side_length;
102 
103  TVoxel() {}
105  const mrpt::math::TPoint3D& coords_, const double side_length_,
106  mrpt::img::TColor color_)
107  : coords(coords_), side_length(side_length_), color(color_)
108  {
109  }
110  };
111 
112  /** The info of each grid block */
113  struct TGridCube
114  {
115  /** opposite corners of the cube */
117 
120  const mrpt::math::TPoint3D& min_, const mrpt::math::TPoint3D& max_)
121  : min(min_), max(max_)
122  {
123  }
124  };
125 
127  {
128  bool visible;
129  std::vector<TVoxel> voxels;
130 
132  };
133 
134  protected:
135  std::deque<TInfoPerVoxelSet> m_voxel_sets;
136  std::vector<TGridCube> m_grid_cubes;
137 
138  /** Cached bounding boxes */
140 
149 
150  public:
151  /** Clears everything */
152  void clear();
153 
154  /** Select the visualization mode. To have any effect, this method has to be
155  * called before loading the octomap. */
157  {
160  }
162  {
163  return m_visual_mode;
164  }
165 
166  /** Can be used to enable/disable the effects of lighting in this object */
167  inline void enableLights(bool enable)
168  {
169  m_enable_lighting = enable;
171  }
172  inline bool areLightsEnabled() const { return m_enable_lighting; }
173  /** By default, the alpha (transparency) component of voxel cubes is taken
174  * into account, but transparency can be disabled with this method. */
175  inline void enableCubeTransparency(bool enable)
176  {
179  }
180  inline bool isCubeTransparencyEnabled() const
181  {
183  }
184 
185  /** Shows/hides the grid lines */
186  inline void showGridLines(bool show)
187  {
188  m_show_grids = show;
190  }
191  inline bool areGridLinesVisible() const { return m_show_grids; }
192  /** Shows/hides the voxels (voxel_set is a 0-based index for the set of
193  * voxels to modify, e.g. VOXEL_SET_OCCUPIED, VOXEL_SET_FREESPACE) */
194  inline void showVoxels(unsigned int voxel_set, bool show)
195  {
196  ASSERT_(voxel_set < m_voxel_sets.size());
197  m_voxel_sets[voxel_set].visible = show;
199  }
200  inline bool areVoxelsVisible(unsigned int voxel_set) const
201  {
202  ASSERT_(voxel_set < m_voxel_sets.size());
203  return m_voxel_sets[voxel_set].visible;
204  }
205 
206  /** For quick renders: render voxels as points instead of cubes. \sa
207  * setVoxelAsPointsSize */
208  inline void showVoxelsAsPoints(const bool enable)
209  {
210  m_showVoxelsAsPoints = enable;
212  }
213  inline bool areVoxelsShownAsPoints() const { return m_showVoxelsAsPoints; }
214  /** Only used when showVoxelsAsPoints() is enabled. */
215  inline void setVoxelAsPointsSize(float pointSize)
216  {
217  m_showVoxelsAsPointsSize = pointSize;
219  }
220  inline float getVoxelAsPointsSize() const
221  {
223  }
224 
225  /** Sets the width of grid lines */
226  inline void setGridLinesWidth(float w)
227  {
228  m_grid_width = w;
230  }
231  /** Gets the width of grid lines */
232  inline float getGridLinesWidth() const { return m_grid_width; }
234  {
237  }
238  inline const mrpt::img::TColor& getGridLinesColor() const
239  {
240  return m_grid_color;
241  }
242 
243  /** Returns the total count of grid cubes. */
244  inline size_t getGridCubeCount() const { return m_grid_cubes.size(); }
245  /** Returns the number of voxel sets. */
246  inline size_t getVoxelSetCount() const { return m_voxel_sets.size(); }
247  /** Returns the total count of voxels in one voxel set. */
248  inline size_t getVoxelCount(const size_t set_index) const
249  {
250  ASSERT_(set_index < m_voxel_sets.size());
251  return m_voxel_sets[set_index].voxels.size();
252  }
253 
254  /** Manually changes the bounding box (normally the user doesn't need to
255  * call this) */
256  void setBoundingBox(
257  const mrpt::math::TPoint3D& bb_min, const mrpt::math::TPoint3D& bb_max);
258 
259  inline void resizeGridCubes(const size_t nCubes)
260  {
261  m_grid_cubes.resize(nCubes);
263  }
264  inline void resizeVoxelSets(const size_t nVoxelSets)
265  {
266  m_voxel_sets.resize(nVoxelSets);
268  }
269  inline void resizeVoxels(const size_t set_index, const size_t nVoxels)
270  {
271  ASSERT_(set_index < m_voxel_sets.size());
272  m_voxel_sets[set_index].voxels.resize(nVoxels);
274  }
275 
276  inline void reserveGridCubes(const size_t nCubes)
277  {
278  m_grid_cubes.reserve(nCubes);
279  }
280  inline void reserveVoxels(const size_t set_index, const size_t nVoxels)
281  {
282  ASSERT_(set_index < m_voxel_sets.size());
283  m_voxel_sets[set_index].voxels.reserve(nVoxels);
285  }
286 
287  inline TGridCube& getGridCubeRef(const size_t idx)
288  {
289  ASSERTDEB_(idx < m_grid_cubes.size());
291  return m_grid_cubes[idx];
292  }
293  inline const TGridCube& getGridCube(const size_t idx) const
294  {
295  ASSERTDEB_(idx < m_grid_cubes.size());
296  return m_grid_cubes[idx];
297  }
298 
299  inline TVoxel& getVoxelRef(const size_t set_index, const size_t idx)
300  {
301  ASSERTDEB_(
302  set_index < m_voxel_sets.size() &&
303  idx < m_voxel_sets[set_index].voxels.size());
305  return m_voxel_sets[set_index].voxels[idx];
306  }
307  inline const TVoxel& getVoxel(
308  const size_t set_index, const size_t idx) const
309  {
310  ASSERTDEB_(
311  set_index < m_voxel_sets.size() &&
312  idx < m_voxel_sets[set_index].voxels.size());
314  return m_voxel_sets[set_index].voxels[idx];
315  }
316 
317  inline void push_back_GridCube(const TGridCube& c)
318  {
320  m_grid_cubes.push_back(c);
321  }
322  inline void push_back_Voxel(const size_t set_index, const TVoxel& v)
323  {
324  ASSERTDEB_(set_index < m_voxel_sets.size());
326  m_voxel_sets[set_index].voxels.push_back(v);
327  }
328 
329  void sort_voxels_by_z();
330 
331  /** Render */
332  void render_dl() const override;
333 
334  /** Evaluates the bounding box of this object (including possible children)
335  * in the coordinate frame of the object parent. */
336  void getBoundingBox(
337  mrpt::math::TPoint3D& bb_min,
338  mrpt::math::TPoint3D& bb_max) const override;
339 
340  /** Sets the contents of the object from a mrpt::maps::COctoMap object.
341  * \tparam Typically, an mrpt::maps::COctoMap object
342  *
343  * \note Declared as a template because in the library [mrpt-opengl] we
344  * don't have access to the library [mrpt-maps].
345  */
346  template <class OCTOMAP>
347  void setFromOctoMap(OCTOMAP& m)
348  {
349  m.getAsOctoMapVoxels(*this);
350  }
351 
352  /** Constructor */
353  COctoMapVoxels();
354  /** Private, virtual destructor: only can be deleted from smart pointers. */
355  virtual ~COctoMapVoxels() {}
356 };
357 
358 } // namespace opengl
359 } // namespace mrpt
void showVoxels(unsigned int voxel_set, bool show)
Shows/hides the voxels (voxel_set is a 0-based index for the set of voxels to modify, e.g.
mrpt::img::TColor m_grid_color
void reserveVoxels(const size_t set_index, const size_t nVoxels)
TGridCube & getGridCubeRef(const size_t idx)
const GLshort * coords
Definition: glext.h:7386
All cubes are of identical color.
visualization_mode_t
The different coloring schemes, which modulate the generic mrpt::opengl::CRenderizable object color...
bool areVoxelsVisible(unsigned int voxel_set) const
const TGridCube & getGridCube(const size_t idx) const
void enableLights(bool enable)
Can be used to enable/disable the effects of lighting in this object.
mrpt::math::TPoint3D m_bb_min
Cached bounding boxes.
std::deque< TInfoPerVoxelSet > m_voxel_sets
void setFromOctoMap(OCTOMAP &m)
Sets the contents of the object from a mrpt::maps::COctoMap object.
void setVoxelAsPointsSize(float pointSize)
Only used when showVoxelsAsPoints() is enabled.
mrpt::math::TPoint3D m_bb_max
size_t getVoxelCount(const size_t set_index) const
Returns the total count of voxels in one voxel set.
TVoxel & getVoxelRef(const size_t set_index, const size_t idx)
Combination of COLOR_FROM_HEIGHT and TRANSPARENCY_FROM_OCCUPANCY.
TVoxel(const mrpt::math::TPoint3D &coords_, const double side_length_, mrpt::img::TColor color_)
void enableCubeTransparency(bool enable)
By default, the alpha (transparency) component of voxel cubes is taken into account, but transparency can be disabled with this method.
float getGridLinesWidth() const
Gets the width of grid lines.
void setBoundingBox(const mrpt::math::TPoint3D &bb_min, const mrpt::math::TPoint3D &bb_max)
Manually changes the bounding box (normally the user doesn&#39;t need to call this)
Color goes from black (at the bottom) to the chosen color (at the top)
size_t getVoxelSetCount() const
Returns the number of voxel sets.
void reserveGridCubes(const size_t nCubes)
EIGEN_STRONG_INLINE void notifyChange() const
Must be called to notify that the object has changed (so, the display list must be updated) ...
Transparency goes from opaque (occupied voxel) to transparent (free voxel).
void clear()
Clears everything.
void resizeVoxels(const size_t set_index, const size_t nVoxels)
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:4178
A renderizable object suitable for rendering with OpenGL&#39;s display lists.
A flexible renderer of voxels, typically from a 3D octo map (see mrpt::maps::COctoMap).
GLuint color
Definition: glext.h:8300
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:113
void push_back_GridCube(const TGridCube &c)
const GLubyte * c
Definition: glext.h:6313
Color goes from black (occupaid voxel) to the chosen color (free voxel) and they are transparent...
size_t getGridCubeCount() const
Returns the total count of grid cubes.
void render_dl() const override
Render.
void setGridLinesColor(const mrpt::img::TColor &color)
const TVoxel & getVoxel(const size_t set_index, const size_t idx) const
const mrpt::img::TColor & getGridLinesColor() const
The info of each grid block.
void push_back_Voxel(const size_t set_index, const TVoxel &v)
GLint mode
Definition: glext.h:5669
The info of each of the voxels.
const GLdouble * v
Definition: glext.h:3678
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
Color goes from black (occupied voxel) to the chosen color (free voxel)
void getBoundingBox(mrpt::math::TPoint3D &bb_min, mrpt::math::TPoint3D &bb_max) const override
Evaluates the bounding box of this object (including possible children) in the coordinate frame of th...
visualization_mode_t m_visual_mode
void showVoxelsAsPoints(const bool enable)
For quick renders: render voxels as points instead of cubes.
#define ASSERTDEB_(f)
Defines an assertion mechanism - only when compiled in debug.
Definition: exceptions.h:205
void resizeVoxelSets(const size_t nVoxelSets)
visualization_mode_t getVisualizationMode() const
void showGridLines(bool show)
Shows/hides the grid lines.
void setVisualizationMode(visualization_mode_t mode)
Select the visualization mode.
std::vector< TGridCube > m_grid_cubes
A RGB color - 8bit.
Definition: TColor.h:22
Lightweight 3D point.
void resizeGridCubes(const size_t nCubes)
bool isCubeTransparencyEnabled() const
virtual ~COctoMapVoxels()
Private, virtual destructor: only can be deleted from smart pointers.
mrpt::math::TPoint3D min
opposite corners of the cube
void setGridLinesWidth(float w)
Sets the width of grid lines.
TGridCube(const mrpt::math::TPoint3D &min_, const mrpt::math::TPoint3D &max_)



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