MRPT  1.9.9
COctoMapVoxels.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, 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 
10 #include "opengl-precomp.h" // Precompiled header
11 
15 #include "opengl_internals.h"
16 
17 using namespace mrpt;
18 using namespace mrpt::opengl;
19 using namespace mrpt::math;
20 using namespace std;
21 
23 
24 /** Ctor */
25 COctoMapVoxels::COctoMapVoxels() : m_grid_color(0xE0, 0xE0, 0xE0, 0x90) {}
26 /** Clears everything */
28 {
29  m_voxel_sets.clear();
30  m_grid_cubes.clear();
31 
33 }
34 
37 {
38  m_bb_min = bb_min;
39  m_bb_max = bb_max;
40 }
41 
42 #if MRPT_HAS_OPENGL_GLUT
43 
44 // See: http://www.songho.ca/opengl/gl_vertexarray.html
45 
46 // cube ///////////////////////////////////////////////////////////////////////
47 // v6----- v5
48 // +Z /| /|
49 // A v1------v0|
50 // | | | | |
51 // | | |v7---|-|v4 / -X
52 // | |/ |/ /
53 // | v2------v3 L +X
54 // ----------------------> +Y
55 //
56 
57 const GLubyte grid_line_indices[] = {0, 1, 1, 2, 2, 3, 3, 0, 4, 5, 5, 6,
58  6, 7, 7, 4, 0, 5, 1, 6, 2, 7, 3, 4};
59 
60 const GLubyte cube_indices[36] = {0, 1, 2, 2, 3, 0, 0, 3, 4, 4, 5, 0,
61  0, 5, 6, 6, 1, 0, 1, 6, 7, 7, 2, 1,
62  7, 4, 3, 3, 2, 7, 4, 7, 6, 6, 5, 4};
63 
64 // normal array
65 const GLfloat normals_cube[3 * 6 * 4] = {
66  1, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, // v0,v1,v2,v3 (front)
67  0, 1, 0, 0, 1, 0, 0, 1, 0, 0, 1, 0, // v0,v3,v4,v5 (right)
68  0, 0, 1, 0, 0, 1, 0, 0, 1, 0, 0, 1, // v0,v5,v6,v1 (top)
69  0, -1, 0, 0, -1, 0, 0, -1, 0, 0, -1, 0, // v1,v6,v7,v2 (left)
70  0, 0, -1, 0, 0, -1, 0, 0, -1, 0, 0, -1, // v7,v4,v3,v2 (bottom)
71  -1, 0, 0, -1, 0, 0, -1, 0, 0, -1, 0, 0}; // v4,v7,v6,v5 (back)
72 
73 #endif
74 
75 /*---------------------------------------------------------------
76  render
77  ---------------------------------------------------------------*/
79 {
80 #if MRPT_HAS_OPENGL_GLUT
81 
82  glEnableClientState(GL_VERTEX_ARRAY);
83 
84  // Draw grids ====================================
85  if (m_show_grids)
86  {
87  glLineWidth(m_grid_width);
89 
90  glDisable(GL_LIGHTING); // Disable lights when drawing lines
91 
92  // Antialiasing:
93  glPushAttrib(GL_COLOR_BUFFER_BIT | GL_LINE_BIT);
94  glEnable(GL_LINE_SMOOTH);
95  if (m_grid_color.A != 255)
96  {
97  glEnable(GL_BLEND);
98  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
99  }
100 
101  glColor4ub(
102  m_grid_color.R, m_grid_color.G, m_grid_color.B, m_grid_color.A);
103 
104  const size_t nGrids = m_grid_cubes.size();
105  for (size_t i = 0; i < nGrids; i++)
106  {
107  const TGridCube& c = m_grid_cubes[i];
108 
109  const GLdouble vertices[8 * 3] = {
110  c.max.x, c.max.y, c.max.z, c.max.x, c.min.y, c.max.z,
111  c.max.x, c.min.y, c.min.z, c.max.x, c.max.y, c.min.z,
112  c.min.x, c.max.y, c.min.z, c.min.x, c.max.y, c.max.z,
113  c.min.x, c.min.y, c.max.z, c.min.x, c.min.y, c.min.z};
114  glVertexPointer(3, GL_DOUBLE, 0, vertices);
115  glDrawElements(
116  GL_LINES,
117  sizeof(grid_line_indices) / sizeof(grid_line_indices[0]),
118  GL_UNSIGNED_BYTE, grid_line_indices);
119  }
120 
121  glEnable(GL_LIGHTING); // Disable lights when drawing lines
122  // End of antialiasing:
123  glPopAttrib();
124  }
125 
126  // Draw cubes ====================================
127  if (!m_enable_lighting) glDisable(GL_LIGHTING);
128 
129  glEnableClientState(GL_NORMAL_ARRAY);
130 
131  glNormalPointer(GL_FLOAT, 0, normals_cube);
132 
133  if (m_enable_cube_transparency)
134  {
135  glEnable(GL_BLEND);
136  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
137  }
138 
139  if (m_showVoxelsAsPoints)
140  {
141  glPointSize(m_showVoxelsAsPointsSize);
142  glBegin(GL_POINTS);
143  }
144 
145  for (const auto& m_voxel_set : m_voxel_sets)
146  {
147  if (!m_voxel_set.visible) continue;
148 
149  const std::vector<TVoxel>& voxels = m_voxel_set.voxels;
150  const size_t N = voxels.size();
151  for (size_t j = 0; j < N; j++)
152  {
153  const mrpt::img::TColor& vx_j_col = voxels[j].color;
154  glColor4ub(vx_j_col.R, vx_j_col.G, vx_j_col.B, vx_j_col.A);
155 
156  const mrpt::math::TPoint3D& c = voxels[j].coords;
157  const double L = voxels[j].side_length * 0.5;
158 
159  if (!m_showVoxelsAsPoints)
160  {
161  // Render as cubes:
162  const GLdouble vertices[8 * 3] = {
163  c.x + L, c.y + L, c.z + L, c.x + L, c.y - L, c.z + L,
164  c.x + L, c.y - L, c.z - L, c.x + L, c.y + L, c.z - L,
165  c.x - L, c.y + L, c.z - L, c.x - L, c.y + L, c.z + L,
166  c.x - L, c.y - L, c.z + L, c.x - L, c.y - L, c.z - L};
167  glVertexPointer(3, GL_DOUBLE, 0, vertices);
168  glDrawElements(
169  GL_TRIANGLES,
170  sizeof(cube_indices) / sizeof(cube_indices[0]),
171  GL_UNSIGNED_BYTE, cube_indices);
172  }
173  else
174  {
175  // Render as simple points:
176  glVertex3f(c.x, c.y, c.z);
177  }
178  }
179  }
180 
181  if (m_showVoxelsAsPoints)
182  {
183  glEnd(); // of GL_POINTS
184  }
185 
186  if (m_enable_cube_transparency) glDisable(GL_BLEND);
187 
188  glDisableClientState(GL_NORMAL_ARRAY);
189 
190  if (!m_enable_lighting) glEnable(GL_LIGHTING);
191 
192  glDisableClientState(GL_VERTEX_ARRAY);
194 
195 #endif
196 }
197 
201 
202 namespace mrpt::opengl
203 {
206 {
207  out << a.visible << a.voxels;
208  return out;
209 }
211 {
212  in >> a.visible >> a.voxels;
213  return in;
214 }
215 
217 {
218  out << a.min << a.max;
219  return out;
220 }
222 {
223  in >> a.min >> a.max;
224  return in;
225 }
226 
228 {
229  out << a.coords << a.side_length << a.color;
230  return out;
231 }
233 {
234  in >> a.coords >> a.side_length >> a.color;
235  return in;
236 }
237 } // end of namespace mrpt::opengl
238 
239 uint8_t COctoMapVoxels::serializeGetVersion() const { return 2; }
241 {
242  writeToStreamRender(out);
243 
244  out << m_voxel_sets << m_grid_cubes << m_bb_min << m_bb_max
245  << m_enable_lighting << m_showVoxelsAsPoints << m_showVoxelsAsPointsSize
246  << m_show_grids << m_grid_width << m_grid_color
247  << m_enable_cube_transparency // added in v1
248  << uint32_t(m_visual_mode); // added in v2
249 }
250 
251 void COctoMapVoxels::serializeFrom(CArchive& in, uint8_t version)
252 {
253  switch (version)
254  {
255  case 0:
256  case 1:
257  case 2:
258  {
259  readFromStreamRender(in);
260 
261  in >> m_voxel_sets >> m_grid_cubes >> m_bb_min >> m_bb_max >>
262  m_enable_lighting >> m_showVoxelsAsPoints >>
263  m_showVoxelsAsPointsSize >> m_show_grids >> m_grid_width >>
264  m_grid_color;
265 
266  if (version >= 1)
267  in >> m_enable_cube_transparency;
268  else
269  m_enable_cube_transparency = false;
270 
271  if (version >= 2)
272  {
273  uint32_t i;
274  in >> i;
275  m_visual_mode =
276  static_cast<COctoMapVoxels::visualization_mode_t>(i);
277  }
278  else
279  m_visual_mode = COctoMapVoxels::COLOR_FROM_OCCUPANCY;
280  }
281  break;
282  default:
284  };
285 
287 }
288 
291 {
292  bb_min = m_bb_min;
293  bb_max = m_bb_max;
294 
295  // Convert to coordinates of my parent:
296  m_pose.composePoint(bb_min, bb_min);
297  m_pose.composePoint(bb_max, bb_max);
298 }
299 
302 {
303  return a.coords.z < b.coords.z;
304 }
305 
307 {
308  for (auto& m_voxel_set : m_voxel_sets)
309  {
310  std::sort(
311  m_voxel_set.voxels.begin(), m_voxel_set.voxels.end(),
312  &sort_voxels_z);
313  }
314 }
void notifyChange() const
Must be called to notify that the object has changed (so, the display list must be updated) ...
visualization_mode_t
The different coloring schemes, which modulate the generic mrpt::opengl::CRenderizable object color...
mrpt::serialization::CArchive & operator<<(mrpt::serialization::CArchive &out, const mrpt::opengl::CLight &o)
Definition: CLight.cpp:122
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
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)
STL namespace.
uint8_t B
Definition: TColor.h:46
uint8_t G
Definition: TColor.h:46
void clear()
Clears everything.
bool sort_voxels_z(const COctoMapVoxels::TVoxel &a, const COctoMapVoxels::TVoxel &b)
A renderizable object suitable for rendering with OpenGL&#39;s display lists.
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
A flexible renderer of voxels, typically from a 3D octo map (see mrpt::maps::COctoMap).
This base provides a set of functions for maths stuff.
void render_dl() const override
Render.
uint8_t R
Definition: TColor.h:46
The info of each grid block.
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:23
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
The info of each of the voxels.
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Color goes from black (occupied voxel) to the chosen color (free voxel)
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:54
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...
#define DECLARE_CUSTOM_TTYPENAME(_TYPE)
Identical to MRPT_DECLARE_TTYPENAME but intended for user code.
Definition: TTypeName.h:90
mrpt::vision::TStereoCalibResults out
void checkOpenGLError()
Checks glGetError and throws an exception if an error situation is found.
Definition: gl_utils.cpp:155
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:15
const auto bb_max
mrpt::serialization::CArchive & operator>>(mrpt::serialization::CArchive &in, mrpt::opengl::CLight &o)
Definition: CLight.cpp:116
const auto bb_min
A RGB color - 8bit.
Definition: TColor.h:20
uint8_t A
Definition: TColor.h:46
mrpt::math::TPoint3D min
opposite corners of the cube



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: d1962bc6a Wed Jan 15 17:38:30 2020 +0100 at miƩ ene 15 17:45:11 CET 2020