Main MRPT website > C++ reference for MRPT 1.9.9
COctreePointRenderer.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-2017, 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 #ifndef opengl_COctreePointRenderer_H
10 #define opengl_COctreePointRenderer_H
11 
12 #include <mrpt/opengl/CRenderizable.h>
13 #include <mrpt/opengl/CSetOfObjects.h>
14 #include <mrpt/opengl/CBox.h>
15 #include <mrpt/opengl/gl_utils.h>
16 #include <mrpt/utils/aligned_containers.h>
17 
18 namespace mrpt
19 {
20 namespace global_settings
21 {
22 /** Default value = 0.01 points/px^2. Affects to these classes (read their docs
23  *for further details):
24  * - mrpt::opengl::CPointCloud
25  * - mrpt::opengl::CPointCloudColoured
26  * \ingroup mrpt_opengl_grp
27  */
28 void OCTREE_RENDER_MAX_DENSITY_POINTS_PER_SQPIXEL(float value);
29 float OCTREE_RENDER_MAX_DENSITY_POINTS_PER_SQPIXEL();
30 
31 /** Default value = 1e5. Maximum number of elements in each octree node before
32  *spliting. Affects to these classes (read their docs for further details):
33  * - mrpt::opengl::CPointCloud
34  * - mrpt::opengl::CPointCloudColoured
35  * \ingroup mrpt_opengl_grp
36  */
37 size_t OCTREE_RENDER_MAX_POINTS_PER_NODE();
38 void OCTREE_RENDER_MAX_POINTS_PER_NODE(size_t value);
39 
40 }
41 
42 namespace opengl
43 {
44 /** Template class that implements the data structure and algorithms for
45  * Octree-based efficient rendering.
46  * \sa mrpt::opengl::CPointCloud, mrpt::opengl::CPointCloudColoured,
47  * http://www.mrpt.org/Efficiently_rendering_point_clouds_of_millions_of_points
48  * \ingroup mrpt_opengl_grp
49  */
50 template <class Derived>
51 class COctreePointRenderer
52 {
53  public:
54  /** Default ctor */
55  COctreePointRenderer()
56  : m_octree_has_to_rebuild_all(true),
57  m_visible_octree_nodes(0),
58  m_visible_octree_nodes_ongoing(0)
59  {
60  }
61 
62  /** Copy ctor */
63  COctreePointRenderer(const COctreePointRenderer&)
64  : m_octree_has_to_rebuild_all(true),
65  m_visible_octree_nodes(0),
66  m_visible_octree_nodes_ongoing(0)
67  {
68  }
69 
70  enum
71  {
72  OCTREE_ROOT_NODE = 0
73  };
74 
75  protected:
76  // Helper methods in any CRTP template
77  inline Derived& octree_derived() { return *static_cast<Derived*>(this); }
78  inline const Derived& octree_derived() const
79  {
80  return *static_cast<const Derived*>(this);
81  }
82 
83  /** Must be called at children class' render() previously to \a
84  * octree_render() */
85  inline void octree_assure_uptodate() const
86  {
87  const_cast<COctreePointRenderer<Derived>*>(this)
88  ->internal_octree_assure_uptodate();
89  }
90 
91  /** Render the entire octree recursively.
92  * Should be called from children's render() method.
93  */
94  void octree_render(const mrpt::opengl::gl_utils::TRenderInfo& ri) const
95  {
96  m_visible_octree_nodes_ongoing = 0;
97 
98  // Stage 1: Build list of visible octrees
99  m_render_queue.clear();
100  m_render_queue.reserve(m_octree_nodes.size());
101 
102  mrpt::utils::TPixelCoordf cr_px[8];
103  float cr_z[8];
104  octree_recursive_render(
105  OCTREE_ROOT_NODE, ri, cr_px, cr_z,
106  false /* corners are not computed for this first iteration */);
107 
108  m_visible_octree_nodes = m_visible_octree_nodes_ongoing;
109 
110  // Stage 2: Render them all
111  for (size_t i = 0; i < m_render_queue.size(); i++)
112  {
113  const TNode& node = m_octree_nodes[m_render_queue[i].node_id];
114  octree_derived().render_subset(
115  node.all, node.pts, m_render_queue[i].render_area_sqpixels);
116  }
117  }
118 
119  void octree_getBoundingBox(
120  mrpt::math::TPoint3D& bb_min, mrpt::math::TPoint3D& bb_max) const
121  {
122  octree_assure_uptodate();
123  if (!m_octree_nodes.empty())
124  {
125  bb_min = mrpt::math::TPoint3D(m_octree_nodes[0].bb_min);
126  bb_max = mrpt::math::TPoint3D(m_octree_nodes[0].bb_max);
127  }
128  }
129 
130  private:
131  /** The structure for each octree spatial node. Each node can either be a
132  * leaf of has 8 children nodes.
133  * Instead of pointers, children are referenced by their indices in \a
134  * m_octree_nodes
135  */
136  struct TNode
137  {
138  TNode()
139  : is_leaf(true),
140  bb_min(
141  std::numeric_limits<float>::max(),
142  std::numeric_limits<float>::max(),
143  std::numeric_limits<float>::max()),
144  bb_max(
145  -std::numeric_limits<float>::max(),
146  -std::numeric_limits<float>::max(),
147  -std::numeric_limits<float>::max()),
148  all(false)
149  {
150  }
151 
152  /** true: it's a leaf and \a pts has valid indices; false: \a children
153  * is valid. */
154  bool is_leaf;
155 
156  // In all cases, the bounding_box:
157  mrpt::math::TPoint3Df bb_min, bb_max;
158 
159  // Fields used if is_leaf=true
160  /** Point indices in the derived class that fall into this node. */
161  std::vector<size_t> pts;
162  /** true: All elements in the reference object; false: only those in \a
163  * pts */
164  bool all;
165 
166  // Fields used if is_leaf=false
167  /** [is_leaf=false] The center of the node, whose coordinates are used
168  * to decide between the 8 children nodes. */
169  mrpt::math::TPoint3Df center;
170  /** [is_leaf=false] The indices in \a m_octree_nodes of the 8 children.
171  */
172  size_t child_id[8];
173 
174  /** update bounding box with a new point: */
175  inline void update_bb(const mrpt::math::TPoint3Df& p)
176  {
177  mrpt::utils::keep_min(bb_min.x, p.x);
178  mrpt::utils::keep_min(bb_min.y, p.y);
179  mrpt::utils::keep_min(bb_min.z, p.z);
180  mrpt::utils::keep_max(bb_max.x, p.x);
181  mrpt::utils::keep_max(bb_max.y, p.y);
182  mrpt::utils::keep_max(bb_max.z, p.z);
183  }
184 
185  inline float getCornerX(int i) const
186  {
187  return (i & 0x01) == 0 ? bb_min.x : bb_max.x;
188  }
189  inline float getCornerY(int i) const
190  {
191  return (i & 0x02) == 0 ? bb_min.y : bb_max.y;
192  }
193  inline float getCornerZ(int i) const
194  {
195  return (i & 0x04) == 0 ? bb_min.z : bb_max.z;
196  }
197 
198  void setBBFromOrderInParent(const TNode& parent, int my_child_index)
199  {
200  // Coordinate signs are relative to the parent center (split point):
201  switch (my_child_index)
202  {
203  case 0: // x-, y-, z-
204  bb_min = parent.bb_min;
205  bb_max = parent.center;
206  break;
207  case 1: // x+, y-, z-
208  bb_min.x = parent.center.x;
209  bb_max.x = parent.bb_max.x;
210  bb_min.y = parent.bb_min.y;
211  bb_max.y = parent.center.y;
212  bb_min.z = parent.bb_min.z;
213  bb_max.z = parent.center.z;
214  break;
215  case 2: // x-, y+, z-
216  bb_min.x = parent.bb_min.x;
217  bb_max.x = parent.center.x;
218  bb_min.y = parent.center.y;
219  bb_max.y = parent.bb_max.y;
220  bb_min.z = parent.bb_min.z;
221  bb_max.z = parent.center.z;
222  break;
223  case 3: // x+, y+, z-
224  bb_min.x = parent.center.x;
225  bb_max.x = parent.bb_max.x;
226  bb_min.y = parent.center.y;
227  bb_max.y = parent.bb_max.y;
228  bb_min.z = parent.bb_min.z;
229  bb_max.z = parent.center.z;
230  break;
231  case 4: // x-, y-, z+
232  bb_min.x = parent.bb_min.x;
233  bb_max.x = parent.center.x;
234  bb_min.y = parent.bb_min.y;
235  bb_max.y = parent.center.y;
236  bb_min.z = parent.center.z;
237  bb_max.z = parent.bb_max.z;
238  break;
239  case 5: // x+, y-, z+
240  bb_min.x = parent.center.x;
241  bb_max.x = parent.bb_max.x;
242  bb_min.y = parent.bb_min.y;
243  bb_max.y = parent.center.y;
244  bb_min.z = parent.center.z;
245  bb_max.z = parent.bb_max.z;
246  break;
247  case 6: // x-, y+, z+
248  bb_min.x = parent.bb_min.x;
249  bb_max.x = parent.center.x;
250  bb_min.y = parent.center.y;
251  bb_max.y = parent.bb_max.y;
252  bb_min.z = parent.center.z;
253  bb_max.z = parent.bb_max.z;
254  break;
255  case 7: // x+, y+, z+
256  bb_min = parent.center;
257  bb_max = parent.bb_max;
258  break;
259  default:
260  throw std::runtime_error("my_child_index!=[0,7]");
261  }
262  }
263 
264  public:
265  MRPT_MAKE_ALIGNED_OPERATOR_NEW
266  };
267 
268  struct TRenderQueueElement
269  {
270  inline TRenderQueueElement(const size_t id, float area_sq)
271  : node_id(id), render_area_sqpixels(area_sq)
272  {
273  }
274 
275  /** The node ID to render */
276  size_t node_id;
277  /** The approximate size of the octree on the screen (squared pixels).
278  */
279  float render_area_sqpixels;
280  };
281  /** The list of elements that really are visible and will be rendered. */
282  mutable std::vector<TRenderQueueElement> m_render_queue;
283 
284  bool m_octree_has_to_rebuild_all;
285  /** First one [0] is always the root node */
286  typename mrpt::aligned_containers<TNode>::deque_t m_octree_nodes;
287 
288  // Counters of visible octrees for each render:
289  volatile mutable size_t m_visible_octree_nodes,
290  m_visible_octree_nodes_ongoing;
291 
292  /** Render a given node. */
293  void octree_recursive_render(
294  size_t node_idx, const mrpt::opengl::gl_utils::TRenderInfo& ri,
295  mrpt::utils::TPixelCoordf cr_px[8], float cr_z[8],
296  bool corners_are_all_computed = true,
297  bool trust_me_youre_visible = false,
298  float approx_area_sqpixels = 0) const
299  {
300  const TNode& node = m_octree_nodes[node_idx];
301 
302  if (!corners_are_all_computed)
303  {
304  for (int i = 0; i < 8; i++)
305  {
306  // project point:
307  ri.projectPointPixels(
308  node.getCornerX(i), node.getCornerY(i), node.getCornerZ(i),
309  cr_px[i].x, cr_px[i].y, cr_z[i]);
310  }
311  }
312 
313  mrpt::utils::TPixelCoordf px_min(
314  std::numeric_limits<float>::max(),
315  std::numeric_limits<float>::max()),
316  px_max(
317  -std::numeric_limits<float>::max(),
318  -std::numeric_limits<float>::max());
319  if (!trust_me_youre_visible)
320  {
321  // Keep the on-screen bounding box of this node:
322  for (int i = 0; i < 8; i++)
323  {
324  mrpt::utils::keep_min(px_min.x, cr_px[i].x);
325  mrpt::utils::keep_min(px_min.y, cr_px[i].y);
326  mrpt::utils::keep_max(px_max.x, cr_px[i].x);
327  mrpt::utils::keep_max(px_max.y, cr_px[i].y);
328  }
329 
330  const bool any_cr_zs_neg =
331  (cr_z[0] < 0 || cr_z[1] < 0 || cr_z[2] < 0 || cr_z[3] < 0 ||
332  cr_z[4] < 0 || cr_z[5] < 0 || cr_z[6] < 0 || cr_z[7] < 0);
333  const bool any_cr_zs_pos =
334  (cr_z[0] > 0 || cr_z[1] > 0 || cr_z[2] > 0 || cr_z[3] > 0 ||
335  cr_z[4] > 0 || cr_z[5] > 0 || cr_z[6] > 0 || cr_z[7] > 0);
336  const bool box_crosses_image_plane = any_cr_zs_pos && any_cr_zs_neg;
337 
338  // If all 8 corners are way out of the screen (and all "cr_z" have
339  // the same sign),
340  // this node and all the children are not visible:
341  if (!box_crosses_image_plane &&
342  (px_min.x >= ri.vp_width || px_min.y >= ri.vp_height ||
343  px_max.x < 0 || px_max.y < 0))
344  return; // Not visible
345  }
346 
347  // Check if the node has points and is visible:
348  if (node.is_leaf)
349  { // Render this leaf node:
350  if (node.all || !node.pts.empty())
351  {
352  // If we are here, it seems at least a part of the Box is
353  // visible:
354  m_visible_octree_nodes_ongoing++;
355 
356  const float render_area_sqpixels =
357  trust_me_youre_visible ? approx_area_sqpixels
358  : std::abs(px_min.x - px_max.x) *
359  std::abs(px_min.y - px_max.y);
360 
361  // OK: Add to list of rendering-pending:
362  m_render_queue.push_back(
363  TRenderQueueElement(node_idx, render_area_sqpixels));
364  }
365  }
366  else
367  { // Render children nodes:
368  // If ALL my 8 corners are within the screen, tell our children that
369  // they
370  // won't need to compute anymore, since all of them and their
371  // children are visible as well:
372  bool children_are_all_visible_for_sure = true;
373 
374  if (!trust_me_youre_visible) // Trust my parent... otherwise:
375  {
376  for (int i = 0; i < 8; i++)
377  {
378  if (!(cr_px[i].x >= 0 && cr_px[i].y >= 0 &&
379  cr_px[i].x < ri.vp_width &&
380  cr_px[i].y < ri.vp_height))
381  {
382  children_are_all_visible_for_sure = false;
383  break;
384  }
385  }
386  }
387 
388  // If all children are visible, it's easy:
389  if (children_are_all_visible_for_sure)
390  {
391  mrpt::utils::TPixelCoordf
392  child_cr_px[8]; // No need to initialize
393  float child_cr_z[8]; // No need to initialize
394 
395  // Approximate area of the children nodes:
396  const float approx_child_area =
397  trust_me_youre_visible
398  ? approx_area_sqpixels / 8.0f
399  : std::abs(px_min.x - px_max.x) *
400  std::abs(px_min.y - px_max.y) / 8.0f;
401 
402  for (int i = 0; i < 8; i++)
403  this->octree_recursive_render(
404  node.child_id[i], ri, child_cr_px, child_cr_z, true,
405  true, approx_child_area);
406  }
407  else
408  {
409 #ifdef __clang__
410 #pragma clang diagnostic push // clang complains about unused vars (becase it
411 // doesn't realize of the macros?)
412 #pragma clang diagnostic ignored "-Wunused-variable"
413 #endif
414 
415  // Precompute the 19 (3*9-8) intermediary points so children
416  // don't have to compute them several times:
417  const mrpt::math::TPoint3Df p_Xm_Ym_Zm(
418  node.bb_min.x, node.bb_min.y, node.bb_min.z); // 0
419  const mrpt::math::TPoint3Df p_X0_Ym_Zm(
420  node.center.x, node.bb_min.y, node.bb_min.z);
421  const mrpt::math::TPoint3Df p_Xp_Ym_Zm(
422  node.bb_max.x, node.bb_min.y, node.bb_min.z); // 1
423  const mrpt::math::TPoint3Df p_Xm_Y0_Zm(
424  node.bb_min.x, node.center.y, node.bb_min.z);
425  const mrpt::math::TPoint3Df p_X0_Y0_Zm(
426  node.center.x, node.center.y, node.bb_min.z);
427  const mrpt::math::TPoint3Df p_Xp_Y0_Zm(
428  node.bb_max.x, node.center.y, node.bb_min.z);
429  const mrpt::math::TPoint3Df p_Xm_Yp_Zm(
430  node.bb_min.x, node.bb_max.y, node.bb_min.z); // 2
431  const mrpt::math::TPoint3Df p_X0_Yp_Zm(
432  node.center.x, node.bb_max.y, node.bb_min.z);
433  const mrpt::math::TPoint3Df p_Xp_Yp_Zm(
434  node.bb_max.x, node.bb_max.y, node.bb_min.z); // 3
435 
436  const mrpt::math::TPoint3Df p_Xm_Ym_Z0(
437  node.bb_min.x, node.bb_min.y, node.center.z);
438  const mrpt::math::TPoint3Df p_X0_Ym_Z0(
439  node.center.x, node.bb_min.y, node.center.z);
440  const mrpt::math::TPoint3Df p_Xp_Ym_Z0(
441  node.bb_max.x, node.bb_min.y, node.center.z);
442  const mrpt::math::TPoint3Df p_Xm_Y0_Z0(
443  node.bb_min.x, node.center.y, node.center.z);
444  const mrpt::math::TPoint3Df p_X0_Y0_Z0(
445  node.center.x, node.center.y, node.center.z);
446  const mrpt::math::TPoint3Df p_Xp_Y0_Z0(
447  node.bb_max.x, node.center.y, node.center.z);
448  const mrpt::math::TPoint3Df p_Xm_Yp_Z0(
449  node.bb_min.x, node.bb_max.y, node.center.z);
450  const mrpt::math::TPoint3Df p_X0_Yp_Z0(
451  node.center.x, node.bb_max.y, node.center.z);
452  const mrpt::math::TPoint3Df p_Xp_Yp_Z0(
453  node.bb_max.x, node.bb_max.y, node.center.z);
454 
455  const mrpt::math::TPoint3Df p_Xm_Ym_Zp(
456  node.bb_min.x, node.bb_min.y, node.bb_max.z); // 4
457  const mrpt::math::TPoint3Df p_X0_Ym_Zp(
458  node.center.x, node.bb_min.y, node.bb_max.z);
459  const mrpt::math::TPoint3Df p_Xp_Ym_Zp(
460  node.bb_min.x, node.bb_min.y, node.bb_max.z); // 5
461  const mrpt::math::TPoint3Df p_Xm_Y0_Zp(
462  node.bb_min.x, node.center.y, node.bb_max.z);
463  const mrpt::math::TPoint3Df p_X0_Y0_Zp(
464  node.center.x, node.center.y, node.bb_max.z);
465  const mrpt::math::TPoint3Df p_Xp_Y0_Zp(
466  node.bb_max.x, node.center.y, node.bb_max.z);
467  const mrpt::math::TPoint3Df p_Xm_Yp_Zp(
468  node.bb_min.x, node.bb_max.y, node.bb_max.z); // 6
469  const mrpt::math::TPoint3Df p_X0_Yp_Zp(
470  node.center.x, node.bb_max.y, node.bb_max.z);
471  const mrpt::math::TPoint3Df p_Xp_Yp_Zp(
472  node.bb_max.x, node.bb_max.y, node.bb_max.z); // 7
473 
474 // Project all these points:
475 #define PROJ_SUB_NODE(POSTFIX) \
476  mrpt::utils::TPixelCoordf px_##POSTFIX; \
477  float depth_##POSTFIX; \
478  ri.projectPointPixels( \
479  p_##POSTFIX.x, p_##POSTFIX.y, p_##POSTFIX.z, px_##POSTFIX.x, \
480  px_##POSTFIX.y, depth_##POSTFIX);
481 
482 #define PROJ_SUB_NODE_ALREADY_DONE(INDEX, POSTFIX) \
483  const mrpt::utils::TPixelCoordf px_##POSTFIX = cr_px[INDEX]; \
484  float depth_##POSTFIX = cr_z[INDEX];
485 
486  PROJ_SUB_NODE_ALREADY_DONE(0, Xm_Ym_Zm)
487  PROJ_SUB_NODE(X0_Ym_Zm)
488  PROJ_SUB_NODE_ALREADY_DONE(1, Xp_Ym_Zm)
489 
490  PROJ_SUB_NODE(Xm_Y0_Zm)
491  PROJ_SUB_NODE(X0_Y0_Zm)
492  PROJ_SUB_NODE(Xp_Y0_Zm)
493 
494  PROJ_SUB_NODE_ALREADY_DONE(2, Xm_Yp_Zm)
495  PROJ_SUB_NODE(X0_Yp_Zm)
496  PROJ_SUB_NODE_ALREADY_DONE(3, Xp_Yp_Zm)
497 
498  PROJ_SUB_NODE(Xm_Ym_Z0)
499  PROJ_SUB_NODE(X0_Ym_Z0)
500  PROJ_SUB_NODE(Xp_Ym_Z0)
501  PROJ_SUB_NODE(Xm_Y0_Z0)
502  PROJ_SUB_NODE(X0_Y0_Z0)
503  PROJ_SUB_NODE(Xp_Y0_Z0)
504  PROJ_SUB_NODE(Xm_Yp_Z0)
505  PROJ_SUB_NODE(X0_Yp_Z0)
506  PROJ_SUB_NODE(Xp_Yp_Z0)
507 
508  PROJ_SUB_NODE_ALREADY_DONE(4, Xm_Ym_Zp)
509  PROJ_SUB_NODE(X0_Ym_Zp)
510  PROJ_SUB_NODE_ALREADY_DONE(5, Xp_Ym_Zp)
511 
512  PROJ_SUB_NODE(Xm_Y0_Zp)
513  PROJ_SUB_NODE(X0_Y0_Zp)
514  PROJ_SUB_NODE(Xp_Y0_Zp)
515 
516  PROJ_SUB_NODE_ALREADY_DONE(6, Xm_Yp_Zp)
517  PROJ_SUB_NODE(X0_Yp_Zp)
518  PROJ_SUB_NODE_ALREADY_DONE(7, Xp_Yp_Zp)
519 
520 // Recursive call children nodes:
521 #define DO_RECURSE_CHILD( \
522  INDEX, SEQ0, SEQ1, SEQ2, SEQ3, SEQ4, SEQ5, SEQ6, SEQ7) \
523  { \
524  mrpt::utils::TPixelCoordf child_cr_px[8] = { \
525  px_##SEQ0, px_##SEQ1, px_##SEQ2, px_##SEQ3, \
526  px_##SEQ4, px_##SEQ5, px_##SEQ6, px_##SEQ7}; \
527  float child_cr_z[8] = {depth_##SEQ0, depth_##SEQ1, depth_##SEQ2, \
528  depth_##SEQ3, depth_##SEQ4, depth_##SEQ5, \
529  depth_##SEQ6, depth_##SEQ7}; \
530  this->octree_recursive_render( \
531  node.child_id[INDEX], ri, child_cr_px, child_cr_z); \
532  }
533 
534  // 0 1 2 3
535  // 4 5 6 7
536  DO_RECURSE_CHILD(
537  0, Xm_Ym_Zm, X0_Ym_Zm, Xm_Y0_Zm, X0_Y0_Zm, Xm_Ym_Z0,
538  X0_Ym_Z0, Xm_Y0_Z0, X0_Y0_Z0)
539  DO_RECURSE_CHILD(
540  1, X0_Ym_Zm, Xp_Ym_Zm, X0_Y0_Zm, Xp_Y0_Zm, X0_Ym_Z0,
541  Xp_Ym_Z0, X0_Y0_Z0, Xp_Y0_Z0)
542  DO_RECURSE_CHILD(
543  2, Xm_Y0_Zm, X0_Y0_Zm, Xm_Yp_Zm, X0_Yp_Zm, Xm_Y0_Z0,
544  X0_Y0_Z0, Xm_Yp_Z0, X0_Yp_Z0)
545  DO_RECURSE_CHILD(
546  3, X0_Y0_Zm, Xp_Y0_Zm, X0_Yp_Zm, Xp_Yp_Zm, X0_Y0_Z0,
547  Xp_Y0_Z0, X0_Yp_Z0, Xp_Yp_Z0)
548  DO_RECURSE_CHILD(
549  4, Xm_Ym_Z0, X0_Ym_Z0, Xm_Y0_Z0, X0_Y0_Z0, Xm_Ym_Zp,
550  X0_Ym_Zp, Xm_Y0_Zp, X0_Y0_Zp)
551  DO_RECURSE_CHILD(
552  5, X0_Ym_Z0, Xp_Ym_Z0, X0_Y0_Z0, Xp_Y0_Z0, X0_Ym_Zp,
553  Xp_Ym_Zp, X0_Y0_Zp, Xp_Y0_Zp)
554  DO_RECURSE_CHILD(
555  6, Xm_Y0_Z0, X0_Y0_Z0, Xm_Yp_Z0, X0_Yp_Z0, Xm_Y0_Zp,
556  X0_Y0_Zp, Xm_Yp_Zp, X0_Yp_Zp)
557  DO_RECURSE_CHILD(
558  7, X0_Y0_Z0, Xp_Y0_Z0, X0_Yp_Z0, Xp_Yp_Z0, X0_Y0_Zp,
559  Xp_Y0_Zp, X0_Yp_Zp, Xp_Yp_Zp)
560 #undef DO_RECURSE_CHILD
561 #undef PROJ_SUB_NODE
562 #undef PROJ_SUB_NODE_ALREADY_DONE
563 
564 #ifdef __clang__
565 #pragma clang diagnostic pop
566 #endif
567  } // end "children_are_all_visible_for_sure"=false
568  }
569  }
570 
571  // The actual implementation (and non-const version) of
572  // octree_assure_uptodate()
573  void internal_octree_assure_uptodate()
574  {
575  if (!m_octree_has_to_rebuild_all) return;
576  m_octree_has_to_rebuild_all = false;
577 
578  // Reset list of nodes:
579  m_octree_nodes.assign(1, TNode());
580 
581  // recursive decide:
582  internal_recursive_split(OCTREE_ROOT_NODE, true);
583  }
584 
585  // Check the node "node_id" and create its children if needed, by looking at
586  // its list
587  // of elements (or all derived object's elements if "all_pts"=true, which
588  // will only happen
589  // for the root node)
590  void internal_recursive_split(
591  const size_t node_id, const bool all_pts = false)
592  {
593  TNode& node = m_octree_nodes[node_id];
594  const size_t N = all_pts ? octree_derived().size() : node.pts.size();
595 
596  const bool has_to_compute_bb = (node_id == OCTREE_ROOT_NODE);
597 
598  if (N <= mrpt::global_settings::OCTREE_RENDER_MAX_POINTS_PER_NODE())
599  {
600  // No need to split this node:
601  node.is_leaf = true;
602  node.all = all_pts;
603 
604  // Update bounding-box:
605  if (has_to_compute_bb)
606  {
607  if (all_pts)
608  for (size_t i = 0; i < N; i++)
609  node.update_bb(octree_derived().getPointf(i));
610  else
611  for (size_t i = 0; i < N; i++)
612  node.update_bb(octree_derived().getPointf(node.pts[i]));
613  }
614  }
615  else
616  {
617  // We have to split the node.
618  // Compute the mean of all elements:
619  mrpt::math::TPoint3Df mean(0, 0, 0);
620  if (all_pts)
621  for (size_t i = 0; i < N; i++)
622  {
623  mrpt::math::TPoint3Df p = octree_derived().getPointf(i);
624  mean += p;
625  if (has_to_compute_bb) node.update_bb(p);
626  }
627  else
628  for (size_t i = 0; i < N; i++)
629  {
630  mrpt::math::TPoint3Df p =
631  octree_derived().getPointf(node.pts[i]);
632  mean += p;
633  if (has_to_compute_bb) node.update_bb(p);
634  }
635 
636  // Save my split point:
637  node.is_leaf = false;
638  node.center = mean * (1.0f / N);
639 
640  // Allocate my 8 children structs
641  const size_t children_idx_base = m_octree_nodes.size();
642  m_octree_nodes.resize(children_idx_base + 8);
643  for (int i = 0; i < 8; i++)
644  node.child_id[i] = children_idx_base + i;
645 
646  // Set the bounding-boxes of my children (we already know them):
647  for (int i = 0; i < 8; i++)
648  m_octree_nodes[children_idx_base + i].setBBFromOrderInParent(
649  node, i);
650 
651  // Divide elements among children:
652  const mrpt::math::TPoint3Df& c =
653  node.center; // to make notation clearer
654  for (size_t j = 0; j < N; j++)
655  {
656  const size_t i = all_pts ? j : node.pts[j];
657  const mrpt::math::TPoint3Df p = octree_derived().getPointf(i);
658  if (p.z < c.z)
659  {
660  if (p.y < c.y)
661  {
662  if (p.x < c.x)
663  m_octree_nodes[children_idx_base + 0].pts.push_back(
664  i);
665  else
666  m_octree_nodes[children_idx_base + 1].pts.push_back(
667  i);
668  }
669  else
670  {
671  if (p.x < c.x)
672  m_octree_nodes[children_idx_base + 2].pts.push_back(
673  i);
674  else
675  m_octree_nodes[children_idx_base + 3].pts.push_back(
676  i);
677  }
678  }
679  else
680  {
681  if (p.y < c.y)
682  {
683  if (p.x < c.x)
684  m_octree_nodes[children_idx_base + 4].pts.push_back(
685  i);
686  else
687  m_octree_nodes[children_idx_base + 5].pts.push_back(
688  i);
689  }
690  else
691  {
692  if (p.x < c.x)
693  m_octree_nodes[children_idx_base + 6].pts.push_back(
694  i);
695  else
696  m_octree_nodes[children_idx_base + 7].pts.push_back(
697  i);
698  }
699  }
700  }
701 
702  // Clear list of elements (they're now in our children):
703  {
704  std::vector<size_t> emptyVec;
705  node.pts.swap(emptyVec); // This is THE way of really clearing
706  // a std::vector
707  }
708 
709  // Recursive call on children:
710  for (int i = 0; i < 8; i++)
711  internal_recursive_split(node.child_id[i]);
712  }
713  } // end of internal_recursive_split
714 
715  public:
716  /** Return the number of octree nodes (all of them, including the empty
717  * ones) \sa octree_get_nonempty_node_count */
718  size_t octree_get_node_count() const { return m_octree_nodes.size(); }
719  /** Return the number of visible octree nodes in the last render event. */
720  size_t octree_get_visible_nodes() const { return m_visible_octree_nodes; }
721  /** Called from the derived class (or the user) to indicate we have/want to
722  * rebuild the entire node tree (for example, after modifying the point
723  * cloud or any global octree parameter) */
724  inline void octree_mark_as_outdated()
725  {
726  m_octree_has_to_rebuild_all = true;
727  }
728 
729  /** Returns a graphical representation of all the bounding boxes of the
730  * octree (leaf) nodes.
731  * \param[in] draw_solid_boxes If false, will draw solid boxes of color \a
732  * lines_color. Otherwise, wireframe boxes will be drawn.
733  */
734  void octree_get_graphics_boundingboxes(
735  mrpt::opengl::CSetOfObjects& gl_bb, const double lines_width = 1,
736  const mrpt::utils::TColorf& lines_color = mrpt::utils::TColorf(1, 1, 1),
737  const bool draw_solid_boxes = false) const
738  {
739  octree_assure_uptodate();
740  gl_bb.clear();
741  for (size_t i = 0; i < m_octree_nodes.size(); i++)
742  {
743  const TNode& node = m_octree_nodes[i];
744  if (!node.is_leaf) continue;
745  mrpt::opengl::CBox::Ptr gl_box =
746  mrpt::make_aligned_shared<mrpt::opengl::CBox>();
747  gl_box->setBoxCorners(
748  mrpt::math::TPoint3D(node.bb_min),
749  mrpt::math::TPoint3D(node.bb_max));
750  gl_box->setColor(lines_color);
751  gl_box->setLineWidth(lines_width);
752  gl_box->setWireframe(!draw_solid_boxes);
753  gl_bb.insert(gl_box);
754  }
755  }
756 
757  /** Used for debug only */
758  void octree_debug_dump_tree(std::ostream& o) const
759  {
760  o << "Octree nodes: " << m_octree_nodes.size() << std::endl;
761  size_t total_elements = 0;
762  for (size_t i = 0; i < m_octree_nodes.size(); i++)
763  {
764  const TNode& node = m_octree_nodes[i];
765 
766  o << "Node #" << i << ": ";
767  if (node.is_leaf)
768  {
769  o << "leaf, ";
770  if (node.all)
771  {
772  o << "(all)\n";
773  total_elements += octree_derived().size();
774  }
775  else
776  {
777  o << node.pts.size() << " elements; ";
778  total_elements += node.pts.size();
779  }
780  }
781  else
782  {
783  o << "parent, center=(" << node.center.x << "," << node.center.y
784  << "," << node.center.z << "), children: " << node.child_id[0]
785  << "," << node.child_id[1] << "," << node.child_id[2] << ","
786  << node.child_id[3] << "," << node.child_id[4] << ","
787  << node.child_id[5] << "," << node.child_id[6] << ","
788  << node.child_id[7] << "; ";
789  }
790  o << " bb: (" << node.bb_min.x << "," << node.bb_min.y << ","
791  << node.bb_min.z << ")-(" << node.bb_max.x << "," << node.bb_max.y
792  << "," << node.bb_max.z << ")\n";
793  }
794  o << "Total elements in all nodes: " << total_elements << std::endl;
795  } // end of octree_debug_dump_tree
796 
797 }; // end of class COctreePointRenderer
798 
799 } // end namespace
800 } // End of namespace
801 #endif
PROJ_SUB_NODE
#define PROJ_SUB_NODE(POSTFIX)
DO_RECURSE_CHILD
#define DO_RECURSE_CHILD( INDEX, SEQ0, SEQ1, SEQ2, SEQ3, SEQ4, SEQ5, SEQ6, SEQ7)
PROJ_SUB_NODE_ALREADY_DONE
#define PROJ_SUB_NODE_ALREADY_DONE(INDEX, POSTFIX)
aligned_containers.h
mrpt::opengl::CBox::Ptr
std::shared_ptr< CBox > Ptr
Definition: CBox.h:44
mrpt::opengl::COctreePointRenderer
Template class that implements the data structure and algorithms for Octree-based efficient rendering...
Definition: COctreePointRenderer.h:52
mrpt::opengl::COctreePointRenderer::m_octree_nodes
mrpt::aligned_containers< TNode >::deque_t m_octree_nodes
First one [0] is always the root node.
Definition: COctreePointRenderer.h:286
mrpt::opengl::COctreePointRenderer::octree_mark_as_outdated
void octree_mark_as_outdated()
Called from the derived class (or the user) to indicate we have/want to rebuild the entire node tree ...
Definition: COctreePointRenderer.h:724
mrpt::opengl::COctreePointRenderer::octree_get_graphics_boundingboxes
void octree_get_graphics_boundingboxes(mrpt::opengl::CSetOfObjects &gl_bb, const double lines_width=1, const mrpt::utils::TColorf &lines_color=mrpt::utils::TColorf(1, 1, 1), const bool draw_solid_boxes=false) const
Returns a graphical representation of all the bounding boxes of the octree (leaf) nodes.
Definition: COctreePointRenderer.h:734
mrpt::opengl::COctreePointRenderer::octree_assure_uptodate
void octree_assure_uptodate() const
Must be called at children class' render() previously to octree_render()
Definition: COctreePointRenderer.h:85
mrpt::opengl::COctreePointRenderer::m_render_queue
std::vector< TRenderQueueElement > m_render_queue
The list of elements that really are visible and will be rendered.
Definition: COctreePointRenderer.h:282
mrpt::opengl::COctreePointRenderer::octree_derived
const Derived & octree_derived() const
Definition: COctreePointRenderer.h:78
mrpt::opengl::COctreePointRenderer::octree_render
void octree_render(const mrpt::opengl::gl_utils::TRenderInfo &ri) const
Render the entire octree recursively.
Definition: COctreePointRenderer.h:94
mrpt::opengl::COctreePointRenderer::octree_get_visible_nodes
size_t octree_get_visible_nodes() const
Return the number of visible octree nodes in the last render event.
Definition: COctreePointRenderer.h:720
mrpt::opengl::COctreePointRenderer::COctreePointRenderer
COctreePointRenderer(const COctreePointRenderer &)
Copy ctor.
Definition: COctreePointRenderer.h:63
mrpt::opengl::COctreePointRenderer::octree_getBoundingBox
void octree_getBoundingBox(mrpt::math::TPoint3D &bb_min, mrpt::math::TPoint3D &bb_max) const
Definition: COctreePointRenderer.h:119
mrpt::opengl::COctreePointRenderer::internal_recursive_split
void internal_recursive_split(const size_t node_id, const bool all_pts=false)
Definition: COctreePointRenderer.h:590
mrpt::opengl::COctreePointRenderer::octree_get_node_count
size_t octree_get_node_count() const
Return the number of octree nodes (all of them, including the empty ones)
Definition: COctreePointRenderer.h:718
mrpt::opengl::COctreePointRenderer::octree_debug_dump_tree
void octree_debug_dump_tree(std::ostream &o) const
Used for debug only.
Definition: COctreePointRenderer.h:758
mrpt::opengl::COctreePointRenderer::octree_recursive_render
void octree_recursive_render(size_t node_idx, const mrpt::opengl::gl_utils::TRenderInfo &ri, mrpt::utils::TPixelCoordf cr_px[8], float cr_z[8], bool corners_are_all_computed=true, bool trust_me_youre_visible=false, float approx_area_sqpixels=0) const
Render a given node.
Definition: COctreePointRenderer.h:293
mrpt::opengl::CSetOfObjects
A set of object, which are referenced to the coordinates framework established in this object.
Definition: CSetOfObjects.h:31
mrpt::opengl::CSetOfObjects::insert
void insert(const CRenderizable::Ptr &newObject)
Insert a new object to the list.
Definition: CSetOfObjects.cpp:117
mrpt::opengl::CSetOfObjects::clear
void clear()
Clear the list of objects in the scene, deleting objects' memory.
Definition: CSetOfObjects.cpp:36
mean
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
Definition: eigen_plugins.h:431
gl_utils.h
c
const GLubyte * c
Definition: glext.h:6313
id
GLuint id
Definition: glext.h:3909
y
GLenum GLint GLint y
Definition: glext.h:3538
x
GLenum GLint x
Definition: glext.h:3538
p
GLfloat GLfloat p
Definition: glext.h:6305
value
GLsizei const GLfloat * value
Definition: glext.h:4117
mrpt::global_settings::OCTREE_RENDER_MAX_DENSITY_POINTS_PER_SQPIXEL
void OCTREE_RENDER_MAX_DENSITY_POINTS_PER_SQPIXEL(float value)
Default value = 0.01 points/px^2.
Definition: CPointCloud.cpp:32
mrpt::global_settings::OCTREE_RENDER_MAX_POINTS_PER_NODE
size_t OCTREE_RENDER_MAX_POINTS_PER_NODE()
Default value = 1e5.
Definition: CPointCloud.cpp:37
for
for(ctr=DCTSIZE;ctr > 0;ctr--)
Definition: jidctflt.cpp:56
MRPT_MAKE_ALIGNED_OPERATOR_NEW
#define MRPT_MAKE_ALIGNED_OPERATOR_NEW
Definition: memory.h:134
mrpt::utils::keep_max
void keep_max(T &var, const K test_val)
If the second argument is above the first one, set the first argument to this higher value.
Definition: bits.h:227
mrpt::utils::keep_min
void keep_min(T &var, const K test_val)
If the second argument is below the first one, set the first argument to this lower value.
Definition: bits.h:220
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CParticleFilter.h:18
mrpt::aligned_containers::deque_t
std::deque< TYPE1, Eigen::aligned_allocator< TYPE1 > > deque_t
Definition: aligned_containers.h:34
mrpt::math::TPoint3D
Lightweight 3D point.
Definition: lightweight_geom_data.h:382
mrpt::math::TPoint3Df
Lightweight 3D point (float version).
Definition: lightweight_geom_data.h:319
mrpt::opengl::COctreePointRenderer::TNode
The structure for each octree spatial node.
Definition: COctreePointRenderer.h:137
mrpt::opengl::COctreePointRenderer::TNode::update_bb
void update_bb(const mrpt::math::TPoint3Df &p)
update bounding box with a new point:
Definition: COctreePointRenderer.h:175
mrpt::opengl::COctreePointRenderer::TNode::child_id
size_t child_id[8]
[is_leaf=false] The indices in m_octree_nodes of the 8 children.
Definition: COctreePointRenderer.h:172
mrpt::opengl::COctreePointRenderer::TNode::center
mrpt::math::TPoint3Df center
[is_leaf=false] The center of the node, whose coordinates are used to decide between the 8 children n...
Definition: COctreePointRenderer.h:169
mrpt::opengl::COctreePointRenderer::TNode::setBBFromOrderInParent
void setBBFromOrderInParent(const TNode &parent, int my_child_index)
Definition: COctreePointRenderer.h:198
mrpt::opengl::COctreePointRenderer::TNode::pts
std::vector< size_t > pts
Point indices in the derived class that fall into this node.
Definition: COctreePointRenderer.h:161
mrpt::opengl::COctreePointRenderer::TNode::is_leaf
bool is_leaf
true: it's a leaf and pts has valid indices; false: children is valid.
Definition: COctreePointRenderer.h:154
mrpt::opengl::COctreePointRenderer::TNode::all
bool all
true: All elements in the reference object; false: only those in pts
Definition: COctreePointRenderer.h:164
mrpt::opengl::COctreePointRenderer::TRenderQueueElement::render_area_sqpixels
float render_area_sqpixels
The approximate size of the octree on the screen (squared pixels).
Definition: COctreePointRenderer.h:279
mrpt::opengl::gl_utils::TRenderInfo
Information about the rendering process being issued.
Definition: gl_utils.h:35
mrpt::opengl::gl_utils::TRenderInfo::vp_width
int vp_width
Definition: gl_utils.h:37
mrpt::opengl::gl_utils::TRenderInfo::vp_height
int vp_height
Definition: gl_utils.h:37
mrpt::opengl::gl_utils::TRenderInfo::projectPointPixels
void projectPointPixels(float x, float y, float z, float &proj_x_px, float &proj_y_px, float &proj_z_depth) const
Exactly like projectPoint but the (x,y) projected coordinates are given in pixels instead of normaliz...
Definition: gl_utils.h:67
mrpt::utils::TColorf
A RGB color - floats in the range [0,1].
Definition: TColor.h:79
mrpt::utils::TPixelCoordf
A pair (x,y) of pixel coordinates (subpixel resolution).
Definition: TPixelCoord.h:22


Page generated by Doxygen 1.9.1 for MRPT 1.9.9 Git: 63ea9d1f1 Thu Nov 23 00:06:53 2017 +0100 at mar 26 may 2026 12:19:29 CEST