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


Page generated by Doxygen 1.8.8 for MRPT 1.2.2 SVN:Unversioned directory at Tue Oct 14 02:14:08 UTC 2014