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-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 #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/core/aligned_std_deque.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 } // namespace global_settings
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::img::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::keep_min(bb_min.x, p.x);
178  mrpt::keep_min(bb_min.y, p.y);
179  mrpt::keep_min(bb_min.z, p.z);
180  mrpt::keep_max(bb_max.x, p.x);
181  mrpt::keep_max(bb_max.y, p.y);
182  mrpt::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  mrpt::aligned_std_deque<TNode> 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::img::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::img::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::keep_min(px_min.x, cr_px[i].x);
325  mrpt::keep_min(px_min.y, cr_px[i].y);
326  mrpt::keep_max(px_max.x, cr_px[i].x);
327  mrpt::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  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  render_area_sqpixels = std::max(1.0f, render_area_sqpixels);
361 
362  // OK: Add to list of rendering-pending:
363  m_render_queue.push_back(
364  TRenderQueueElement(node_idx, render_area_sqpixels));
365  }
366  }
367  else
368  { // Render children nodes:
369  // If ALL my 8 corners are within the screen, tell our children that
370  // they
371  // won't need to compute anymore, since all of them and their
372  // children are visible as well:
373  bool children_are_all_visible_for_sure = true;
374 
375  if (!trust_me_youre_visible) // Trust my parent... otherwise:
376  {
377  for (int i = 0; i < 8; i++)
378  {
379  if (!(cr_px[i].x >= 0 && cr_px[i].y >= 0 &&
380  cr_px[i].x < ri.vp_width &&
381  cr_px[i].y < ri.vp_height))
382  {
383  children_are_all_visible_for_sure = false;
384  break;
385  }
386  }
387  }
388 
389  // If all children are visible, it's easy:
390  if (children_are_all_visible_for_sure)
391  {
392  mrpt::img::TPixelCoordf
393  child_cr_px[8]; // No need to initialize
394  float child_cr_z[8]; // No need to initialize
395 
396  // Approximate area of the children nodes:
397  const float approx_child_area =
398  trust_me_youre_visible
399  ? approx_area_sqpixels / 8.0f
400  : std::abs(px_min.x - px_max.x) *
401  std::abs(px_min.y - px_max.y) / 8.0f;
402 
403  for (int i = 0; i < 8; i++)
404  this->octree_recursive_render(
405  node.child_id[i], ri, child_cr_px, child_cr_z, true,
406  true, approx_child_area);
407  }
408  else
409  {
410 #ifdef __clang__
411 #pragma clang diagnostic push // clang complains about unused vars (becase it
412 // doesn't realize of the macros?)
413 #pragma clang diagnostic ignored "-Wunused-variable"
414 #endif
415 
416  // Precompute the 19 (3*9-8) intermediary points so children
417  // don't have to compute them several times:
418  const mrpt::math::TPoint3Df p_Xm_Ym_Zm(
419  node.bb_min.x, node.bb_min.y, node.bb_min.z); // 0
420  const mrpt::math::TPoint3Df p_X0_Ym_Zm(
421  node.center.x, node.bb_min.y, node.bb_min.z);
422  const mrpt::math::TPoint3Df p_Xp_Ym_Zm(
423  node.bb_max.x, node.bb_min.y, node.bb_min.z); // 1
424  const mrpt::math::TPoint3Df p_Xm_Y0_Zm(
425  node.bb_min.x, node.center.y, node.bb_min.z);
426  const mrpt::math::TPoint3Df p_X0_Y0_Zm(
427  node.center.x, node.center.y, node.bb_min.z);
428  const mrpt::math::TPoint3Df p_Xp_Y0_Zm(
429  node.bb_max.x, node.center.y, node.bb_min.z);
430  const mrpt::math::TPoint3Df p_Xm_Yp_Zm(
431  node.bb_min.x, node.bb_max.y, node.bb_min.z); // 2
432  const mrpt::math::TPoint3Df p_X0_Yp_Zm(
433  node.center.x, node.bb_max.y, node.bb_min.z);
434  const mrpt::math::TPoint3Df p_Xp_Yp_Zm(
435  node.bb_max.x, node.bb_max.y, node.bb_min.z); // 3
436 
437  const mrpt::math::TPoint3Df p_Xm_Ym_Z0(
438  node.bb_min.x, node.bb_min.y, node.center.z);
439  const mrpt::math::TPoint3Df p_X0_Ym_Z0(
440  node.center.x, node.bb_min.y, node.center.z);
441  const mrpt::math::TPoint3Df p_Xp_Ym_Z0(
442  node.bb_max.x, node.bb_min.y, node.center.z);
443  const mrpt::math::TPoint3Df p_Xm_Y0_Z0(
444  node.bb_min.x, node.center.y, node.center.z);
445  const mrpt::math::TPoint3Df p_X0_Y0_Z0(
446  node.center.x, node.center.y, node.center.z);
447  const mrpt::math::TPoint3Df p_Xp_Y0_Z0(
448  node.bb_max.x, node.center.y, node.center.z);
449  const mrpt::math::TPoint3Df p_Xm_Yp_Z0(
450  node.bb_min.x, node.bb_max.y, node.center.z);
451  const mrpt::math::TPoint3Df p_X0_Yp_Z0(
452  node.center.x, node.bb_max.y, node.center.z);
453  const mrpt::math::TPoint3Df p_Xp_Yp_Z0(
454  node.bb_max.x, node.bb_max.y, node.center.z);
455 
456  const mrpt::math::TPoint3Df p_Xm_Ym_Zp(
457  node.bb_min.x, node.bb_min.y, node.bb_max.z); // 4
458  const mrpt::math::TPoint3Df p_X0_Ym_Zp(
459  node.center.x, node.bb_min.y, node.bb_max.z);
460  const mrpt::math::TPoint3Df p_Xp_Ym_Zp(
461  node.bb_min.x, node.bb_min.y, node.bb_max.z); // 5
462  const mrpt::math::TPoint3Df p_Xm_Y0_Zp(
463  node.bb_min.x, node.center.y, node.bb_max.z);
464  const mrpt::math::TPoint3Df p_X0_Y0_Zp(
465  node.center.x, node.center.y, node.bb_max.z);
466  const mrpt::math::TPoint3Df p_Xp_Y0_Zp(
467  node.bb_max.x, node.center.y, node.bb_max.z);
468  const mrpt::math::TPoint3Df p_Xm_Yp_Zp(
469  node.bb_min.x, node.bb_max.y, node.bb_max.z); // 6
470  const mrpt::math::TPoint3Df p_X0_Yp_Zp(
471  node.center.x, node.bb_max.y, node.bb_max.z);
472  const mrpt::math::TPoint3Df p_Xp_Yp_Zp(
473  node.bb_max.x, node.bb_max.y, node.bb_max.z); // 7
474 
475 // Project all these points:
476 #define PROJ_SUB_NODE(POSTFIX) \
477  mrpt::img::TPixelCoordf px_##POSTFIX; \
478  float depth_##POSTFIX; \
479  ri.projectPointPixels( \
480  p_##POSTFIX.x, p_##POSTFIX.y, p_##POSTFIX.z, px_##POSTFIX.x, \
481  px_##POSTFIX.y, depth_##POSTFIX);
482 
483 #define PROJ_SUB_NODE_ALREADY_DONE(INDEX, POSTFIX) \
484  const mrpt::img::TPixelCoordf px_##POSTFIX = cr_px[INDEX]; \
485  float depth_##POSTFIX = cr_z[INDEX];
486 
487  PROJ_SUB_NODE_ALREADY_DONE(0, Xm_Ym_Zm)
488  PROJ_SUB_NODE(X0_Ym_Zm)
489  PROJ_SUB_NODE_ALREADY_DONE(1, Xp_Ym_Zm)
490 
491  PROJ_SUB_NODE(Xm_Y0_Zm)
492  PROJ_SUB_NODE(X0_Y0_Zm)
493  PROJ_SUB_NODE(Xp_Y0_Zm)
494 
495  PROJ_SUB_NODE_ALREADY_DONE(2, Xm_Yp_Zm)
496  PROJ_SUB_NODE(X0_Yp_Zm)
497  PROJ_SUB_NODE_ALREADY_DONE(3, Xp_Yp_Zm)
498 
499  PROJ_SUB_NODE(Xm_Ym_Z0)
500  PROJ_SUB_NODE(X0_Ym_Z0)
501  PROJ_SUB_NODE(Xp_Ym_Z0)
502  PROJ_SUB_NODE(Xm_Y0_Z0)
503  PROJ_SUB_NODE(X0_Y0_Z0)
504  PROJ_SUB_NODE(Xp_Y0_Z0)
505  PROJ_SUB_NODE(Xm_Yp_Z0)
506  PROJ_SUB_NODE(X0_Yp_Z0)
507  PROJ_SUB_NODE(Xp_Yp_Z0)
508 
509  PROJ_SUB_NODE_ALREADY_DONE(4, Xm_Ym_Zp)
510  PROJ_SUB_NODE(X0_Ym_Zp)
511  PROJ_SUB_NODE_ALREADY_DONE(5, Xp_Ym_Zp)
512 
513  PROJ_SUB_NODE(Xm_Y0_Zp)
514  PROJ_SUB_NODE(X0_Y0_Zp)
515  PROJ_SUB_NODE(Xp_Y0_Zp)
516 
517  PROJ_SUB_NODE_ALREADY_DONE(6, Xm_Yp_Zp)
518  PROJ_SUB_NODE(X0_Yp_Zp)
519  PROJ_SUB_NODE_ALREADY_DONE(7, Xp_Yp_Zp)
520 
521 // Recursive call children nodes:
522 #define DO_RECURSE_CHILD( \
523  INDEX, SEQ0, SEQ1, SEQ2, SEQ3, SEQ4, SEQ5, SEQ6, SEQ7) \
524  { \
525  mrpt::img::TPixelCoordf child_cr_px[8] = { \
526  px_##SEQ0, px_##SEQ1, px_##SEQ2, px_##SEQ3, \
527  px_##SEQ4, px_##SEQ5, px_##SEQ6, px_##SEQ7}; \
528  float child_cr_z[8] = {depth_##SEQ0, depth_##SEQ1, depth_##SEQ2, \
529  depth_##SEQ3, depth_##SEQ4, depth_##SEQ5, \
530  depth_##SEQ6, depth_##SEQ7}; \
531  this->octree_recursive_render( \
532  node.child_id[INDEX], ri, child_cr_px, child_cr_z); \
533  }
534 
535  // 0 1 2 3
536  // 4 5 6 7
537  DO_RECURSE_CHILD(
538  0, Xm_Ym_Zm, X0_Ym_Zm, Xm_Y0_Zm, X0_Y0_Zm, Xm_Ym_Z0,
539  X0_Ym_Z0, Xm_Y0_Z0, X0_Y0_Z0)
540  DO_RECURSE_CHILD(
541  1, X0_Ym_Zm, Xp_Ym_Zm, X0_Y0_Zm, Xp_Y0_Zm, X0_Ym_Z0,
542  Xp_Ym_Z0, X0_Y0_Z0, Xp_Y0_Z0)
543  DO_RECURSE_CHILD(
544  2, Xm_Y0_Zm, X0_Y0_Zm, Xm_Yp_Zm, X0_Yp_Zm, Xm_Y0_Z0,
545  X0_Y0_Z0, Xm_Yp_Z0, X0_Yp_Z0)
546  DO_RECURSE_CHILD(
547  3, X0_Y0_Zm, Xp_Y0_Zm, X0_Yp_Zm, Xp_Yp_Zm, X0_Y0_Z0,
548  Xp_Y0_Z0, X0_Yp_Z0, Xp_Yp_Z0)
549  DO_RECURSE_CHILD(
550  4, Xm_Ym_Z0, X0_Ym_Z0, Xm_Y0_Z0, X0_Y0_Z0, Xm_Ym_Zp,
551  X0_Ym_Zp, Xm_Y0_Zp, X0_Y0_Zp)
552  DO_RECURSE_CHILD(
553  5, X0_Ym_Z0, Xp_Ym_Z0, X0_Y0_Z0, Xp_Y0_Z0, X0_Ym_Zp,
554  Xp_Ym_Zp, X0_Y0_Zp, Xp_Y0_Zp)
555  DO_RECURSE_CHILD(
556  6, Xm_Y0_Z0, X0_Y0_Z0, Xm_Yp_Z0, X0_Yp_Z0, Xm_Y0_Zp,
557  X0_Y0_Zp, Xm_Yp_Zp, X0_Yp_Zp)
558  DO_RECURSE_CHILD(
559  7, X0_Y0_Z0, Xp_Y0_Z0, X0_Yp_Z0, Xp_Yp_Z0, X0_Y0_Zp,
560  Xp_Y0_Zp, X0_Yp_Zp, Xp_Yp_Zp)
561 #undef DO_RECURSE_CHILD
562 #undef PROJ_SUB_NODE
563 #undef PROJ_SUB_NODE_ALREADY_DONE
564 
565 #ifdef __clang__
566 #pragma clang diagnostic pop
567 #endif
568  } // end "children_are_all_visible_for_sure"=false
569  }
570  }
571 
572  // The actual implementation (and non-const version) of
573  // octree_assure_uptodate()
574  void internal_octree_assure_uptodate()
575  {
576  if (!m_octree_has_to_rebuild_all) return;
577  m_octree_has_to_rebuild_all = false;
578 
579  // Reset list of nodes:
580  m_octree_nodes.assign(1, TNode());
581 
582  // recursive decide:
583  internal_recursive_split(OCTREE_ROOT_NODE, true);
584  }
585 
586  // Check the node "node_id" and create its children if needed, by looking at
587  // its list
588  // of elements (or all derived object's elements if "all_pts"=true, which
589  // will only happen
590  // for the root node)
591  void internal_recursive_split(
592  const size_t node_id, const bool all_pts = false)
593  {
594  TNode& node = m_octree_nodes[node_id];
595  const size_t N = all_pts ? octree_derived().size() : node.pts.size();
596 
597  const bool has_to_compute_bb = (node_id == OCTREE_ROOT_NODE);
598 
599  if (N <= mrpt::global_settings::OCTREE_RENDER_MAX_POINTS_PER_NODE())
600  {
601  // No need to split this node:
602  node.is_leaf = true;
603  node.all = all_pts;
604 
605  // Update bounding-box:
606  if (has_to_compute_bb)
607  {
608  if (all_pts)
609  for (size_t i = 0; i < N; i++)
610  node.update_bb(octree_derived().getPointf(i));
611  else
612  for (size_t i = 0; i < N; i++)
613  node.update_bb(octree_derived().getPointf(node.pts[i]));
614  }
615  }
616  else
617  {
618  // We have to split the node.
619  // Compute the mean of all elements:
620  mrpt::math::TPoint3Df mean(0, 0, 0);
621  if (all_pts)
622  for (size_t i = 0; i < N; i++)
623  {
624  mrpt::math::TPoint3Df p = octree_derived().getPointf(i);
625  mean += p;
626  if (has_to_compute_bb) node.update_bb(p);
627  }
628  else
629  for (size_t i = 0; i < N; i++)
630  {
631  mrpt::math::TPoint3Df p =
632  octree_derived().getPointf(node.pts[i]);
633  mean += p;
634  if (has_to_compute_bb) node.update_bb(p);
635  }
636 
637  // Save my split point:
638  node.is_leaf = false;
639  node.center = mean * (1.0f / N);
640 
641  // Allocate my 8 children structs
642  const size_t children_idx_base = m_octree_nodes.size();
643  m_octree_nodes.resize(children_idx_base + 8);
644  for (int i = 0; i < 8; i++)
645  node.child_id[i] = children_idx_base + i;
646 
647  // Set the bounding-boxes of my children (we already know them):
648  for (int i = 0; i < 8; i++)
649  m_octree_nodes[children_idx_base + i].setBBFromOrderInParent(
650  node, i);
651 
652  // Divide elements among children:
653  const mrpt::math::TPoint3Df& c =
654  node.center; // to make notation clearer
655  for (size_t j = 0; j < N; j++)
656  {
657  const size_t i = all_pts ? j : node.pts[j];
658  const mrpt::math::TPoint3Df p = octree_derived().getPointf(i);
659  if (p.z < c.z)
660  {
661  if (p.y < c.y)
662  {
663  if (p.x < c.x)
664  m_octree_nodes[children_idx_base + 0].pts.push_back(
665  i);
666  else
667  m_octree_nodes[children_idx_base + 1].pts.push_back(
668  i);
669  }
670  else
671  {
672  if (p.x < c.x)
673  m_octree_nodes[children_idx_base + 2].pts.push_back(
674  i);
675  else
676  m_octree_nodes[children_idx_base + 3].pts.push_back(
677  i);
678  }
679  }
680  else
681  {
682  if (p.y < c.y)
683  {
684  if (p.x < c.x)
685  m_octree_nodes[children_idx_base + 4].pts.push_back(
686  i);
687  else
688  m_octree_nodes[children_idx_base + 5].pts.push_back(
689  i);
690  }
691  else
692  {
693  if (p.x < c.x)
694  m_octree_nodes[children_idx_base + 6].pts.push_back(
695  i);
696  else
697  m_octree_nodes[children_idx_base + 7].pts.push_back(
698  i);
699  }
700  }
701  }
702 
703  // Clear list of elements (they're now in our children):
704  {
705  std::vector<size_t> emptyVec;
706  node.pts.swap(emptyVec); // This is THE way of really clearing
707  // a std::vector
708  }
709 
710  // Recursive call on children:
711  for (int i = 0; i < 8; i++)
712  internal_recursive_split(node.child_id[i]);
713  }
714  } // end of internal_recursive_split
715 
716  public:
717  /** Return the number of octree nodes (all of them, including the empty
718  * ones) \sa octree_get_nonempty_node_count */
719  size_t octree_get_node_count() const { return m_octree_nodes.size(); }
720  /** Return the number of visible octree nodes in the last render event. */
721  size_t octree_get_visible_nodes() const { return m_visible_octree_nodes; }
722  /** Called from the derived class (or the user) to indicate we have/want to
723  * rebuild the entire node tree (for example, after modifying the point
724  * cloud or any global octree parameter) */
725  inline void octree_mark_as_outdated()
726  {
727  m_octree_has_to_rebuild_all = true;
728  }
729 
730  /** Returns a graphical representation of all the bounding boxes of the
731  * octree (leaf) nodes.
732  * \param[in] draw_solid_boxes If false, will draw solid boxes of color \a
733  * lines_color. Otherwise, wireframe boxes will be drawn.
734  */
735  void octree_get_graphics_boundingboxes(
736  mrpt::opengl::CSetOfObjects& gl_bb, const double lines_width = 1,
737  const mrpt::img::TColorf& lines_color = mrpt::img::TColorf(1, 1, 1),
738  const bool draw_solid_boxes = false) const
739  {
740  octree_assure_uptodate();
741  gl_bb.clear();
742  for (size_t i = 0; i < m_octree_nodes.size(); i++)
743  {
744  const TNode& node = m_octree_nodes[i];
745  if (!node.is_leaf) continue;
746  mrpt::opengl::CBox::Ptr gl_box =
747  mrpt::make_aligned_shared<mrpt::opengl::CBox>();
748  gl_box->setBoxCorners(
749  mrpt::math::TPoint3D(node.bb_min),
750  mrpt::math::TPoint3D(node.bb_max));
751  gl_box->setColor(lines_color);
752  gl_box->setLineWidth(lines_width);
753  gl_box->setWireframe(!draw_solid_boxes);
754  gl_bb.insert(gl_box);
755  }
756  }
757 
758  /** Used for debug only */
759  void octree_debug_dump_tree(std::ostream& o) const
760  {
761  o << "Octree nodes: " << m_octree_nodes.size() << std::endl;
762  size_t total_elements = 0;
763  for (size_t i = 0; i < m_octree_nodes.size(); i++)
764  {
765  const TNode& node = m_octree_nodes[i];
766 
767  o << "Node #" << i << ": ";
768  if (node.is_leaf)
769  {
770  o << "leaf, ";
771  if (node.all)
772  {
773  o << "(all)\n";
774  total_elements += octree_derived().size();
775  }
776  else
777  {
778  o << node.pts.size() << " elements; ";
779  total_elements += node.pts.size();
780  }
781  }
782  else
783  {
784  o << "parent, center=(" << node.center.x << "," << node.center.y
785  << "," << node.center.z << "), children: " << node.child_id[0]
786  << "," << node.child_id[1] << "," << node.child_id[2] << ","
787  << node.child_id[3] << "," << node.child_id[4] << ","
788  << node.child_id[5] << "," << node.child_id[6] << ","
789  << node.child_id[7] << "; ";
790  }
791  o << " bb: (" << node.bb_min.x << "," << node.bb_min.y << ","
792  << node.bb_min.z << ")-(" << node.bb_max.x << "," << node.bb_max.y
793  << "," << node.bb_max.z << ")\n";
794  }
795  o << "Total elements in all nodes: " << total_elements << std::endl;
796  } // end of octree_debug_dump_tree
797 
798 }; // end of class COctreePointRenderer
799 
800 } // namespace opengl
801 } // namespace mrpt
802 #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)
MRPT_MAKE_ALIGNED_OPERATOR_NEW
#define MRPT_MAKE_ALIGNED_OPERATOR_NEW
Put this macro inside any class with members that require {16,32,64}-byte memory alignment (e....
Definition: aligned_allocator.h:90
aligned_std_deque.h
mrpt::opengl::CBox::Ptr
std::shared_ptr< CBox > Ptr
Definition: CBox.h:42
mrpt::opengl::COctreePointRenderer
Template class that implements the data structure and algorithms for Octree-based efficient rendering...
Definition: COctreePointRenderer.h:52
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:725
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::m_octree_nodes
mrpt::aligned_std_deque< TNode > m_octree_nodes
First one [0] is always the root node.
Definition: COctreePointRenderer.h:286
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_recursive_render
void octree_recursive_render(size_t node_idx, const mrpt::opengl::gl_utils::TRenderInfo &ri, mrpt::img::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::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:721
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::octree_get_graphics_boundingboxes
void octree_get_graphics_boundingboxes(mrpt::opengl::CSetOfObjects &gl_bb, const double lines_width=1, const mrpt::img::TColorf &lines_color=mrpt::img::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:735
mrpt::opengl::COctreePointRenderer::internal_recursive_split
void internal_recursive_split(const size_t node_id, const bool all_pts=false)
Definition: COctreePointRenderer.h:591
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:719
mrpt::opengl::COctreePointRenderer::octree_debug_dump_tree
void octree_debug_dump_tree(std::ostream &o) const
Used for debug only.
Definition: COctreePointRenderer.h:759
mrpt::opengl::CSetOfObjects
A set of object, which are referenced to the coordinates framework established in this object.
Definition: CSetOfObjects.h:27
mrpt::opengl::CSetOfObjects::insert
void insert(const CRenderizable::Ptr &newObject)
Insert a new object to the list.
Definition: CSetOfObjects.cpp:107
mrpt::opengl::CSetOfObjects::clear
void clear()
Clear the list of objects in the scene, deleting objects' memory.
Definition: CSetOfObjects.cpp:35
mean
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
Definition: eigen_plugins.h:427
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:38
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: CKalmanFilterCapable.h:31
mrpt::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: core/include/mrpt/core/bits_math.h:124
mrpt::aligned_std_deque
std::deque< T, mrpt::aligned_allocator_cpp11< T > > aligned_std_deque
Definition: aligned_std_deque.h:15
mrpt::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: core/include/mrpt/core/bits_math.h:131
mrpt::img::TColorf
A RGB color - floats in the range [0,1].
Definition: TColor.h:78
mrpt::img::TPixelCoordf
A pair (x,y) of pixel coordinates (subpixel resolution).
Definition: TPixelCoord.h:19
mrpt::math::TPoint3D
Lightweight 3D point.
Definition: lightweight_geom_data.h:379
mrpt::math::TPoint3Df
Lightweight 3D point (float version).
Definition: lightweight_geom_data.h:316
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:31
mrpt::opengl::gl_utils::TRenderInfo::vp_width
int vp_width
Definition: gl_utils.h:33
mrpt::opengl::gl_utils::TRenderInfo::vp_height
int vp_height
Definition: gl_utils.h:33
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:63


Page generated by Doxygen 1.9.1 for MRPT 1.9.9 Git: 814d80880 Fri Aug 24 01:51:28 2018 +0200 at mar 26 may 2026 12:30:59 CEST