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


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013