Main MRPT website > C++ reference
MRPT logo

COctreePointRenderer.h

Go to the documentation of this file.
00001 /* +---------------------------------------------------------------------------+
00002    |          The Mobile Robot Programming Toolkit (MRPT) C++ library          |
00003    |                                                                           |
00004    |                   http://mrpt.sourceforge.net/                            |
00005    |                                                                           |
00006    |   Copyright (C) 2005-2011  University of Malaga                           |
00007    |                                                                           |
00008    |    This software was written by the Machine Perception and Intelligent    |
00009    |      Robotics Lab, University of Malaga (Spain).                          |
00010    |    Contact: Jose-Luis Blanco  <jlblanco@ctima.uma.es>                     |
00011    |                                                                           |
00012    |  This file is part of the MRPT project.                                   |
00013    |                                                                           |
00014    |     MRPT is free software: you can redistribute it and/or modify          |
00015    |     it under the terms of the GNU General Public License as published by  |
00016    |     the Free Software Foundation, either version 3 of the License, or     |
00017    |     (at your option) any later version.                                   |
00018    |                                                                           |
00019    |   MRPT is distributed in the hope that it will be useful,                 |
00020    |     but WITHOUT ANY WARRANTY; without even the implied warranty of        |
00021    |     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         |
00022    |     GNU General Public License for more details.                          |
00023    |                                                                           |
00024    |     You should have received a copy of the GNU General Public License     |
00025    |     along with MRPT.  If not, see <http://www.gnu.org/licenses/>.         |
00026    |                                                                           |
00027    +---------------------------------------------------------------------------+ */
00028 #ifndef opengl_COctreePointRenderer_H
00029 #define opengl_COctreePointRenderer_H
00030 
00031 #include <mrpt/opengl/CRenderizable.h>
00032 #include <mrpt/opengl/CSetOfObjects.h>
00033 #include <mrpt/opengl/CBox.h>
00034 
00035 
00036 namespace mrpt
00037 {
00038         namespace global_settings
00039         {
00040                 /** Default value = 0.01 points/px^2. Affects to these classes (read their docs for further details):
00041                   *             - mrpt::opengl::CPointCloud
00042                   *             - mrpt::opengl::CPointCloudColoured
00043                   */
00044                 extern OPENGL_IMPEXP float OCTREE_RENDER_MAX_DENSITY_POINTS_PER_SQPIXEL;
00045 
00046                 /** Default value = 1e5. Maximum number of elements in each octree node before spliting. Affects to these classes (read their docs for further details):
00047                   *             - mrpt::opengl::CPointCloud
00048                   *             - mrpt::opengl::CPointCloudColoured
00049                   */
00050                 extern OPENGL_IMPEXP size_t OCTREE_RENDER_MAX_POINTS_PER_NODE;
00051         }
00052 
00053 
00054         namespace opengl
00055         {
00056                 using namespace mrpt::utils;
00057 
00058                 /** Template class that implements the data structure and algorithms for Octree-based efficient rendering.
00059                   *  \sa mrpt::opengl::CPointCloud, mrpt::opengl::CPointCloudColoured, http://www.mrpt.org/Efficiently_rendering_point_clouds_of_millions_of_points
00060                   */
00061                 template <class Derived>
00062                 class COctreePointRenderer
00063                 {
00064                 public:
00065                         /** Default ctor */
00066                         COctreePointRenderer() :
00067                                 m_octree_has_to_rebuild_all(true),
00068                                 m_visible_octree_nodes(0),
00069                                 m_visible_octree_nodes_ongoing(0)
00070                         { }
00071 
00072                         /** Copy ctor */
00073                         COctreePointRenderer(const COctreePointRenderer &) :
00074                                 m_octree_has_to_rebuild_all(true)
00075                         { }
00076 
00077 
00078                         enum { OCTREE_ROOT_NODE = 0 };
00079 
00080                 protected:
00081                         // Helper methods in any CRTP template
00082                         inline       Derived & octree_derived()       { return *static_cast<Derived*>(this); }
00083                         inline const Derived & octree_derived() const { return *static_cast<const Derived*>(this); }
00084 
00085                         /** Must be called at children class' render() previously to \a octree_render() */
00086                         inline void octree_assure_uptodate() const
00087                         {
00088                                 const_cast<COctreePointRenderer<Derived>*>(this)->internal_octree_assure_uptodate();
00089                         }
00090 
00091                         /** Render the entire octree recursively.
00092                           * Should be called from children's render() method.
00093                           */
00094                         void octree_render(const mrpt::opengl::CRenderizable::TRenderInfo &ri ) const
00095                         {
00096                                 m_visible_octree_nodes_ongoing = 0;
00097 
00098                                 // Stage 1: Build list of visible octrees
00099                                 m_render_queue.clear();
00100                                 m_render_queue.reserve(m_octree_nodes.size());
00101 
00102                                 TPixelCoordf cr_px[8];
00103                                 float        cr_z[8];
00104                                 octree_recursive_render(OCTREE_ROOT_NODE,ri, cr_px, cr_z, false /* corners are not computed for this first iteration */ );
00105 
00106                                 m_visible_octree_nodes = m_visible_octree_nodes_ongoing;
00107 
00108                                 // Stage 2: Render them all
00109                                 for (size_t i=0;i<m_render_queue.size();i++)
00110                                 {
00111                                         const TNode & node = m_octree_nodes[ m_render_queue[i].node_id ];
00112                                         octree_derived().render_subset( node.all,node.pts,m_render_queue[i].render_area_sqpixels);
00113                                 }
00114                         }
00115 
00116 
00117                 private:
00118                         /** The structure for each octree spatial node. Each node can either be a leaf of has 8 children nodes.
00119                           *  Instead of pointers, children are referenced by their indices in \a m_octree_nodes
00120                           */
00121                         struct OPENGL_IMPEXP TNode
00122                         {
00123                                 TNode() :
00124                                         bb_min( std::numeric_limits<float>::max(), std::numeric_limits<float>::max(), std::numeric_limits<float>::max() ),
00125                                         bb_max(-std::numeric_limits<float>::max(),-std::numeric_limits<float>::max(),-std::numeric_limits<float>::max() )
00126                                 { }
00127 
00128                                 bool                  is_leaf;     //!< true: it's a leaf and \a pts has valid indices; false: \a children is valid.
00129 
00130                                 // In all cases, the bounding_box:
00131                                 mrpt::math::TPoint3Df  bb_min, bb_max;
00132 
00133                                 // Fields used if is_leaf=true
00134                                 std::vector<size_t>   pts;         //!< Point indices in the derived class that fall into this node.
00135                                 bool                  all;         //!< true: All elements in the reference object; false: only those in \a pts
00136 
00137                                 // Fields used if is_leaf=false
00138                                 mrpt::math::TPoint3Df center;      //!< [is_leaf=false] The center of the node, whose coordinates are used to decide between the 8 children nodes.
00139                                 size_t                child_id[8]; //!< [is_leaf=false] The indices in \a m_octree_nodes of the 8 children.
00140 
00141                                 /** update bounding box with a new point: */
00142                                 inline void update_bb(const mrpt::math::TPoint3Df &p)
00143                                 {
00144                                         keep_min(bb_min.x, p.x); keep_min(bb_min.y, p.y); keep_min(bb_min.z, p.z);
00145                                         keep_max(bb_max.x, p.x); keep_max(bb_max.y, p.y); keep_max(bb_max.z, p.z);
00146                                 }
00147 
00148                                 inline float getCornerX(int i) const { return (i & 0x01)==0 ? bb_min.x : bb_max.x; }
00149                                 inline float getCornerY(int i) const { return (i & 0x02)==0 ? bb_min.y : bb_max.y; }
00150                                 inline float getCornerZ(int i) const { return (i & 0x04)==0 ? bb_min.z : bb_max.z; }
00151 
00152                                 void setBBFromOrderInParent(const TNode &parent, int my_child_index)
00153                                 {
00154                                         // Coordinate signs are relative to the parent center (split point):
00155                                         switch (my_child_index)
00156                                         {
00157                                         case 0:  // x-, y-, z-
00158                                                 bb_min = parent.bb_min;
00159                                                 bb_max = parent.center;
00160                                                 break;
00161                                         case 1:  // x+, y-, z-
00162                                                 bb_min.x = parent.center.x; bb_max.x = parent.bb_max.x;
00163                                                 bb_min.y = parent.bb_min.y; bb_max.y = parent.center.y;
00164                                                 bb_min.z = parent.bb_min.z; bb_max.z = parent.center.z;
00165                                                 break;
00166                                         case 2:  // x-, y+, z-
00167                                                 bb_min.x = parent.bb_min.x; bb_max.x = parent.center.x;
00168                                                 bb_min.y = parent.center.y; bb_max.y = parent.bb_max.y;
00169                                                 bb_min.z = parent.bb_min.z; bb_max.z = parent.center.z;
00170                                                 break;
00171                                         case 3:  // x+, y+, z-
00172                                                 bb_min.x = parent.center.x; bb_max.x = parent.bb_max.x;
00173                                                 bb_min.y = parent.center.y; bb_max.y = parent.bb_max.y;
00174                                                 bb_min.z = parent.bb_min.z; bb_max.z = parent.center.z;
00175                                                 break;
00176                                         case 4:  // x-, y-, z+
00177                                                 bb_min.x = parent.bb_min.x; bb_max.x = parent.center.x;
00178                                                 bb_min.y = parent.bb_min.y; bb_max.y = parent.center.y;
00179                                                 bb_min.z = parent.center.z; bb_max.z = parent.bb_max.z;
00180                                                 break;
00181                                         case 5:  // x+, y-, z+
00182                                                 bb_min.x = parent.center.x; bb_max.x = parent.bb_max.x;
00183                                                 bb_min.y = parent.bb_min.y; bb_max.y = parent.center.y;
00184                                                 bb_min.z = parent.center.z; bb_max.z = parent.bb_max.z;
00185                                                 break;
00186                                         case 6:  // x-, y+, z+
00187                                                 bb_min.x = parent.bb_min.x; bb_max.x = parent.center.x;
00188                                                 bb_min.y = parent.center.y; bb_max.y = parent.bb_max.y;
00189                                                 bb_min.z = parent.center.z; bb_max.z = parent.bb_max.z;
00190                                                 break;
00191                                         case 7:  // x+, y+, z+
00192                                                 bb_min = parent.center;
00193                                                 bb_max = parent.bb_max;
00194                                                 break;
00195                                         default: throw std::runtime_error("my_child_index!=[0,7]");
00196                                         }
00197                                 }
00198                         };
00199 
00200                         struct OPENGL_IMPEXP TRenderQueueElement
00201                         {
00202                                 inline TRenderQueueElement(const size_t id, float area_sq) : node_id(id), render_area_sqpixels(area_sq) {  }
00203 
00204                                 size_t  node_id;              //!< The node ID to render
00205                                 float   render_area_sqpixels; //!< The approximate size of the octree on the screen (squared pixels).
00206                         };
00207                         mutable std::vector<TRenderQueueElement>  m_render_queue; //!< The list of elements that really are visible and will be rendered.
00208 
00209 
00210                         bool  m_octree_has_to_rebuild_all;
00211                         std::deque<TNode>  m_octree_nodes; //!< First one [0] is always the root node
00212 
00213                         // Counters of visible octrees for each render:
00214                         volatile mutable size_t m_visible_octree_nodes, m_visible_octree_nodes_ongoing;
00215 
00216                         /** Render a given node. */
00217                         void octree_recursive_render(
00218                                 size_t node_idx,
00219                                 const mrpt::opengl::CRenderizable::TRenderInfo &ri,
00220                                 TPixelCoordf cr_px[8],
00221                                 float        cr_z[8],
00222                                 bool         corners_are_all_computed = true,
00223                                 bool         trust_me_youre_visible   = false,
00224                                 float        approx_area_sqpixels     = 0
00225                                 ) const
00226                         {
00227                                 const TNode &node = m_octree_nodes[node_idx];
00228 
00229                                 if (!corners_are_all_computed)
00230                                 {
00231                                         for (int i=0;i<8;i++)
00232                                         {
00233                                                 // project point:
00234                                                 ri.projectPointPixels(
00235                                                         node.getCornerX(i),node.getCornerY(i),node.getCornerZ(i),
00236                                                         cr_px[i].x,cr_px[i].y,cr_z[i]);
00237                                         }
00238                                 }
00239 
00240                                 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());
00241                                 if (!trust_me_youre_visible)
00242                                 {
00243                                         // Keep the on-screen bounding box of this node:
00244                                         for (int i=0;i<8;i++)
00245                                         {
00246                                                 keep_min(px_min.x,cr_px[i].x); keep_min(px_min.y,cr_px[i].y);
00247                                                 keep_max(px_max.x,cr_px[i].x); keep_max(px_max.y,cr_px[i].y);
00248                                         }
00249                                         
00250                                         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);
00251                                         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);
00252                                         const bool box_crosses_image_plane = any_cr_zs_pos && any_cr_zs_neg;
00253 
00254                                         // If all 8 corners are way out of the screen (and all "cr_z" have the same sign),
00255                                         // this node and all the children are not visible:
00256                                         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) )
00257                                                 return; // Not visible
00258                                 }
00259 
00260                                 // Check if the node has points and is visible:
00261                                 if (node.is_leaf)
00262                                 {       // Render this leaf node:
00263                                         if (node.all || !node.pts.empty())
00264                                         {
00265                                                 // If we are here, it seems at least a part of the Box is visible:
00266                                                 m_visible_octree_nodes_ongoing++;
00267 
00268                                                 const float render_area_sqpixels = trust_me_youre_visible ? 
00269                                                         approx_area_sqpixels 
00270                                                         :                                                       
00271                                                         std::abs(px_min.x-px_max.x) * std::abs(px_min.y-px_max.y);
00272 
00273                                                 // OK: Add to list of rendering-pending:
00274                                                 m_render_queue.push_back( TRenderQueueElement(node_idx,render_area_sqpixels) );
00275                                         }
00276                                 }
00277                                 else
00278                                 {       // Render children nodes:
00279                                         // If ALL my 8 corners are within the screen, tell our children that they 
00280                                         //  won't need to compute anymore, since all of them and their children are visible as well:
00281                                         bool children_are_all_visible_for_sure = true;
00282 
00283                                         if (!trust_me_youre_visible) // Trust my parent... otherwise:
00284                                         {
00285                                                 for (int i=0;i<8;i++) 
00286                                                 {
00287                                                         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 ))
00288                                                         {
00289                                                                 children_are_all_visible_for_sure = false;
00290                                                                 break;
00291                                                         }
00292                                                 }
00293                                         }
00294 
00295                                         // If all children are visible, it's easy:
00296                                         if (children_are_all_visible_for_sure)
00297                                         {
00298                                                 TPixelCoordf child_cr_px[8]; // No need to initialize
00299                                                 float        child_cr_z[8];  // No need to initialize
00300                                                 
00301                                                 // Approximate area of the children nodes:
00302                                                 const float approx_child_area = trust_me_youre_visible ?
00303                                                         approx_area_sqpixels/8.0f 
00304                                                         :
00305                                                         std::abs(px_min.x-px_max.x) * std::abs(px_min.y-px_max.y) / 8.0f;
00306 
00307                                                 for (int i=0;i<8;i++)
00308                                                         this->octree_recursive_render(node.child_id[i],ri,child_cr_px, child_cr_z, true,  true, approx_child_area); \
00309                                         }
00310                                         else
00311                                         {
00312                                                 // Precompute the 19 (3*9-8) intermediary points so children don't have to compute them several times:
00313                                                 const TPoint3Df p_Xm_Ym_Zm ( node.bb_min.x, node.bb_min.y, node.bb_min.z ); // 0
00314                                                 const TPoint3Df p_X0_Ym_Zm ( node.center.x, node.bb_min.y, node.bb_min.z );
00315                                                 const TPoint3Df p_Xp_Ym_Zm ( node.bb_max.x, node.bb_min.y, node.bb_min.z ); // 1
00316                                                 const TPoint3Df p_Xm_Y0_Zm ( node.bb_min.x, node.center.y, node.bb_min.z );
00317                                                 const TPoint3Df p_X0_Y0_Zm ( node.center.x, node.center.y, node.bb_min.z );
00318                                                 const TPoint3Df p_Xp_Y0_Zm ( node.bb_max.x, node.center.y, node.bb_min.z );
00319                                                 const TPoint3Df p_Xm_Yp_Zm ( node.bb_min.x, node.bb_max.y, node.bb_min.z ); // 2
00320                                                 const TPoint3Df p_X0_Yp_Zm ( node.center.x, node.bb_max.y, node.bb_min.z );
00321                                                 const TPoint3Df p_Xp_Yp_Zm ( node.bb_max.x, node.bb_max.y, node.bb_min.z ); // 3
00322 
00323                                                 const TPoint3Df p_Xm_Ym_Z0 ( node.bb_min.x, node.bb_min.y, node.center.z );
00324                                                 const TPoint3Df p_X0_Ym_Z0 ( node.center.x, node.bb_min.y, node.center.z );
00325                                                 const TPoint3Df p_Xp_Ym_Z0 ( node.bb_max.x, node.bb_min.y, node.center.z );
00326                                                 const TPoint3Df p_Xm_Y0_Z0 ( node.bb_min.x, node.center.y, node.center.z );
00327                                                 const TPoint3Df p_X0_Y0_Z0 ( node.center.x, node.center.y, node.center.z );
00328                                                 const TPoint3Df p_Xp_Y0_Z0 ( node.bb_max.x, node.center.y, node.center.z );
00329                                                 const TPoint3Df p_Xm_Yp_Z0 ( node.bb_min.x, node.bb_max.y, node.center.z );
00330                                                 const TPoint3Df p_X0_Yp_Z0 ( node.center.x, node.bb_max.y, node.center.z );
00331                                                 const TPoint3Df p_Xp_Yp_Z0 ( node.bb_max.x, node.bb_max.y, node.center.z );
00332 
00333                                                 const TPoint3Df p_Xm_Ym_Zp ( node.bb_min.x, node.bb_min.y, node.bb_max.z ); // 4
00334                                                 const TPoint3Df p_X0_Ym_Zp ( node.center.x, node.bb_min.y, node.bb_max.z );
00335                                                 const TPoint3Df p_Xp_Ym_Zp ( node.bb_min.x, node.bb_min.y, node.bb_max.z ); // 5
00336                                                 const TPoint3Df p_Xm_Y0_Zp ( node.bb_min.x, node.center.y, node.bb_max.z );
00337                                                 const TPoint3Df p_X0_Y0_Zp ( node.center.x, node.center.y, node.bb_max.z );
00338                                                 const TPoint3Df p_Xp_Y0_Zp ( node.bb_max.x, node.center.y, node.bb_max.z );
00339                                                 const TPoint3Df p_Xm_Yp_Zp ( node.bb_min.x, node.bb_max.y, node.bb_max.z ); // 6
00340                                                 const TPoint3Df p_X0_Yp_Zp ( node.center.x, node.bb_max.y, node.bb_max.z );
00341                                                 const TPoint3Df p_Xp_Yp_Zp ( node.bb_max.x, node.bb_max.y, node.bb_max.z ); // 7
00342 
00343                                                 // Project all these points:
00344 #define PROJ_SUB_NODE(POSTFIX) \
00345                                                 TPixelCoordf px_##POSTFIX; \
00346                                                 float        depth_##POSTFIX; \
00347                                                 ri.projectPointPixels( p_##POSTFIX.x, p_##POSTFIX.y, p_##POSTFIX.z, px_##POSTFIX.x,px_##POSTFIX.y,depth_##POSTFIX);
00348 
00349 #define PROJ_SUB_NODE_ALREADY_DONE(INDEX, POSTFIX) \
00350                                                 const TPixelCoordf px_##POSTFIX = cr_px[INDEX]; \
00351                                                 float        depth_##POSTFIX = cr_z[INDEX];
00352 
00353                                                 PROJ_SUB_NODE_ALREADY_DONE(0,Xm_Ym_Zm)
00354                                                 PROJ_SUB_NODE(X0_Ym_Zm)
00355                                                 PROJ_SUB_NODE_ALREADY_DONE(1, Xp_Ym_Zm)
00356 
00357                                                 PROJ_SUB_NODE(Xm_Y0_Zm)
00358                                                 PROJ_SUB_NODE(X0_Y0_Zm)
00359                                                 PROJ_SUB_NODE(Xp_Y0_Zm)
00360 
00361                                                 PROJ_SUB_NODE_ALREADY_DONE(2, Xm_Yp_Zm)
00362                                                 PROJ_SUB_NODE(X0_Yp_Zm)
00363                                                 PROJ_SUB_NODE_ALREADY_DONE(3, Xp_Yp_Zm)
00364 
00365                                                 PROJ_SUB_NODE(Xm_Ym_Z0)
00366                                                 PROJ_SUB_NODE(X0_Ym_Z0)
00367                                                 PROJ_SUB_NODE(Xp_Ym_Z0)
00368                                                 PROJ_SUB_NODE(Xm_Y0_Z0)
00369                                                 PROJ_SUB_NODE(X0_Y0_Z0)
00370                                                 PROJ_SUB_NODE(Xp_Y0_Z0)
00371                                                 PROJ_SUB_NODE(Xm_Yp_Z0)
00372                                                 PROJ_SUB_NODE(X0_Yp_Z0)
00373                                                 PROJ_SUB_NODE(Xp_Yp_Z0)
00374 
00375                                                 PROJ_SUB_NODE_ALREADY_DONE(4, Xm_Ym_Zp)
00376                                                 PROJ_SUB_NODE(X0_Ym_Zp)
00377                                                 PROJ_SUB_NODE_ALREADY_DONE(5, Xp_Ym_Zp)
00378 
00379                                                 PROJ_SUB_NODE(Xm_Y0_Zp)
00380                                                 PROJ_SUB_NODE(X0_Y0_Zp)
00381                                                 PROJ_SUB_NODE(Xp_Y0_Zp)
00382 
00383                                                 PROJ_SUB_NODE_ALREADY_DONE(6, Xm_Yp_Zp)
00384                                                 PROJ_SUB_NODE(X0_Yp_Zp)
00385                                                 PROJ_SUB_NODE_ALREADY_DONE(7, Xp_Yp_Zp)
00386 
00387                                                 // Recursive call children nodes:
00388 #define DO_RECURSE_CHILD(INDEX, SEQ0,SEQ1,SEQ2,SEQ3,SEQ4,SEQ5,SEQ6,SEQ7) \
00389                                                 { \
00390                                                         TPixelCoordf child_cr_px[8] = { px_##SEQ0,px_##SEQ1,px_##SEQ2,px_##SEQ3,px_##SEQ4,px_##SEQ5,px_##SEQ6,px_##SEQ7 }; \
00391                                                         float        child_cr_z[8]  = { depth_##SEQ0,depth_##SEQ1,depth_##SEQ2,depth_##SEQ3,depth_##SEQ4,depth_##SEQ5,depth_##SEQ6,depth_##SEQ7 }; \
00392                                                         this->octree_recursive_render(node.child_id[INDEX],ri,child_cr_px, child_cr_z); \
00393                                                 }
00394 
00395                                                 //                     0         1         2         3          4         5        6         7
00396                                                 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 )
00397                                                 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 )
00398                                                 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 )
00399                                                 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 )
00400                                                 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 )
00401                                                 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 )
00402                                                 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 )
00403                                                 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 )
00404 #undef DO_RECURSE_CHILD
00405 #undef PROJ_SUB_NODE
00406 #undef PROJ_SUB_NODE_ALREADY_DONE
00407                                         } // end "children_are_all_visible_for_sure"=false
00408                                 }
00409                         }
00410 
00411                         // The actual implementation (and non-const version) of octree_assure_uptodate()
00412                         void internal_octree_assure_uptodate()
00413                         {
00414                                 if (!m_octree_has_to_rebuild_all) return;
00415                                 m_octree_has_to_rebuild_all = false;
00416 
00417                                 // Reset list of nodes:
00418                                 m_octree_nodes.assign(1, TNode() );
00419 
00420                                 // recursive decide:
00421                                 internal_recursive_split( OCTREE_ROOT_NODE, true );
00422                         }
00423 
00424                         // Check the node "node_id" and create its children if needed, by looking at its list
00425                         //  of elements (or all derived object's elements if "all_pts"=true, which will only happen
00426                         //  for the root node)
00427                         void internal_recursive_split(const size_t node_id, const bool all_pts = false)
00428                         {
00429                                 TNode &node = m_octree_nodes[node_id];
00430                                 const size_t N = all_pts ? octree_derived().size() : node.pts.size();
00431 
00432                                 const bool has_to_compute_bb = (node_id ==OCTREE_ROOT_NODE);
00433 
00434                                 if (N<=mrpt::global_settings::OCTREE_RENDER_MAX_POINTS_PER_NODE)
00435                                 {
00436                                         // No need to split this node:
00437                                         node.is_leaf = true;
00438                                         node.all     = all_pts;
00439 
00440                                         // Update bounding-box:
00441                                         if (has_to_compute_bb)
00442                                         {
00443                                                 if (all_pts)
00444                                                          for (size_t i=0;i<N;i++) node.update_bb( octree_derived().getPointf(i) );
00445                                                 else for (size_t i=0;i<N;i++) node.update_bb( octree_derived().getPointf(node.pts[i]) );
00446                                         }
00447                                 }
00448                                 else
00449                                 {
00450                                         // We have to split the node.
00451                                         // Compute the mean of all elements:
00452                                         mrpt::math::TPoint3Df mean(0,0,0);
00453                                         if (all_pts)
00454                                                 for (size_t i=0;i<N;i++)
00455                                                 {
00456                                                         mrpt::math::TPoint3Df p = octree_derived().getPointf(i);
00457                                                         mean+= p;
00458                                                         if (has_to_compute_bb) node.update_bb( p );
00459                                                 }
00460                                         else
00461                                                 for (size_t i=0;i<N;i++)
00462                                                 {
00463                                                         mrpt::math::TPoint3Df p = octree_derived().getPointf(node.pts[i]);
00464                                                         mean+= p;
00465                                                         if (has_to_compute_bb) node.update_bb( p );
00466                                                 }
00467 
00468                                         // Save my split point:
00469                                         node.is_leaf = false;
00470                                         node.center  = mean * (1.0f/N);
00471 
00472                                         // Allocate my 8 children structs
00473                                         const size_t children_idx_base = m_octree_nodes.size();
00474                                         m_octree_nodes.resize(children_idx_base + 8 );
00475                                         for (int i=0;i<8;i++)
00476                                                 node.child_id[i] = children_idx_base + i;
00477 
00478                                         // Set the bounding-boxes of my children (we already know them):
00479                                         for (int i=0;i<8;i++)
00480                                                 m_octree_nodes[children_idx_base + i].setBBFromOrderInParent(node,i);
00481 
00482                                         // Divide elements among children:
00483                                         const mrpt::math::TPoint3Df &c = node.center; // to make notation clearer
00484                                         for (size_t j=0;j<N;j++)
00485                                         {
00486                                                 const size_t i = all_pts ? j : node.pts[j];
00487                                                 const TPoint3Df p = octree_derived().getPointf(i);
00488                                                 if (p.z<c.z)
00489                                                 {
00490                                                         if (p.y<c.y)
00491                                                         {
00492                                                                 if (p.x<c.x)
00493                                                                       m_octree_nodes[children_idx_base+ 0 ].pts.push_back(i);
00494                                                                 else  m_octree_nodes[children_idx_base+ 1 ].pts.push_back(i);
00495                                                         }
00496                                                         else
00497                                                         {
00498                                                                 if (p.x<c.x)
00499                                                                       m_octree_nodes[children_idx_base+ 2 ].pts.push_back(i);
00500                                                                 else  m_octree_nodes[children_idx_base+ 3 ].pts.push_back(i);
00501                                                         }
00502                                                 }
00503                                                 else
00504                                                 {
00505                                                         if (p.y<c.y)
00506                                                         {
00507                                                                 if (p.x<c.x)
00508                                                                       m_octree_nodes[children_idx_base+ 4 ].pts.push_back(i);
00509                                                                 else  m_octree_nodes[children_idx_base+ 5 ].pts.push_back(i);
00510                                                         }
00511                                                         else
00512                                                         {
00513                                                                 if (p.x<c.x)
00514                                                                       m_octree_nodes[children_idx_base+ 6 ].pts.push_back(i);
00515                                                                 else  m_octree_nodes[children_idx_base+ 7 ].pts.push_back(i);
00516                                                         }
00517                                                 }
00518                                         }
00519 
00520                                         // Clear list of elements (they're now in our children):
00521                                         {
00522                                                 std::vector<size_t> emptyVec;
00523                                                 node.pts.swap(emptyVec);  // This is THE way of really clearing a std::vector
00524                                         }
00525 
00526                                         // Recursive call on children:
00527                                         for (int i=0;i<8;i++)
00528                                                 internal_recursive_split( node.child_id[i] );
00529                                 }
00530                         } // end of internal_recursive_split
00531 
00532                 public:
00533 
00534                         /** Return the number of octree nodes (all of them, including the empty ones) \sa octree_get_nonempty_node_count */
00535                         size_t octree_get_node_count() const { return m_octree_nodes.size(); }
00536 
00537                         /** Return the number of visible octree nodes in the last render event. */
00538                         size_t octree_get_visible_nodes() const { return m_visible_octree_nodes; }
00539 
00540                         /** 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) */
00541                         inline void octree_mark_as_outdated() { m_octree_has_to_rebuild_all=true; }
00542 
00543                         /** Returns a graphical representation of all the bounding boxes of the octree (leaf) nodes.
00544                           */
00545                         void octree_get_graphics_boundingboxes(
00546                                 mrpt::opengl::CSetOfObjects &gl_bb,
00547                                 const double lines_width = 1,
00548                                 const TColorf lines_color = TColorf(1,1,1) ) const
00549                         {
00550                                 octree_assure_uptodate();
00551                                 gl_bb.clear();
00552                                 for (size_t i=0;i<m_octree_nodes.size();i++)
00553                                 {
00554                                         const TNode & node = m_octree_nodes[i];
00555                                         if (!node.is_leaf) continue;
00556                                         mrpt::opengl::CBoxPtr gl_box = mrpt::opengl::CBox::Create();
00557                                         gl_box->setBoxCorners( mrpt::math::TPoint3D(node.bb_min), mrpt::math::TPoint3D(node.bb_max) );
00558                                         gl_box->setColor(lines_color);
00559                                         gl_box->setLineWidth(lines_width);
00560                                         gl_box->setWireframe(true);
00561                                         gl_bb.insert(gl_box);
00562                                 }
00563                         }
00564 
00565 
00566                         /** Used for debug only */
00567                         void octree_debug_dump_tree(std::ostream &o) const
00568                         {
00569                                 o << "Octree nodes: " << m_octree_nodes.size() << std::endl;
00570                                 size_t total_elements = 0;
00571                                 for (size_t i=0;i<m_octree_nodes.size();i++)
00572                                 {
00573                                         const TNode & node = m_octree_nodes[i];
00574 
00575                                         o << "Node #" << i << ": ";
00576                                         if (node.is_leaf)
00577                                         {
00578                                                 o << "leaf, ";
00579                                                 if (node.all) { o << "(all)\n"; total_elements+=octree_derived().size(); }
00580                                                 else { o << node.pts.size() << " elements; "; total_elements+=node.pts.size(); }
00581 
00582                                         }
00583                                         else
00584                                         {
00585                                                 o << "parent, center=(" << node.center.x << "," << node.center.y<<","<<node.center.z<<"), children: "
00586                                                   << node.child_id[0] << ","<< node.child_id[1] << ","<< node.child_id[2] << ","<< node.child_id[3] << ","
00587                                                   << node.child_id[4] << ","<< node.child_id[5] << ","<< node.child_id[6] << ","<< node.child_id[7] << "; ";
00588                                         }
00589                                         o << " bb: (" << node.bb_min.x << ","<< node.bb_min.y << ","<< node.bb_min.z << ")-("
00590                                                       << node.bb_max.x << ","<< node.bb_max.y << ","<< node.bb_max.z << ")\n";
00591                                 }
00592                                 o << "Total elements in all nodes: " << total_elements << std::endl;
00593                         } // end of octree_debug_dump_tree
00594 
00595                 }; // end of class COctreePointRenderer
00596 
00597         } // end namespace
00598 } // End of namespace
00599 #endif



Page generated by Doxygen 1.7.3 for MRPT 0.9.4 SVN: at Sat Mar 26 06:16:28 UTC 2011