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



Page generated by Doxygen 1.8.14 for MRPT 2.0.4 Git: 5711e29ae Wed May 27 14:29:47 2020 +0200 at miƩ may 27 14:30:10 CEST 2020