ColorOcTreeDrawer.cpp
Go to the documentation of this file.
00001 /*
00002  * This file is part of OctoMap - An Efficient Probabilistic 3D Mapping
00003  * Framework Based on Octrees
00004  * http://octomap.github.io
00005  *
00006  * Copyright (c) 2009-2014, K.M. Wurm and A. Hornung, University of Freiburg
00007  * All rights reserved. License for the viewer octovis: GNU GPL v2
00008  * http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt
00009  *
00010  *
00011  * This program is free software; you can redistribute it and/or modify
00012  * it under the terms of the GNU General Public License as published by
00013  * the Free Software Foundation; either version 2 of the License, or
00014  * (at your option) any later version.
00015  *
00016  * This program is distributed in the hope that it will be useful, but
00017  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
00018  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
00019  * for more details.
00020  *
00021  * You should have received a copy of the GNU General Public License
00022  * along with this program. If not, see http://www.gnu.org/licenses/.
00023  */
00024 
00025 #include <octovis/ColorOcTreeDrawer.h>
00026 
00027 namespace octomap {
00028 
00029   ColorOcTreeDrawer::ColorOcTreeDrawer() 
00030     : OcTreeDrawer() {
00031   }
00032 
00033   ColorOcTreeDrawer::~ColorOcTreeDrawer() {
00034   }
00035 
00036   void ColorOcTreeDrawer::setOcTree(const AbstractOcTree& tree_pnt,
00037                                     const octomap::pose6d& origin_,
00038                                     int map_id_) {
00039 
00040     const ColorOcTree& tree = ((const ColorOcTree&) tree_pnt);
00041 
00042     this->map_id = map_id_;
00043 
00044     // save origin used during cube generation
00045     this->initial_origin = octomap::pose6d(octomap::point3d(0,0,0), origin_.rot());
00046     // origin is in global coords
00047     this->origin = origin_;
00048     
00049     // maximum size to prevent crashes on large maps: (should be checked in a better way than a constant)
00050     bool showAll = (tree.size() < 5 * 1e6);
00051     bool uses_origin = ( (origin_.rot().x() != 0.) && (origin_.rot().y() != 0.)
00052                          && (origin_.rot().z() != 0.) && (origin_.rot().u() != 1.) );
00053 
00054     // walk the tree one to find the number of nodes in each category
00055     // (this is used to set up the OpenGL arrays)
00056     // TODO: this step may be left out, if we maintained the GLArrays in std::vectors instead...
00057     unsigned int cnt_occupied(0), cnt_occupied_thres(0), cnt_free(0), cnt_free_thres(0);
00058     for(ColorOcTree::tree_iterator it = tree.begin_tree(this->m_max_tree_depth),
00059           end=tree.end_tree(); it!= end; ++it) {
00060       if (it.isLeaf()) { 
00061         if (tree.isNodeOccupied(*it)){ // occupied voxels
00062           if (tree.isNodeAtThreshold(*it)) ++cnt_occupied_thres;
00063           else                               ++cnt_occupied;
00064         }
00065         else if (showAll) { // freespace voxels
00066           if (tree.isNodeAtThreshold(*it)) ++cnt_free_thres;
00067           else                               ++cnt_free;
00068         }
00069       }        
00070     }    
00071     // setup GL arrays for cube quads and cube colors
00072     initGLArrays(cnt_occupied      , m_occupiedSize     , &m_occupiedArray     , &m_occupiedColorArray);
00073     initGLArrays(cnt_occupied_thres, m_occupiedThresSize, &m_occupiedThresArray, &m_occupiedThresColorArray);
00074     initGLArrays(cnt_free          , m_freeSize         , &m_freeArray, NULL);
00075     initGLArrays(cnt_free_thres    , m_freeThresSize    , &m_freeThresArray, NULL);
00076 
00077     std::vector<octomath::Vector3> cube_template;
00078     initCubeTemplate(origin, cube_template);
00079 
00080     unsigned int idx_occupied(0), idx_occupied_thres(0), idx_free(0), idx_free_thres(0);
00081     unsigned int color_idx_occupied(0), color_idx_occupied_thres(0);
00082 
00083     m_grid_voxels.clear();
00084     OcTreeVolume voxel; // current voxel, possibly transformed 
00085     for(ColorOcTree::tree_iterator it = tree.begin_tree(this->m_max_tree_depth),
00086           end=tree.end_tree(); it!= end; ++it) {
00087 
00088       if (it.isLeaf()) { // voxels for leaf nodes
00089         if (uses_origin) 
00090           voxel = OcTreeVolume(origin.rot().rotate(it.getCoordinate()), it.getSize());
00091         else 
00092           voxel = OcTreeVolume(it.getCoordinate(), it.getSize());
00093         
00094         if (tree.isNodeOccupied(*it)){ // occupied voxels
00095           if (tree.isNodeAtThreshold(*it)) {
00096             idx_occupied_thres = generateCube(voxel, cube_template, idx_occupied_thres, &m_occupiedThresArray);
00097             color_idx_occupied_thres =  setCubeColorRGBA(it->getColor().r, it->getColor().g, it->getColor().b, 
00098                                                          (unsigned char) (it->getOccupancy() * 255.),
00099                                                          color_idx_occupied_thres, &m_occupiedThresColorArray);
00100           }
00101           else {
00102             idx_occupied = generateCube(voxel, cube_template, idx_occupied, &m_occupiedArray);
00103             color_idx_occupied = setCubeColorRGBA(it->getColor().r, it->getColor().g, it->getColor().b, 
00104                                                   (unsigned char)(it->getOccupancy() * 255.),
00105                                                   color_idx_occupied, &m_occupiedColorArray);
00106           }
00107         }
00108         else if (showAll) { // freespace voxels
00109           if (tree.isNodeAtThreshold(*it)) {
00110             idx_free_thres = generateCube(voxel, cube_template, idx_free_thres, &m_freeThresArray);
00111           }
00112           else {
00113             idx_free = generateCube(voxel, cube_template, idx_free, &m_freeArray);
00114           }
00115         }
00116 
00117         // grid structure voxel
00118         if (showAll) m_grid_voxels.push_back(voxel);        
00119       }
00120       
00121       else { // inner node voxels (for grid structure only)
00122         if (showAll) {
00123           if (uses_origin)
00124             voxel = OcTreeVolume(origin.rot().rotate(it.getCoordinate()), it.getSize());
00125           else
00126             voxel = OcTreeVolume(it.getCoordinate(), it.getSize());
00127           m_grid_voxels.push_back(voxel);
00128         }
00129       }      
00130     } // end for all voxels
00131 
00132     m_octree_grid_vis_initialized = false;
00133 
00134     if(m_drawOcTreeGrid)
00135       initOctreeGridVis();    
00136   }
00137 
00138 } // end namespace


octovis
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Thu Feb 11 2016 23:51:19