Main MRPT website > C++ reference for MRPT 1.5.3
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 
14 #include <mrpt/opengl/CBox.h>
15 #include <mrpt/opengl/gl_utils.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 for further details):
23  * - mrpt::opengl::CPointCloud
24  * - mrpt::opengl::CPointCloudColoured
25  * \ingroup mrpt_opengl_grp
26  */
28 
29  /** Default value = 1e5. Maximum number of elements in each octree node before spliting. Affects to these classes (read their docs for further details):
30  * - mrpt::opengl::CPointCloud
31  * - mrpt::opengl::CPointCloudColoured
32  * \ingroup mrpt_opengl_grp
33  */
35  }
36 
37 
38  namespace opengl
39  {
40  /** Template class that implements the data structure and algorithms for Octree-based efficient rendering.
41  * \sa mrpt::opengl::CPointCloud, mrpt::opengl::CPointCloudColoured, http://www.mrpt.org/Efficiently_rendering_point_clouds_of_millions_of_points
42  * \ingroup mrpt_opengl_grp
43  */
44  template <class Derived>
46  {
47  public:
48  /** Default ctor */
50  m_octree_has_to_rebuild_all(true),
51  m_visible_octree_nodes(0),
52  m_visible_octree_nodes_ongoing(0)
53  { }
54 
55  /** Copy ctor */
57  m_octree_has_to_rebuild_all(true),
58  m_visible_octree_nodes(0),
59  m_visible_octree_nodes_ongoing(0)
60  { }
61 
62 
63  enum { OCTREE_ROOT_NODE = 0 };
64 
65  protected:
66  // Helper methods in any CRTP template
67  inline Derived & octree_derived() { return *static_cast<Derived*>(this); }
68  inline const Derived & octree_derived() const { return *static_cast<const Derived*>(this); }
69 
70  /** Must be called at children class' render() previously to \a octree_render() */
71  inline void octree_assure_uptodate() const
72  {
73  const_cast<COctreePointRenderer<Derived>*>(this)->internal_octree_assure_uptodate();
74  }
75 
76  /** Render the entire octree recursively.
77  * Should be called from children's render() method.
78  */
80  {
81  m_visible_octree_nodes_ongoing = 0;
82 
83  // Stage 1: Build list of visible octrees
84  m_render_queue.clear();
85  m_render_queue.reserve(m_octree_nodes.size());
86 
88  float cr_z[8];
89  octree_recursive_render(OCTREE_ROOT_NODE,ri, cr_px, cr_z, false /* corners are not computed for this first iteration */ );
90 
91  m_visible_octree_nodes = m_visible_octree_nodes_ongoing;
92 
93  // Stage 2: Render them all
94  for (size_t i=0;i<m_render_queue.size();i++)
95  {
96  const TNode & node = m_octree_nodes[ m_render_queue[i].node_id ];
97  octree_derived().render_subset( node.all,node.pts,m_render_queue[i].render_area_sqpixels);
98  }
99  }
100 
101 
103  {
104  octree_assure_uptodate();
105  if (!m_octree_nodes.empty())
106  {
107  bb_min = mrpt::math::TPoint3D( m_octree_nodes[0].bb_min );
108  bb_max = mrpt::math::TPoint3D( m_octree_nodes[0].bb_max );
109  }
110  }
111 
112 
113  private:
114  /** The structure for each octree spatial node. Each node can either be a leaf of has 8 children nodes.
115  * Instead of pointers, children are referenced by their indices in \a m_octree_nodes
116  */
118  {
119  TNode() :
120  bb_min( std::numeric_limits<float>::max(), std::numeric_limits<float>::max(), std::numeric_limits<float>::max() ),
121  bb_max(-std::numeric_limits<float>::max(),-std::numeric_limits<float>::max(),-std::numeric_limits<float>::max() )
122  { }
123 
124  bool is_leaf; //!< true: it's a leaf and \a pts has valid indices; false: \a children is valid.
125 
126  // In all cases, the bounding_box:
128 
129  // Fields used if is_leaf=true
130  std::vector<size_t> pts; //!< Point indices in the derived class that fall into this node.
131  bool all; //!< true: All elements in the reference object; false: only those in \a pts
132 
133  // Fields used if is_leaf=false
134  mrpt::math::TPoint3Df center; //!< [is_leaf=false] The center of the node, whose coordinates are used to decide between the 8 children nodes.
135  size_t child_id[8]; //!< [is_leaf=false] The indices in \a m_octree_nodes of the 8 children.
136 
137  /** update bounding box with a new point: */
138  inline void update_bb(const mrpt::math::TPoint3Df &p)
139  {
140  mrpt::utils::keep_min(bb_min.x, p.x); mrpt::utils::keep_min(bb_min.y, p.y); mrpt::utils::keep_min(bb_min.z, p.z);
141  mrpt::utils::keep_max(bb_max.x, p.x); mrpt::utils::keep_max(bb_max.y, p.y); mrpt::utils::keep_max(bb_max.z, p.z);
142  }
143 
144  inline float getCornerX(int i) const { return (i & 0x01)==0 ? bb_min.x : bb_max.x; }
145  inline float getCornerY(int i) const { return (i & 0x02)==0 ? bb_min.y : bb_max.y; }
146  inline float getCornerZ(int i) const { return (i & 0x04)==0 ? bb_min.z : bb_max.z; }
147 
148  void setBBFromOrderInParent(const TNode &parent, int my_child_index)
149  {
150  // Coordinate signs are relative to the parent center (split point):
151  switch (my_child_index)
152  {
153  case 0: // x-, y-, z-
154  bb_min = parent.bb_min;
155  bb_max = parent.center;
156  break;
157  case 1: // x+, y-, z-
158  bb_min.x = parent.center.x; bb_max.x = parent.bb_max.x;
159  bb_min.y = parent.bb_min.y; bb_max.y = parent.center.y;
160  bb_min.z = parent.bb_min.z; bb_max.z = parent.center.z;
161  break;
162  case 2: // x-, y+, z-
163  bb_min.x = parent.bb_min.x; bb_max.x = parent.center.x;
164  bb_min.y = parent.center.y; bb_max.y = parent.bb_max.y;
165  bb_min.z = parent.bb_min.z; bb_max.z = parent.center.z;
166  break;
167  case 3: // x+, y+, z-
168  bb_min.x = parent.center.x; bb_max.x = parent.bb_max.x;
169  bb_min.y = parent.center.y; bb_max.y = parent.bb_max.y;
170  bb_min.z = parent.bb_min.z; bb_max.z = parent.center.z;
171  break;
172  case 4: // x-, y-, z+
173  bb_min.x = parent.bb_min.x; bb_max.x = parent.center.x;
174  bb_min.y = parent.bb_min.y; bb_max.y = parent.center.y;
175  bb_min.z = parent.center.z; bb_max.z = parent.bb_max.z;
176  break;
177  case 5: // x+, y-, z+
178  bb_min.x = parent.center.x; bb_max.x = parent.bb_max.x;
179  bb_min.y = parent.bb_min.y; bb_max.y = parent.center.y;
180  bb_min.z = parent.center.z; bb_max.z = parent.bb_max.z;
181  break;
182  case 6: // x-, y+, z+
183  bb_min.x = parent.bb_min.x; bb_max.x = parent.center.x;
184  bb_min.y = parent.center.y; bb_max.y = parent.bb_max.y;
185  bb_min.z = parent.center.z; bb_max.z = parent.bb_max.z;
186  break;
187  case 7: // x+, y+, z+
188  bb_min = parent.center;
189  bb_max = parent.bb_max;
190  break;
191  default: throw std::runtime_error("my_child_index!=[0,7]");
192  }
193  }
194 
195  public:
197  };
198 
200  {
201  inline TRenderQueueElement(const size_t id, float area_sq) : node_id(id), render_area_sqpixels(area_sq) { }
202 
203  size_t node_id; //!< The node ID to render
204  float render_area_sqpixels; //!< The approximate size of the octree on the screen (squared pixels).
205  };
206  mutable std::vector<TRenderQueueElement> m_render_queue; //!< The list of elements that really are visible and will be rendered.
207 
208 
210  typename mrpt::aligned_containers<TNode>::deque_t m_octree_nodes; //!< First one [0] is always the root node
211 
212  // Counters of visible octrees for each render:
213  volatile mutable size_t m_visible_octree_nodes, m_visible_octree_nodes_ongoing;
214 
215  /** Render a given node. */
217  size_t node_idx,
219  mrpt::utils::TPixelCoordf cr_px[8],
220  float cr_z[8],
221  bool corners_are_all_computed = true,
222  bool trust_me_youre_visible = false,
223  float approx_area_sqpixels = 0
224  ) const
225  {
226  const TNode &node = m_octree_nodes[node_idx];
227 
228  if (!corners_are_all_computed)
229  {
230  for (int i=0;i<8;i++)
231  {
232  // project point:
234  node.getCornerX(i),node.getCornerY(i),node.getCornerZ(i),
235  cr_px[i].x,cr_px[i].y,cr_z[i]);
236  }
237  }
238 
240  if (!trust_me_youre_visible)
241  {
242  // Keep the on-screen bounding box of this node:
243  for (int i=0;i<8;i++)
244  {
245  mrpt::utils::keep_min(px_min.x,cr_px[i].x); mrpt::utils::keep_min(px_min.y,cr_px[i].y);
246  mrpt::utils::keep_max(px_max.x,cr_px[i].x); mrpt::utils::keep_max(px_max.y,cr_px[i].y);
247  }
248 
249  const bool any_cr_zs_neg = (cr_z[0]<0 ||cr_z[1]<0 ||cr_z[2]<0 ||cr_z[3]<0 ||cr_z[4]<0 ||cr_z[5]<0 ||cr_z[6]<0 ||cr_z[7]<0);
250  const bool any_cr_zs_pos = (cr_z[0]>0 ||cr_z[1]>0 ||cr_z[2]>0 ||cr_z[3]>0 ||cr_z[4]>0 ||cr_z[5]>0 ||cr_z[6]>0 ||cr_z[7]>0);
251  const bool box_crosses_image_plane = any_cr_zs_pos && any_cr_zs_neg;
252 
253  // If all 8 corners are way out of the screen (and all "cr_z" have the same sign),
254  // this node and all the children are not visible:
255  if (!box_crosses_image_plane && ( px_min.x>=ri.vp_width || px_min.y>=ri.vp_height || px_max.x<0 || px_max.y<0) )
256  return; // Not visible
257  }
258 
259  // Check if the node has points and is visible:
260  if (node.is_leaf)
261  { // Render this leaf node:
262  if (node.all || !node.pts.empty())
263  {
264  // If we are here, it seems at least a part of the Box is visible:
265  m_visible_octree_nodes_ongoing++;
266 
267  const float render_area_sqpixels = trust_me_youre_visible ?
268  approx_area_sqpixels
269  :
270  std::abs(px_min.x-px_max.x) * std::abs(px_min.y-px_max.y);
271 
272  // OK: Add to list of rendering-pending:
273  m_render_queue.push_back( TRenderQueueElement(node_idx,render_area_sqpixels) );
274  }
275  }
276  else
277  { // Render children nodes:
278  // If ALL my 8 corners are within the screen, tell our children that they
279  // won't need to compute anymore, since all of them and their children are visible as well:
280  bool children_are_all_visible_for_sure = true;
281 
282  if (!trust_me_youre_visible) // Trust my parent... otherwise:
283  {
284  for (int i=0;i<8;i++)
285  {
286  if (!( cr_px[i].x>=0 && cr_px[i].y>=0 && cr_px[i].x<ri.vp_width && cr_px[i].y<ri.vp_height ))
287  {
288  children_are_all_visible_for_sure = false;
289  break;
290  }
291  }
292  }
293 
294  // If all children are visible, it's easy:
295  if (children_are_all_visible_for_sure)
296  {
297  mrpt::utils::TPixelCoordf child_cr_px[8]; // No need to initialize
298  float child_cr_z[8]; // No need to initialize
299 
300  // Approximate area of the children nodes:
301  const float approx_child_area = trust_me_youre_visible ?
302  approx_area_sqpixels/8.0f
303  :
304  std::abs(px_min.x-px_max.x) * std::abs(px_min.y-px_max.y) / 8.0f;
305 
306  for (int i=0;i<8;i++)
307  this->octree_recursive_render(node.child_id[i],ri,child_cr_px, child_cr_z, true, true, approx_child_area); \
308  }
309  else
310  {
311 #ifdef __clang__
312 #pragma clang diagnostic push // clang complains about unused vars (becase it doesn't realize of the macros?)
313 #pragma clang diagnostic ignored "-Wunused-variable"
314 #endif
315 
316  // Precompute the 19 (3*9-8) intermediary points so children don't have to compute them several times:
317  const mrpt::math::TPoint3Df p_Xm_Ym_Zm ( node.bb_min.x, node.bb_min.y, node.bb_min.z ); // 0
318  const mrpt::math::TPoint3Df p_X0_Ym_Zm ( node.center.x, node.bb_min.y, node.bb_min.z );
319  const mrpt::math::TPoint3Df p_Xp_Ym_Zm ( node.bb_max.x, node.bb_min.y, node.bb_min.z ); // 1
320  const mrpt::math::TPoint3Df p_Xm_Y0_Zm ( node.bb_min.x, node.center.y, node.bb_min.z );
321  const mrpt::math::TPoint3Df p_X0_Y0_Zm ( node.center.x, node.center.y, node.bb_min.z );
322  const mrpt::math::TPoint3Df p_Xp_Y0_Zm ( node.bb_max.x, node.center.y, node.bb_min.z );
323  const mrpt::math::TPoint3Df p_Xm_Yp_Zm ( node.bb_min.x, node.bb_max.y, node.bb_min.z ); // 2
324  const mrpt::math::TPoint3Df p_X0_Yp_Zm ( node.center.x, node.bb_max.y, node.bb_min.z );
325  const mrpt::math::TPoint3Df p_Xp_Yp_Zm ( node.bb_max.x, node.bb_max.y, node.bb_min.z ); // 3
326 
327  const mrpt::math::TPoint3Df p_Xm_Ym_Z0 ( node.bb_min.x, node.bb_min.y, node.center.z );
328  const mrpt::math::TPoint3Df p_X0_Ym_Z0 ( node.center.x, node.bb_min.y, node.center.z );
329  const mrpt::math::TPoint3Df p_Xp_Ym_Z0 ( node.bb_max.x, node.bb_min.y, node.center.z );
330  const mrpt::math::TPoint3Df p_Xm_Y0_Z0 ( node.bb_min.x, node.center.y, node.center.z );
331  const mrpt::math::TPoint3Df p_X0_Y0_Z0 ( node.center.x, node.center.y, node.center.z );
332  const mrpt::math::TPoint3Df p_Xp_Y0_Z0 ( node.bb_max.x, node.center.y, node.center.z );
333  const mrpt::math::TPoint3Df p_Xm_Yp_Z0 ( node.bb_min.x, node.bb_max.y, node.center.z );
334  const mrpt::math::TPoint3Df p_X0_Yp_Z0 ( node.center.x, node.bb_max.y, node.center.z );
335  const mrpt::math::TPoint3Df p_Xp_Yp_Z0 ( node.bb_max.x, node.bb_max.y, node.center.z );
336 
337  const mrpt::math::TPoint3Df p_Xm_Ym_Zp ( node.bb_min.x, node.bb_min.y, node.bb_max.z ); // 4
338  const mrpt::math::TPoint3Df p_X0_Ym_Zp ( node.center.x, node.bb_min.y, node.bb_max.z );
339  const mrpt::math::TPoint3Df p_Xp_Ym_Zp ( node.bb_min.x, node.bb_min.y, node.bb_max.z ); // 5
340  const mrpt::math::TPoint3Df p_Xm_Y0_Zp ( node.bb_min.x, node.center.y, node.bb_max.z );
341  const mrpt::math::TPoint3Df p_X0_Y0_Zp ( node.center.x, node.center.y, node.bb_max.z );
342  const mrpt::math::TPoint3Df p_Xp_Y0_Zp ( node.bb_max.x, node.center.y, node.bb_max.z );
343  const mrpt::math::TPoint3Df p_Xm_Yp_Zp ( node.bb_min.x, node.bb_max.y, node.bb_max.z ); // 6
344  const mrpt::math::TPoint3Df p_X0_Yp_Zp ( node.center.x, node.bb_max.y, node.bb_max.z );
345  const mrpt::math::TPoint3Df p_Xp_Yp_Zp ( node.bb_max.x, node.bb_max.y, node.bb_max.z ); // 7
346 
347  // Project all these points:
348 #define PROJ_SUB_NODE(POSTFIX) \
349  mrpt::utils::TPixelCoordf px_##POSTFIX; \
350  float depth_##POSTFIX; \
351  ri.projectPointPixels( p_##POSTFIX.x, p_##POSTFIX.y, p_##POSTFIX.z, px_##POSTFIX.x,px_##POSTFIX.y,depth_##POSTFIX);
352 
353 #define PROJ_SUB_NODE_ALREADY_DONE(INDEX, POSTFIX) \
354  const mrpt::utils::TPixelCoordf px_##POSTFIX = cr_px[INDEX]; \
355  float depth_##POSTFIX = cr_z[INDEX];
356 
357  PROJ_SUB_NODE_ALREADY_DONE(0,Xm_Ym_Zm)
358  PROJ_SUB_NODE(X0_Ym_Zm)
359  PROJ_SUB_NODE_ALREADY_DONE(1, Xp_Ym_Zm)
360 
361  PROJ_SUB_NODE(Xm_Y0_Zm)
362  PROJ_SUB_NODE(X0_Y0_Zm)
363  PROJ_SUB_NODE(Xp_Y0_Zm)
364 
365  PROJ_SUB_NODE_ALREADY_DONE(2, Xm_Yp_Zm)
366  PROJ_SUB_NODE(X0_Yp_Zm)
367  PROJ_SUB_NODE_ALREADY_DONE(3, Xp_Yp_Zm)
368 
369  PROJ_SUB_NODE(Xm_Ym_Z0)
370  PROJ_SUB_NODE(X0_Ym_Z0)
371  PROJ_SUB_NODE(Xp_Ym_Z0)
372  PROJ_SUB_NODE(Xm_Y0_Z0)
373  PROJ_SUB_NODE(X0_Y0_Z0)
374  PROJ_SUB_NODE(Xp_Y0_Z0)
375  PROJ_SUB_NODE(Xm_Yp_Z0)
376  PROJ_SUB_NODE(X0_Yp_Z0)
377  PROJ_SUB_NODE(Xp_Yp_Z0)
378 
379  PROJ_SUB_NODE_ALREADY_DONE(4, Xm_Ym_Zp)
380  PROJ_SUB_NODE(X0_Ym_Zp)
381  PROJ_SUB_NODE_ALREADY_DONE(5, Xp_Ym_Zp)
382 
383  PROJ_SUB_NODE(Xm_Y0_Zp)
384  PROJ_SUB_NODE(X0_Y0_Zp)
385  PROJ_SUB_NODE(Xp_Y0_Zp)
386 
387  PROJ_SUB_NODE_ALREADY_DONE(6, Xm_Yp_Zp)
388  PROJ_SUB_NODE(X0_Yp_Zp)
389  PROJ_SUB_NODE_ALREADY_DONE(7, Xp_Yp_Zp)
390 
391  // Recursive call children nodes:
392 #define DO_RECURSE_CHILD(INDEX, SEQ0,SEQ1,SEQ2,SEQ3,SEQ4,SEQ5,SEQ6,SEQ7) \
393  { \
394  mrpt::utils::TPixelCoordf child_cr_px[8] = { px_##SEQ0,px_##SEQ1,px_##SEQ2,px_##SEQ3,px_##SEQ4,px_##SEQ5,px_##SEQ6,px_##SEQ7 }; \
395  float child_cr_z[8] = { depth_##SEQ0,depth_##SEQ1,depth_##SEQ2,depth_##SEQ3,depth_##SEQ4,depth_##SEQ5,depth_##SEQ6,depth_##SEQ7 }; \
396  this->octree_recursive_render(node.child_id[INDEX],ri,child_cr_px, child_cr_z); \
397  }
398 
399  // 0 1 2 3 4 5 6 7
400  DO_RECURSE_CHILD(0, Xm_Ym_Zm, X0_Ym_Zm, Xm_Y0_Zm, X0_Y0_Zm, Xm_Ym_Z0, X0_Ym_Z0, Xm_Y0_Z0, X0_Y0_Z0 )
401  DO_RECURSE_CHILD(1, X0_Ym_Zm, Xp_Ym_Zm, X0_Y0_Zm, Xp_Y0_Zm, X0_Ym_Z0, Xp_Ym_Z0, X0_Y0_Z0, Xp_Y0_Z0 )
402  DO_RECURSE_CHILD(2, Xm_Y0_Zm, X0_Y0_Zm, Xm_Yp_Zm, X0_Yp_Zm, Xm_Y0_Z0, X0_Y0_Z0, Xm_Yp_Z0, X0_Yp_Z0 )
403  DO_RECURSE_CHILD(3, X0_Y0_Zm, Xp_Y0_Zm, X0_Yp_Zm, Xp_Yp_Zm, X0_Y0_Z0, Xp_Y0_Z0, X0_Yp_Z0, Xp_Yp_Z0 )
404  DO_RECURSE_CHILD(4, Xm_Ym_Z0, X0_Ym_Z0, Xm_Y0_Z0, X0_Y0_Z0, Xm_Ym_Zp, X0_Ym_Zp, Xm_Y0_Zp, X0_Y0_Zp )
405  DO_RECURSE_CHILD(5, X0_Ym_Z0, Xp_Ym_Z0, X0_Y0_Z0, Xp_Y0_Z0, X0_Ym_Zp, Xp_Ym_Zp, X0_Y0_Zp, Xp_Y0_Zp )
406  DO_RECURSE_CHILD(6, Xm_Y0_Z0, X0_Y0_Z0, Xm_Yp_Z0, X0_Yp_Z0, Xm_Y0_Zp, X0_Y0_Zp, Xm_Yp_Zp, X0_Yp_Zp )
407  DO_RECURSE_CHILD(7, X0_Y0_Z0, Xp_Y0_Z0, X0_Yp_Z0, Xp_Yp_Z0, X0_Y0_Zp, Xp_Y0_Zp, X0_Yp_Zp, Xp_Yp_Zp )
408 #undef DO_RECURSE_CHILD
409 #undef PROJ_SUB_NODE
410 #undef PROJ_SUB_NODE_ALREADY_DONE
411 
412 #ifdef __clang__
413 #pragma clang diagnostic pop
414 #endif
415  } // end "children_are_all_visible_for_sure"=false
416  }
417  }
418 
419  // The actual implementation (and non-const version) of octree_assure_uptodate()
421  {
422  if (!m_octree_has_to_rebuild_all) return;
423  m_octree_has_to_rebuild_all = false;
424 
425  // Reset list of nodes:
426  m_octree_nodes.assign(1, TNode() );
427 
428  // recursive decide:
429  internal_recursive_split( OCTREE_ROOT_NODE, true );
430  }
431 
432  // Check the node "node_id" and create its children if needed, by looking at its list
433  // of elements (or all derived object's elements if "all_pts"=true, which will only happen
434  // for the root node)
435  void internal_recursive_split(const size_t node_id, const bool all_pts = false)
436  {
437  TNode &node = m_octree_nodes[node_id];
438  const size_t N = all_pts ? octree_derived().size() : node.pts.size();
439 
440  const bool has_to_compute_bb = (node_id ==OCTREE_ROOT_NODE);
441 
443  {
444  // No need to split this node:
445  node.is_leaf = true;
446  node.all = all_pts;
447 
448  // Update bounding-box:
449  if (has_to_compute_bb)
450  {
451  if (all_pts)
452  for (size_t i=0;i<N;i++) node.update_bb( octree_derived().getPointf(i) );
453  else for (size_t i=0;i<N;i++) node.update_bb( octree_derived().getPointf(node.pts[i]) );
454  }
455  }
456  else
457  {
458  // We have to split the node.
459  // Compute the mean of all elements:
461  if (all_pts)
462  for (size_t i=0;i<N;i++)
463  {
464  mrpt::math::TPoint3Df p = octree_derived().getPointf(i);
465  mean+= p;
466  if (has_to_compute_bb) node.update_bb( p );
467  }
468  else
469  for (size_t i=0;i<N;i++)
470  {
471  mrpt::math::TPoint3Df p = octree_derived().getPointf(node.pts[i]);
472  mean+= p;
473  if (has_to_compute_bb) node.update_bb( p );
474  }
475 
476  // Save my split point:
477  node.is_leaf = false;
478  node.center = mean * (1.0f/N);
479 
480  // Allocate my 8 children structs
481  const size_t children_idx_base = m_octree_nodes.size();
482  m_octree_nodes.resize(children_idx_base + 8 );
483  for (int i=0;i<8;i++)
484  node.child_id[i] = children_idx_base + i;
485 
486  // Set the bounding-boxes of my children (we already know them):
487  for (int i=0;i<8;i++)
488  m_octree_nodes[children_idx_base + i].setBBFromOrderInParent(node,i);
489 
490  // Divide elements among children:
491  const mrpt::math::TPoint3Df &c = node.center; // to make notation clearer
492  for (size_t j=0;j<N;j++)
493  {
494  const size_t i = all_pts ? j : node.pts[j];
495  const mrpt::math::TPoint3Df p = octree_derived().getPointf(i);
496  if (p.z<c.z)
497  {
498  if (p.y<c.y)
499  {
500  if (p.x<c.x)
501  m_octree_nodes[children_idx_base+ 0 ].pts.push_back(i);
502  else m_octree_nodes[children_idx_base+ 1 ].pts.push_back(i);
503  }
504  else
505  {
506  if (p.x<c.x)
507  m_octree_nodes[children_idx_base+ 2 ].pts.push_back(i);
508  else m_octree_nodes[children_idx_base+ 3 ].pts.push_back(i);
509  }
510  }
511  else
512  {
513  if (p.y<c.y)
514  {
515  if (p.x<c.x)
516  m_octree_nodes[children_idx_base+ 4 ].pts.push_back(i);
517  else m_octree_nodes[children_idx_base+ 5 ].pts.push_back(i);
518  }
519  else
520  {
521  if (p.x<c.x)
522  m_octree_nodes[children_idx_base+ 6 ].pts.push_back(i);
523  else m_octree_nodes[children_idx_base+ 7 ].pts.push_back(i);
524  }
525  }
526  }
527 
528  // Clear list of elements (they're now in our children):
529  {
530  std::vector<size_t> emptyVec;
531  node.pts.swap(emptyVec); // This is THE way of really clearing a std::vector
532  }
533 
534  // Recursive call on children:
535  for (int i=0;i<8;i++)
536  internal_recursive_split( node.child_id[i] );
537  }
538  } // end of internal_recursive_split
539 
540  public:
541 
542  /** Return the number of octree nodes (all of them, including the empty ones) \sa octree_get_nonempty_node_count */
543  size_t octree_get_node_count() const { return m_octree_nodes.size(); }
544 
545  /** Return the number of visible octree nodes in the last render event. */
546  size_t octree_get_visible_nodes() const { return m_visible_octree_nodes; }
547 
548  /** Called from the derived class (or the user) to indicate we have/want to rebuild the entire node tree (for example, after modifying the point cloud or any global octree parameter) */
549  inline void octree_mark_as_outdated() { m_octree_has_to_rebuild_all=true; }
550 
551  /** Returns a graphical representation of all the bounding boxes of the octree (leaf) nodes.
552  * \param[in] draw_solid_boxes If false, will draw solid boxes of color \a lines_color. Otherwise, wireframe boxes will be drawn.
553  */
556  const double lines_width = 1,
557  const mrpt::utils::TColorf &lines_color = mrpt::utils::TColorf(1,1,1),
558  const bool draw_solid_boxes = false ) const
559  {
560  octree_assure_uptodate();
561  gl_bb.clear();
562  for (size_t i=0;i<m_octree_nodes.size();i++)
563  {
564  const TNode & node = m_octree_nodes[i];
565  if (!node.is_leaf) continue;
566  mrpt::opengl::CBoxPtr gl_box = mrpt::opengl::CBox::Create();
567  gl_box->setBoxCorners( mrpt::math::TPoint3D(node.bb_min), mrpt::math::TPoint3D(node.bb_max) );
568  gl_box->setColor(lines_color);
569  gl_box->setLineWidth(lines_width);
570  gl_box->setWireframe(!draw_solid_boxes);
571  gl_bb.insert(gl_box);
572  }
573  }
574 
575 
576  /** Used for debug only */
577  void octree_debug_dump_tree(std::ostream &o) const
578  {
579  o << "Octree nodes: " << m_octree_nodes.size() << std::endl;
580  size_t total_elements = 0;
581  for (size_t i=0;i<m_octree_nodes.size();i++)
582  {
583  const TNode & node = m_octree_nodes[i];
584 
585  o << "Node #" << i << ": ";
586  if (node.is_leaf)
587  {
588  o << "leaf, ";
589  if (node.all) { o << "(all)\n"; total_elements+=octree_derived().size(); }
590  else { o << node.pts.size() << " elements; "; total_elements+=node.pts.size(); }
591 
592  }
593  else
594  {
595  o << "parent, center=(" << node.center.x << "," << node.center.y<<","<<node.center.z<<"), children: "
596  << node.child_id[0] << ","<< node.child_id[1] << ","<< node.child_id[2] << ","<< node.child_id[3] << ","
597  << node.child_id[4] << ","<< node.child_id[5] << ","<< node.child_id[6] << ","<< node.child_id[7] << "; ";
598  }
599  o << " bb: (" << node.bb_min.x << ","<< node.bb_min.y << ","<< node.bb_min.z << ")-("
600  << node.bb_max.x << ","<< node.bb_max.y << ","<< node.bb_max.z << ")\n";
601  }
602  o << "Total elements in all nodes: " << total_elements << std::endl;
603  } // end of octree_debug_dump_tree
604 
605  }; // end of class COctreePointRenderer
606 
607  } // end namespace
608 } // End of namespace
609 #endif
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.
A pair (x,y) of pixel coordinates (subpixel resolution).
Definition: TPixelCoord.h:21
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
void clear()
Clear the list of objects in the scene, deleting objects&#39; memory.
A set of object, which are referenced to the coordinates framework established in this object...
Definition: CSetOfObjects.h:32
size_t octree_get_visible_nodes() const
Return the number of visible octree nodes in the last render event.
#define MRPT_MAKE_ALIGNED_OPERATOR_NEW
Definition: memory.h:112
static CBoxPtr Create()
STL namespace.
OPENGL_IMPEXP size_t OCTREE_RENDER_MAX_POINTS_PER_NODE
Default value = 1e5.
std::deque< TYPE1, Eigen::aligned_allocator< TYPE1 > > deque_t
const Derived & octree_derived() const
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...
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...
int vp_height
Rendering viewport geometry (in pixels)
Definition: gl_utils.h:37
Lightweight 3D point (float version).
T max(const T v0, const T v1)
Definition: exprtk.hpp:1331
size_t octree_get_node_count() const
Return the number of octree nodes (all of them, including the empty ones)
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:35
void octree_render(const mrpt::opengl::gl_utils::TRenderInfo &ri) const
Render the entire octree recursively.
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.
mrpt::aligned_containers< TNode >::deque_t m_octree_nodes
First one [0] is always the root node.
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...
void insert(const CRenderizablePtr &newObject)
Insert a new object to the list.
void octree_getBoundingBox(mrpt::math::TPoint3D &bb_min, mrpt::math::TPoint3D &bb_max) const
x y t t *t x y t t t x y t t t x *y t *t t x *y t *t t x y t t t x y t t t x(y+z)
OPENGL_IMPEXP float OCTREE_RENDER_MAX_DENSITY_POINTS_PER_SQPIXEL
Default value = 0.01 points/px^2.
A RGB color - floats in the range [0,1].
Definition: TColor.h:80
#define DO_RECURSE_CHILD(INDEX, SEQ0, SEQ1, SEQ2, SEQ3, SEQ4, SEQ5, SEQ6, SEQ7)
float render_area_sqpixels
The approximate size of the octree on the screen (squared pixels).
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.
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
Lightweight 3D point.
void setBBFromOrderInParent(const TNode &parent, int my_child_index)
void octree_assure_uptodate() const
Must be called at children class&#39; render() previously to octree_render()
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...
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
#define PROJ_SUB_NODE_ALREADY_DONE(INDEX, POSTFIX)



Page generated by Doxygen 1.8.13 for MRPT 1.5.3 at Sun Nov 26 00:44:48 UTC 2017