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-2020, 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 
13 #include <mrpt/opengl/opengl_api.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 
43 {
44  switch (rc.shader_id)
45  {
47  if (m_showVoxelsAsPoints) CRenderizableShaderPoints::render(rc);
48  break;
50  if (!m_showVoxelsAsPoints) CRenderizableShaderTriangles::render(rc);
51  break;
53  if (m_show_grids) CRenderizableShaderWireFrame::render(rc);
54  break;
55  };
56 }
58 {
62 }
63 
64 // See: http://www.songho.ca/opengl/gl_vertexarray.html
65 
66 // cube ///////////////////////////////////////////////////////////////////////
67 // v6----- v5
68 // +Z /| /|
69 // A v1------v0|
70 // | | | | |
71 // | | |v7---|-|v4 / -X
72 // | |/ |/ /
73 // | v2------v3 L +X
74 // ----------------------> +Y
75 //
76 
77 static const uint8_t grid_line_indices[] = {0, 1, 1, 2, 2, 3, 3, 0, 4, 5, 5, 6,
78  6, 7, 7, 4, 0, 5, 1, 6, 2, 7, 3, 4};
79 
80 static const uint8_t cube_indices[2 * 6 * 3] = {
81  // front:
82  0, 1, 2, 0, 2, 3,
83  // right:
84  3, 4, 0, 4, 5, 0,
85  // top
86  0, 5, 6, 6, 1, 0,
87  // left
88  1, 6, 7, 7, 2, 1,
89  // bottom
90  7, 3, 4, 2, 3, 7,
91  // back
92  4, 7, 6, 5, 6, 4};
93 
94 // normal array: one per triangle
95 static const mrpt::math::TPoint3Df normals_cube[6 * 2] = {
96  {1, 0, 0}, {1, 0, 0}, // v0,v1,v2,v3 (front)
97  {0, 1, 0}, {0, 1, 0}, // v0,v3,v4,v5 (right)
98  {0, 0, 1}, {0, 0, 1}, // v0,v5,v6,v1 (top)
99  {0, -1, 0}, {0, -1, 0}, // v1,v6,v7,v2 (left)
100  {0, 0, -1}, {0, 0, -1}, // v7,v4,v3,v2 (bottom)
101  {-1, 0, 0}, {-1, 0, 0}}; // v4,v7,v6,v5 (back)
102 
104 {
107  vbd.clear();
108 
110 
111  const size_t nGrids = m_grid_cubes.size();
112  for (size_t i = 0; i < nGrids; i++)
113  {
114  const TGridCube& c = m_grid_cubes[i];
115 
116  const mrpt::math::TPoint3Df vs[8] = {
117  {c.max.x, c.max.y, c.max.z}, {c.max.x, c.min.y, c.max.z},
118  {c.max.x, c.min.y, c.min.z}, {c.max.x, c.max.y, c.min.z},
119  {c.min.x, c.max.y, c.min.z}, {c.min.x, c.max.y, c.max.z},
120  {c.min.x, c.min.y, c.max.z}, {c.min.x, c.min.y, c.min.z}};
121 
122  const auto& gli = grid_line_indices;
123 
124  // glDrawElements(GL_LINES, , GL_UNSIGNED_BYTE, grid_line_indices);
125  for (size_t k = 0; k < sizeof(gli) / sizeof(gli[0]); k += 2)
126  {
127  vbd.emplace_back(vs[gli[k]]);
128  vbd.emplace_back(vs[gli[k + 1]]);
129  }
130  }
131 
132  cbd.assign(vbd.size(), m_grid_color);
133 }
134 
136 {
138  tris.clear();
139 
140  for (const auto& m_voxel_set : m_voxel_sets)
141  {
142  if (!m_voxel_set.visible) continue;
143 
144  const std::vector<TVoxel>& voxels = m_voxel_set.voxels;
145  const size_t N = voxels.size();
146  for (size_t j = 0; j < N; j++)
147  {
148  const mrpt::img::TColor& vx_j_col = voxels[j].color;
149  // Was: glColor4ub(vx_j_col.R, vx_j_col.G, vx_j_col.B, vx_j_col.A);
150 
151  const mrpt::math::TPoint3Df& c = voxels[j].coords;
152  const float L = voxels[j].side_length * 0.5f;
153 
154  // Render as cubes:
155  const mrpt::math::TPoint3Df vs[8] = {
156  {c.x + L, c.y + L, c.z + L}, {c.x + L, c.y - L, c.z + L},
157  {c.x + L, c.y - L, c.z - L}, {c.x + L, c.y + L, c.z - L},
158  {c.x - L, c.y + L, c.z - L}, {c.x - L, c.y + L, c.z + L},
159  {c.x - L, c.y - L, c.z + L}, {c.x - L, c.y - L, c.z - L}};
160 
161  // Was: glDrawElements(
162  // GL_TRIANGLES, sizeof(cube_indices) / sizeof(cube_indices[0]),
163  // GL_UNSIGNED_BYTE, cube_indices);
164  // glDrawElements(GL_LINES, , GL_UNSIGNED_BYTE, grid_line_indices);
165 
166  const auto& ci = cube_indices;
167  const auto& ns = normals_cube;
168 
169  for (size_t k = 0; k < sizeof(ci) / sizeof(ci[0]); k += 3)
170  {
172  // vertices:
173  vs[ci[k]], vs[ci[k + 1]], vs[ci[k + 2]],
174  // normals:
175  ns[k / 3], ns[k / 3], ns[k / 3]);
176 
177  for (int p = 0; p < 3; p++)
178  {
179  tri.vertices[p].xyzrgba.r = vx_j_col.R;
180  tri.vertices[p].xyzrgba.g = vx_j_col.G;
181  tri.vertices[p].xyzrgba.b = vx_j_col.B;
182  tri.vertices[p].xyzrgba.a = vx_j_col.A;
183  }
184 
185  tris.emplace_back(std::move(tri));
186  }
187  }
188  }
189 }
190 
192 {
195 
196  for (const auto& m_voxel_set : m_voxel_sets)
197  {
198  if (!m_voxel_set.visible) continue;
199 
200  const std::vector<TVoxel>& voxels = m_voxel_set.voxels;
201  const size_t N = voxels.size();
202  for (size_t j = 0; j < N; j++)
203  {
204  const mrpt::img::TColor& vx_j_col = voxels[j].color;
205  const mrpt::math::TPoint3D& c = voxels[j].coords;
206 
207  vbd.emplace_back(c);
208  cbd.emplace_back(vx_j_col);
209  }
210  }
211 }
212 
216 
217 namespace mrpt::opengl
218 {
221 {
222  out << a.visible << a.voxels;
223  return out;
224 }
226 {
227  in >> a.visible >> a.voxels;
228  return in;
229 }
230 
232 {
233  out << a.min << a.max;
234  return out;
235 }
237 {
238  in >> a.min >> a.max;
239  return in;
240 }
241 
243 {
244  out << a.coords << a.side_length << a.color;
245  return out;
246 }
248 {
249  in >> a.coords >> a.side_length >> a.color;
250  return in;
251 }
252 } // end of namespace mrpt::opengl
253 
254 uint8_t COctoMapVoxels::serializeGetVersion() const { return 2; }
256 {
257  writeToStreamRender(out);
258 
259  out << m_voxel_sets << m_grid_cubes << m_bb_min << m_bb_max
260  << m_enable_lighting << m_showVoxelsAsPoints << m_showVoxelsAsPointsSize
261  << m_show_grids << m_grid_width << m_grid_color
262  << m_enable_cube_transparency // added in v1
263  << uint32_t(m_visual_mode); // added in v2
264 }
265 
266 void COctoMapVoxels::serializeFrom(CArchive& in, uint8_t version)
267 {
268  switch (version)
269  {
270  case 0:
271  case 1:
272  case 2:
273  {
274  readFromStreamRender(in);
275 
276  in >> m_voxel_sets >> m_grid_cubes >> m_bb_min >> m_bb_max >>
277  m_enable_lighting >> m_showVoxelsAsPoints >>
278  m_showVoxelsAsPointsSize >> m_show_grids >> m_grid_width >>
279  m_grid_color;
280 
281  if (version >= 1)
282  in >> m_enable_cube_transparency;
283  else
284  m_enable_cube_transparency = false;
285 
286  if (version >= 2)
287  {
288  uint32_t i;
289  in >> i;
290  m_visual_mode =
291  static_cast<COctoMapVoxels::visualization_mode_t>(i);
292  }
293  else
294  m_visual_mode = COctoMapVoxels::COLOR_FROM_OCCUPANCY;
295  }
296  break;
297  default:
299  };
300 
302 }
303 
306 {
307  bb_min = m_bb_min;
308  bb_max = m_bb_max;
309 
310  // Convert to coordinates of my parent:
311  m_pose.composePoint(bb_min, bb_min);
312  m_pose.composePoint(bb_max, bb_max);
313 }
314 
317 {
318  return a.coords.z < b.coords.z;
319 }
320 
322 {
323  for (auto& m_voxel_set : m_voxel_sets)
324  {
325  std::sort(
326  m_voxel_set.voxels.begin(), m_voxel_set.voxels.end(),
327  &sort_voxels_z);
328  }
329 }
void renderUpdateBuffers() const override
Called whenever m_outdatedBuffers is true: used to re-generate OpenGL vertex buffers, etc.
std::vector< mrpt::math::TPoint3Df > m_vertex_buffer_data
visualization_mode_t
The different coloring schemes, which modulate the generic mrpt::opengl::CRenderizable object color...
mrpt::serialization::CArchive & operator>>(mrpt::serialization::CArchive &in, CPolyhedron::TPolyhedronEdge &o)
Reads a polyhedron edge from a binary stream.
#define IMPLEMENTS_SERIALIZABLE(class_name, base, NameSpace)
To be added to all CSerializable-classes implementation files.
void notifyChange() const
Call to enable calling renderUpdateBuffers() before the next render() rendering iteration.
void onUpdateBuffers_Wireframe() override
Must be implemented in derived classes to update the geometric entities to be drawn in "m_*_buffer" f...
A triangle (float coordinates) with RGBA colors (u8) and UV (texture coordinates) for each vertex...
Definition: TTriangle.h:35
void serializeFrom(mrpt::serialization::CArchive &in, uint8_t serial_version) override
Pure virtual method for reading (deserializing) from an abstract archive.
mrpt::math::TPoint3Df min
opposite corners of the cube
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)
static const uint8_t grid_line_indices[]
COpenGLScene::Ptr & operator<<(COpenGLScene::Ptr &s, const CRenderizable::Ptr &r)
Inserts an openGL object into a scene.
Definition: COpenGLScene.h:252
The base class of 3D objects that can be directly rendered through OpenGL.
Definition: CRenderizable.h:48
STL namespace.
uint8_t B
Definition: TColor.h:51
uint8_t G
Definition: TColor.h:51
void clear()
Clears everything.
bool sort_voxels_z(const COctoMapVoxels::TVoxel &a, const COctoMapVoxels::TVoxel &b)
void renderUpdateBuffers() const override
Called whenever m_outdatedBuffers is true: used to re-generate OpenGL vertex buffers, etc.
void renderUpdateBuffers() const override
Called whenever m_outdatedBuffers is true: used to re-generate OpenGL vertex buffers, etc.
Context for calls to render()
std::vector< mrpt::math::TPoint3Df > m_vertex_buffer_data
#define MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(__V)
For use in CSerializable implementations.
Definition: exceptions.h:97
std::vector< mrpt::img::TColor > m_color_buffer_data
A flexible renderer of voxels, typically from a 3D octo map (see mrpt::maps::COctoMap).
std::array< Vertex, 3 > vertices
Definition: TTriangle.h:88
This base provides a set of functions for maths stuff.
static constexpr shader_id_t WIREFRAME
void onUpdateBuffers_Triangles() override
Must be implemented in derived classes to update the geometric entities to be drawn in "m_*_buffer" f...
uint8_t R
Definition: TColor.h:51
static const uint8_t cube_indices[2 *6 *3]
The info of each grid block.
static constexpr shader_id_t TRIANGLES
void serializeTo(mrpt::serialization::CArchive &out) const override
Pure virtual method for writing (serializing) to an abstract archive.
void onUpdateBuffers_Points() override
Must be implemented in derived classes to update the geometric entities to be drawn in "m_*_buffer" f...
T x
X,Y,Z coordinates.
Definition: TPoint3D.h:29
uint8_t serializeGetVersion() const override
Must return the current versioning number of the object.
The info of each of the voxels.
std::vector< mrpt::img::TColor > m_color_buffer_data
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...
std::vector< mrpt::opengl::TTriangle > m_triangles
List of triangles.
#define DECLARE_CUSTOM_TTYPENAME(_TYPE)
Identical to MRPT_DECLARE_TTYPENAME but intended for user code.
Definition: TTypeName.h:90
mrpt::vision::TStereoCalibResults out
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:13
const auto bb_max
void render(const RenderContext &rc) const override
Implements the rendering of 3D objects in each class derived from CRenderizable.
static const mrpt::math::TPoint3Df normals_cube[6 *2]
const auto bb_min
A RGB color - 8bit.
Definition: TColor.h:25
void render(const RenderContext &rc) const override
Implements the rendering of 3D objects in each class derived from CRenderizable.
void render(const RenderContext &rc) const override
Implements the rendering of 3D objects in each class derived from CRenderizable.
static constexpr shader_id_t POINTS
uint8_t A
Definition: TColor.h:51
void render(const RenderContext &rc) const override
Implements the rendering of 3D objects in each class derived from CRenderizable.
void renderUpdateBuffers() const override
Called whenever m_outdatedBuffers is true: used to re-generate OpenGL vertex buffers, etc.



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020