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


Page generated by Doxygen 1.8.14 for MRPT 1.5.5 Git: e06b63dbf Fri Dec 1 14:41:11 2017 +0100 at lun oct 28 01:31:35 CET 2019