OcTreeDrawer.h
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 #ifndef OCTREEDRAWER_H_
00026 #define OCTREEDRAWER_H_
00027 
00028 #include "SceneObject.h"
00029 
00030 namespace octomap {
00031 
00032   class OcTreeDrawer: public octomap::SceneObject {
00033   public:
00034     OcTreeDrawer();
00035     virtual ~OcTreeDrawer();
00036     void clear();
00037 
00038     void draw() const;
00039 
00040     // initialization of drawer  -------------------------
00041 
00043     void setOcTree(const AbstractOcTree& octree){
00044       octomap::pose6d o; // initialized to (0,0,0) , (0,0,0,1) by default
00045       setOcTree(octree, o, 0);
00046     }
00047 
00050     virtual void setOcTree(const AbstractOcTree& octree, const octomap::pose6d& origin, int map_id_);
00051 
00052     // modification of existing drawer  ------------------
00053 
00055     void setOcTreeSelection(const std::list<octomap::OcTreeVolume>& selectedPoints);
00056 
00058     void clearOcTreeSelection();
00059 
00061     void setAlphaOccupied(double alpha);
00062 
00063     void enableOcTree(bool enabled = true);
00064     void enableOcTreeCells(bool enabled = true) {m_drawOccupied = enabled; };
00065     void enableFreespace(bool enabled = true) {m_drawFree = enabled; };
00066     void enableSelection(bool enabled = true) {m_drawSelection = enabled; };
00067     void setMax_tree_depth(unsigned int max_tree_depth) {m_max_tree_depth = max_tree_depth;};
00068 
00069     // set new origin (move object)
00070     void setOrigin(octomap::pose6d t);
00071     void enableAxes(bool enabled = true) { m_displayAxes = enabled; };
00072 
00073   protected:
00074     //void clearOcTree();
00075     void clearOcTreeStructure();
00076 
00077     void drawOctreeGrid() const;
00078     void drawOccupiedVoxels() const;
00079     void drawFreeVoxels() const;
00080     void drawSelection() const;
00081     void drawCubes(GLfloat** cubeArray, unsigned int cubeArraySize,
00082         GLfloat* cubeColorArray = NULL) const;
00083 
00084     void drawAxes() const;
00085 
00089     void generateCubes(const std::list<octomap::OcTreeVolume>& voxels,
00090                        GLfloat*** glArray, unsigned int& glArraySize, 
00091                        octomath::Pose6D& origin,
00092                        GLfloat** glColorArray = NULL);
00093     
00095     void clearCubes(GLfloat*** glArray, unsigned int& glArraySize,
00096                     GLfloat** glColorArray = NULL);
00098     void initGLArrays(const unsigned int& num_cubes, unsigned int& glArraySize,
00099                        GLfloat*** glArray, GLfloat** glColorArray);
00101     void initCubeTemplate(const octomath::Pose6D& origin,
00102                           std::vector<octomath::Vector3>& cube_template);
00104     unsigned int generateCube(const octomap::OcTreeVolume& v,
00105                               const std::vector<octomath::Vector3>& cube_template,
00106                               const unsigned int& current_array_idx,
00107                               GLfloat*** glArray);
00108     unsigned int setCubeColorHeightmap(const octomap::OcTreeVolume& v,
00109                                        const unsigned int& current_array_idx,
00110                                        GLfloat** glColorArray);
00111     unsigned int setCubeColorRGBA(const unsigned char& r, const unsigned char& g, 
00112                                   const unsigned char& b, const unsigned char& a,
00113                                   const unsigned int& current_array_idx,
00114                                   GLfloat** glColorArray);
00115       
00116 
00117     void initOctreeGridVis();
00118 
00120 
00121     GLfloat** m_occupiedThresArray;
00122     unsigned int m_occupiedThresSize;
00123     GLfloat** m_freeThresArray;
00124     unsigned int m_freeThresSize;
00125     GLfloat** m_occupiedArray;
00126     unsigned int m_occupiedSize;
00127     GLfloat** m_freeArray;
00128     unsigned int m_freeSize;
00129     GLfloat** m_selectionArray;
00130     unsigned int m_selectionSize;
00131 
00133     GLfloat* m_occupiedThresColorArray;
00134     GLfloat* m_occupiedColorArray;
00135 
00137     // TODO: put in its own drawer object!
00138     GLfloat* octree_grid_vertex_array;
00139     unsigned int octree_grid_vertex_size;
00140 
00141     std::list<octomap::OcTreeVolume> m_grid_voxels;
00142 
00143     bool m_drawOccupied;
00144     bool m_drawOcTreeGrid;
00145     bool m_drawFree;
00146     bool m_drawSelection;
00147     bool m_octree_grid_vis_initialized;
00148     bool m_displayAxes;
00149 
00150     unsigned int m_max_tree_depth;
00151     double m_alphaOccupied;
00152 
00153     octomap::pose6d origin;
00154     octomap::pose6d initial_origin;
00155 
00156     int map_id;
00157   };
00158 }
00159 
00160 #endif /* OCTREEDRAWER_H_ */


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