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



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 1de0e027c Sat Sep 14 16:15:22 2019 +0200 at sáb sep 14 16:20:14 CEST 2019