Main MRPT website > C++ reference for MRPT 1.4.0
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-2016, 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>
17
18namespace 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 */
53 { }
54
55 /** Copy ctor */
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 {
74 }
75
76 /** Render the entire octree recursively.
77 * Should be called from children's render() method.
78 */
80 {
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
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 {
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 {
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:
214
215 /** Render a given node. */
217 size_t node_idx,
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
239 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());
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:
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
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;
424
425 // Reset list of nodes:
426 m_octree_nodes.assign(1, TNode() );
427
428 // recursive decide:
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++)
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. */
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) */
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 {
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;
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
#define PROJ_SUB_NODE(POSTFIX)
#define DO_RECURSE_CHILD(INDEX, SEQ0, SEQ1, SEQ2, SEQ3, SEQ4, SEQ5, SEQ6, SEQ7)
#define PROJ_SUB_NODE_ALREADY_DONE(INDEX, POSTFIX)
static CBoxPtr Create()
Template class that implements the data structure and algorithms for Octree-based efficient rendering...
mrpt::aligned_containers< TNode >::deque_t m_octree_nodes
First one [0] is always the root node.
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_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 octree_assure_uptodate() const
Must be called at children class' render() previously to octree_render()
std::vector< TRenderQueueElement > m_render_queue
The list of elements that really are visible and will be rendered.
const Derived & octree_derived() const
void octree_render(const mrpt::opengl::gl_utils::TRenderInfo &ri) const
Render the entire octree recursively.
size_t octree_get_visible_nodes() const
Return the number of visible octree nodes in the last render event.
COctreePointRenderer(const COctreePointRenderer &)
Copy ctor.
void octree_getBoundingBox(mrpt::math::TPoint3D &bb_min, mrpt::math::TPoint3D &bb_max) const
void internal_recursive_split(const size_t node_id, const bool all_pts=false)
size_t octree_get_node_count() const
Return the number of octree nodes (all of them, including the empty ones)
void octree_debug_dump_tree(std::ostream &o) const
Used for debug only.
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.
A set of object, which are referenced to the coordinates framework established in this object.
Definition: CSetOfObjects.h:33
void clear()
Clear the list of objects in the scene, deleting objects' memory.
void insert(const CRenderizablePtr &newObject)
Insert a new object to the list.
EIGEN_STRONG_INLINE double mean() const
Computes the mean of the entire matrix.
OPENGL_IMPEXP size_t OCTREE_RENDER_MAX_POINTS_PER_NODE
Default value = 1e5.
OPENGL_IMPEXP float OCTREE_RENDER_MAX_DENSITY_POINTS_PER_SQPIXEL
Default value = 0.01 points/px^2.
#define MRPT_MAKE_ALIGNED_OPERATOR_NEW
Definition: memory.h:112
struct OPENGL_IMPEXP CBoxPtr
Definition: CBox.h:19
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:145
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:140
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
STL namespace.
std::deque< TYPE1, Eigen::aligned_allocator< TYPE1 > > deque_t
Lightweight 3D point.
Lightweight 3D point (float version).
The structure for each octree spatial node.
void update_bb(const mrpt::math::TPoint3Df &p)
update bounding box with a new point:
size_t child_id[8]
[is_leaf=false] The indices in m_octree_nodes of the 8 children.
mrpt::math::TPoint3Df center
[is_leaf=false] The center of the node, whose coordinates are used to decide between the 8 children n...
void setBBFromOrderInParent(const TNode &parent, int my_child_index)
std::vector< size_t > pts
Point indices in the derived class that fall into this node.
bool is_leaf
true: it's a leaf and pts has valid indices; false: children is valid.
bool all
true: All elements in the reference object; false: only those in pts
float render_area_sqpixels
The approximate size of the octree on the screen (squared pixels).
Information about the rendering process being issued.
Definition: gl_utils.h:36
int vp_height
Rendering viewport geometry (in pixels)
Definition: gl_utils.h:37
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
A RGB color - floats in the range [0,1].
Definition: TColor.h:53
A pair (x,y) of pixel coordinates (subpixel resolution).
Definition: TPixelCoord.h:22



Page generated by Doxygen 1.9.6 for MRPT 1.4.0 SVN: at Fri Jan 20 00:45:34 UTC 2023