OcTreeDrawer.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/OcTreeDrawer.h>
00026 
00027 #define OTD_RAD2DEG 57.2957795
00028 
00029 namespace octomap {
00030 
00031   OcTreeDrawer::OcTreeDrawer() : SceneObject(),
00032                                  m_occupiedThresSize(0), m_freeThresSize(0),
00033                                  m_occupiedSize(0), m_freeSize(0), m_selectionSize(0),
00034                                  octree_grid_vertex_size(0), m_alphaOccupied(0.8), map_id(0)
00035   {
00036     m_octree_grid_vis_initialized = false;
00037     m_drawOccupied = true;
00038     m_drawOcTreeGrid = false;
00039     m_drawFree = false;
00040     m_drawSelection = true;
00041     m_displayAxes = false;
00042 
00043     m_occupiedArray = NULL;
00044     m_freeArray = NULL;
00045     m_occupiedThresArray = NULL;
00046     m_freeThresArray = NULL;
00047     m_occupiedColorArray = NULL;
00048     m_occupiedThresColorArray = NULL;
00049     m_selectionArray = NULL;
00050 
00051     // origin and movement
00052     initial_origin = octomap::pose6d(0,0,0,0,0,0);
00053     origin = initial_origin;
00054   }
00055 
00056   OcTreeDrawer::~OcTreeDrawer() {
00057     clear();
00058   }
00059 
00060   void OcTreeDrawer::draw() const {
00061 
00062     // push current status
00063     glPushMatrix();
00064     // octomap::pose6d relative_transform = origin * initial_origin.inv();
00065 
00066     octomap::pose6d relative_transform = origin;// * initial_origin;
00067 
00068     // apply relative transform
00069     const octomath::Quaternion& q = relative_transform.rot();
00070     glTranslatef(relative_transform.x(), relative_transform.y(), relative_transform.z());
00071     
00072     // convert quaternion to angle/axis notation
00073     float scale = sqrt(q.x() * q.x() + q.y() * q.y() + q.z() * q.z());
00074     if (scale) {
00075       float axis_x = q.x() / scale;
00076       float axis_y = q.y() / scale;
00077       float axis_z = q.z() / scale;
00078       float angle = acos(q.u()) * 2.0f * OTD_RAD2DEG;  //  opengl expects DEG
00079       glRotatef(angle, axis_x, axis_y, axis_z);
00080     }
00081 
00082     glEnableClientState(GL_VERTEX_ARRAY);
00083 
00084     if (m_drawOccupied)
00085       drawOccupiedVoxels();
00086     if (m_drawFree)
00087       drawFreeVoxels();
00088     if (m_drawOcTreeGrid)
00089       drawOctreeGrid();
00090     if (m_drawSelection)
00091       drawSelection();
00092 
00093     if (m_displayAxes) {
00094       drawAxes();
00095     }
00096 
00097     glDisableClientState(GL_VERTEX_ARRAY);
00098 
00099     // reset previous status
00100     glPopMatrix();
00101 
00102   }
00103 
00104   void OcTreeDrawer::setAlphaOccupied(double alpha){
00105     m_alphaOccupied = alpha;
00106   }
00107 
00108 
00109   void OcTreeDrawer::setOcTree(const AbstractOcTree& tree, const pose6d& origin, int map_id_) {
00110       
00111     const OcTree& octree = (const OcTree&) tree;
00112     this->map_id = map_id_;
00113 
00114     // save origin used during cube generation
00115     this->initial_origin = octomap::pose6d(octomap::point3d(0,0,0), origin.rot());
00116 
00117     // origin is in global coords
00118     this->origin = origin;
00119     
00120     // maximum size to prevent crashes on large maps: (should be checked in a better way than a constant)
00121     bool showAll = (octree.size() < 5 * 1e6);
00122     bool uses_origin = ( (origin.rot().x() != 0.) && (origin.rot().y() != 0.)
00123         && (origin.rot().z() != 0.) && (origin.rot().u() != 1.) );
00124 
00125     // walk the tree one to find the number of nodes in each category
00126     // (this is used to set up the OpenGL arrays)
00127     // TODO: this step may be left out, if we maintained the GLArrays in std::vectors instead...
00128     unsigned int cnt_occupied(0), cnt_occupied_thres(0), cnt_free(0), cnt_free_thres(0);
00129     for(OcTree::tree_iterator it = octree.begin_tree(this->m_max_tree_depth),
00130             end=octree.end_tree(); it!= end; ++it) {
00131       if (it.isLeaf()) { 
00132         if (octree.isNodeOccupied(*it)){ // occupied voxels
00133           if (octree.isNodeAtThreshold(*it)) ++cnt_occupied_thres;
00134           else                               ++cnt_occupied;
00135         }
00136         else if (showAll) { // freespace voxels
00137           if (octree.isNodeAtThreshold(*it)) ++cnt_free_thres;
00138           else                               ++cnt_free;
00139         }
00140       }        
00141     }    
00142     // setup GL arrays for cube quads and cube colors
00143     initGLArrays(cnt_occupied      , m_occupiedSize     , &m_occupiedArray     , &m_occupiedColorArray);
00144     initGLArrays(cnt_occupied_thres, m_occupiedThresSize, &m_occupiedThresArray, &m_occupiedThresColorArray);
00145     initGLArrays(cnt_free          , m_freeSize         , &m_freeArray, NULL);
00146     initGLArrays(cnt_free_thres    , m_freeThresSize    , &m_freeThresArray, NULL);
00147 
00148     double minX, minY, minZ, maxX, maxY, maxZ;
00149     octree.getMetricMin(minX, minY, minZ);
00150     octree.getMetricMax(maxX, maxY, maxZ);
00151 
00152     // set min/max Z for color height map
00153     m_zMin = minZ;
00154     m_zMax = maxZ;
00155 
00156     std::vector<octomath::Vector3> cube_template;
00157     initCubeTemplate(origin, cube_template);
00158 
00159     unsigned int idx_occupied(0), idx_occupied_thres(0), idx_free(0), idx_free_thres(0);
00160     unsigned int color_idx_occupied(0), color_idx_occupied_thres(0);
00161 
00162     m_grid_voxels.clear();
00163     OcTreeVolume voxel; // current voxel, possibly transformed 
00164     for(OcTree::tree_iterator it = octree.begin_tree(this->m_max_tree_depth),
00165             end=octree.end_tree(); it!= end; ++it) {
00166 
00167       if (it.isLeaf()) { // voxels for leaf nodes
00168         if (uses_origin) 
00169           voxel = OcTreeVolume(origin.rot().rotate(it.getCoordinate()), it.getSize());
00170         else 
00171           voxel = OcTreeVolume(it.getCoordinate(), it.getSize());
00172         
00173         if (octree.isNodeOccupied(*it)){ // occupied voxels
00174           if (octree.isNodeAtThreshold(*it)) {
00175             idx_occupied_thres = generateCube(voxel, cube_template, idx_occupied_thres, &m_occupiedThresArray);
00176             color_idx_occupied_thres = setCubeColorHeightmap(voxel, color_idx_occupied_thres, &m_occupiedThresColorArray);
00177           }
00178           else {
00179             idx_occupied = generateCube(voxel, cube_template, idx_occupied, &m_occupiedArray);
00180             color_idx_occupied = setCubeColorHeightmap(voxel, color_idx_occupied, &m_occupiedColorArray);
00181           }
00182         }
00183         else if (showAll) { // freespace voxels
00184           if (octree.isNodeAtThreshold(*it)) {
00185             idx_free_thres = generateCube(voxel, cube_template, idx_free_thres, &m_freeThresArray);
00186           }
00187           else {
00188             idx_free = generateCube(voxel, cube_template, idx_free, &m_freeArray);
00189           }
00190         }
00191       }
00192       
00193       else { // inner node voxels (for grid structure only)
00194         if (showAll) {
00195           if (uses_origin)
00196             voxel = OcTreeVolume(origin.rot().rotate(it.getCoordinate()), it.getSize());
00197           else
00198             voxel = OcTreeVolume(it.getCoordinate(), it.getSize());
00199           m_grid_voxels.push_back(voxel);
00200         }
00201       }      
00202     } // end for all voxels
00203 
00204     m_octree_grid_vis_initialized = false;
00205 
00206     if(m_drawOcTreeGrid)
00207       initOctreeGridVis();
00208   }
00209 
00210   void OcTreeDrawer::setOcTreeSelection(const std::list<octomap::OcTreeVolume>& selectedVoxels){
00211     generateCubes(selectedVoxels, &m_selectionArray, m_selectionSize, this->origin);
00212   }
00213 
00214   void OcTreeDrawer::clearOcTreeSelection(){
00215     clearCubes(&m_selectionArray, m_selectionSize);
00216   }
00217 
00218   void OcTreeDrawer::initGLArrays(const unsigned int& num_cubes,
00219                                   unsigned int& glArraySize,
00220                                   GLfloat*** glArray, GLfloat** glColorArray) {
00221 
00222     clearCubes(glArray, glArraySize, glColorArray);
00223 
00224     // store size of GL arrays for drawing
00225     glArraySize = num_cubes * 4 * 3;
00226 
00227     // allocate cube arrays, 6 quads per cube
00228     *glArray = new GLfloat* [6];
00229     for (unsigned i = 0; i<6; ++i){
00230       (*glArray)[i] = new GLfloat[glArraySize];
00231     }
00232     // setup quad color array, if given
00233     if (glColorArray != NULL)
00234       *glColorArray = new GLfloat[glArraySize * 4 *4];
00235   }
00236 
00237   void OcTreeDrawer::initCubeTemplate(const octomath::Pose6D& origin,
00238                                       std::vector<octomath::Vector3>& cube_template) {
00239     cube_template.clear();
00240     cube_template.reserve(24);
00241 
00242     cube_template.push_back(octomath::Vector3( 1, 1,-1));
00243     cube_template.push_back(octomath::Vector3( 1,-1,-1));
00244     cube_template.push_back(octomath::Vector3( 1, 1,-1));
00245     cube_template.push_back(octomath::Vector3(-1, 1,-1));
00246     cube_template.push_back(octomath::Vector3( 1, 1,-1));
00247     cube_template.push_back(octomath::Vector3( 1, 1, 1));
00248 
00249     cube_template.push_back(octomath::Vector3(-1, 1,-1));
00250     cube_template.push_back(octomath::Vector3(-1,-1,-1));
00251     cube_template.push_back(octomath::Vector3( 1, 1, 1));
00252     cube_template.push_back(octomath::Vector3(-1, 1, 1));
00253     cube_template.push_back(octomath::Vector3( 1,-1,-1));
00254     cube_template.push_back(octomath::Vector3( 1,-1, 1));
00255 
00256     cube_template.push_back(octomath::Vector3(-1, 1, 1));
00257     cube_template.push_back(octomath::Vector3(-1,-1, 1));
00258     cube_template.push_back(octomath::Vector3( 1,-1, 1));
00259     cube_template.push_back(octomath::Vector3(-1,-1, 1));
00260     cube_template.push_back(octomath::Vector3(-1,-1,-1));
00261     cube_template.push_back(octomath::Vector3(-1,-1, 1));
00262 
00263     cube_template.push_back(octomath::Vector3( 1, 1, 1));
00264     cube_template.push_back(octomath::Vector3( 1,-1, 1));
00265     cube_template.push_back(octomath::Vector3( 1,-1,-1));
00266     cube_template.push_back(octomath::Vector3(-1,-1,-1));
00267     cube_template.push_back(octomath::Vector3(-1, 1,-1));
00268     cube_template.push_back(octomath::Vector3(-1, 1, 1));
00269   }
00270 
00271   unsigned int OcTreeDrawer::generateCube(const octomap::OcTreeVolume& v,
00272                                           const std::vector<octomath::Vector3>& cube_template,
00273                                           const unsigned int& current_array_idx,
00274                                           GLfloat*** glArray) {
00275 
00276     // epsilon to be substracted from cube size so that neighboring planes don't overlap
00277     // seems to introduce strange artifacts nevertheless...
00278     double eps = 1e-5;
00279     
00280     octomath::Vector3 p;
00281 
00282     double half_cube_size = GLfloat(v.second /2.0 -eps);
00283     unsigned int i = current_array_idx;
00284     // Cube surfaces are in gl_array in order: back, front, top, down, left, right.
00285     // Arrays are filled in parallel (increasing i for all at once)
00286     // One color array for all surfaces is filled when requested
00287 
00288     p = v.first + cube_template[0] * half_cube_size;
00289     (*glArray)[0][i]   = p.x();
00290     (*glArray)[0][i+1] = p.y();
00291     (*glArray)[0][i+2] = p.z();
00292 
00293     p = v.first + cube_template[1] * half_cube_size;
00294     (*glArray)[1][i]   = p.x();
00295     (*glArray)[1][i+1] = p.y();
00296     (*glArray)[1][i+2] = p.z();
00297 
00298     p = v.first + cube_template[2] * half_cube_size;
00299     (*glArray)[2][i]   = p.x();
00300     (*glArray)[2][i+1] = p.y();
00301     (*glArray)[2][i+2] = p.z();
00302 
00303     p = v.first + cube_template[3] * half_cube_size;
00304     (*glArray)[3][i]   = p.x(); 
00305     (*glArray)[3][i+1] = p.y(); 
00306     (*glArray)[3][i+2] = p.z(); 
00307 
00308     p = v.first + cube_template[4] * half_cube_size;
00309     (*glArray)[4][i]   = p.x(); 
00310     (*glArray)[4][i+1] = p.y(); 
00311     (*glArray)[4][i+2] = p.z(); 
00312 
00313     p = v.first + cube_template[5] * half_cube_size;
00314     (*glArray)[5][i]   = p.x(); 
00315     (*glArray)[5][i+1] = p.y(); 
00316     (*glArray)[5][i+2] = p.z(); 
00317     i+= 3;  //-------------------
00318 
00319     p = v.first + cube_template[6] * half_cube_size;
00320     (*glArray)[0][i]   = p.x();
00321     (*glArray)[0][i+1] = p.y();
00322     (*glArray)[0][i+2] = p.z();
00323 
00324     p = v.first + cube_template[7] * half_cube_size;
00325     (*glArray)[1][i]   = p.x();
00326     (*glArray)[1][i+1] = p.y();
00327     (*glArray)[1][i+2] = p.z();
00328 
00329     p = v.first + cube_template[8] * half_cube_size;
00330     (*glArray)[2][i]   = p.x();
00331     (*glArray)[2][i+1] = p.y();
00332     (*glArray)[2][i+2] = p.z();
00333 
00334     p = v.first + cube_template[9] * half_cube_size;
00335     (*glArray)[3][i]   = p.x();
00336     (*glArray)[3][i+1] = p.y();
00337     (*glArray)[3][i+2] = p.z();
00338 
00339     p = v.first + cube_template[10] * half_cube_size;
00340     (*glArray)[4][i]   = p.x();
00341     (*glArray)[4][i+1] = p.y();
00342     (*glArray)[4][i+2] = p.z();
00343 
00344     p = v.first + cube_template[11] * half_cube_size;
00345     (*glArray)[5][i]   = p.x();
00346     (*glArray)[5][i+1] = p.y();
00347     (*glArray)[5][i+2] = p.z();
00348     i+= 3;  //-------------------
00349 
00350     p = v.first + cube_template[12] * half_cube_size;
00351     (*glArray)[0][i]   = p.x();
00352     (*glArray)[0][i+1] = p.y();
00353     (*glArray)[0][i+2] = p.z();
00354 
00355     p = v.first + cube_template[13] * half_cube_size;
00356     (*glArray)[1][i]   = p.x();
00357     (*glArray)[1][i+1] = p.y();
00358     (*glArray)[1][i+2] = p.z();
00359 
00360     p = v.first + cube_template[14] * half_cube_size;
00361     (*glArray)[2][i]   = p.x();
00362     (*glArray)[2][i+1] = p.y();
00363     (*glArray)[2][i+2] = p.z();
00364 
00365     p = v.first + cube_template[15] * half_cube_size;
00366     (*glArray)[3][i]   = p.x();
00367     (*glArray)[3][i+1] = p.y();
00368     (*glArray)[3][i+2] = p.z();
00369 
00370     p = v.first + cube_template[16] * half_cube_size;
00371     (*glArray)[4][i]   = p.x();
00372     (*glArray)[4][i+1] = p.y();
00373     (*glArray)[4][i+2] = p.z();
00374 
00375     p = v.first + cube_template[17] * half_cube_size;
00376     (*glArray)[5][i]   = p.x();
00377     (*glArray)[5][i+1] = p.y();
00378     (*glArray)[5][i+2] = p.z();
00379     i+= 3;  //-------------------
00380 
00381     p = v.first + cube_template[18] * half_cube_size;
00382     (*glArray)[0][i]   = p.x();
00383     (*glArray)[0][i+1] = p.y();
00384     (*glArray)[0][i+2] = p.z();
00385 
00386     p = v.first + cube_template[19] * half_cube_size;
00387     (*glArray)[1][i]   = p.x();
00388     (*glArray)[1][i+1] = p.y();
00389     (*glArray)[1][i+2] = p.z();
00390 
00391     p = v.first + cube_template[20] * half_cube_size;
00392     (*glArray)[2][i]   = p.x();
00393     (*glArray)[2][i+1] = p.y();
00394     (*glArray)[2][i+2] = p.z();
00395 
00396     p = v.first + cube_template[21] * half_cube_size;
00397     (*glArray)[3][i]   = p.x();
00398     (*glArray)[3][i+1] = p.y();
00399     (*glArray)[3][i+2] = p.z();
00400 
00401     p = v.first + cube_template[22] * half_cube_size;
00402     (*glArray)[4][i]   = p.x();
00403     (*glArray)[4][i+1] = p.y();
00404     (*glArray)[4][i+2] = p.z();
00405 
00406     p = v.first + cube_template[23] * half_cube_size;
00407     (*glArray)[5][i]   = p.x();
00408     (*glArray)[5][i+1] = p.y();
00409     (*glArray)[5][i+2] = p.z();
00410     i += 3;  //-------------------
00411     
00412     return i; // updated array idx
00413   }
00414 
00415 
00416   unsigned int OcTreeDrawer::setCubeColorHeightmap(const octomap::OcTreeVolume& v,
00417                                           const unsigned int& current_array_idx,
00418                                           GLfloat** glColorArray) {
00419 
00420     if (glColorArray == NULL) return current_array_idx;
00421 
00422     unsigned int colorIdx = current_array_idx;
00423     // color for all 4 vertices (same height)
00424     for (int k = 0; k < 4; ++k) {
00425       if (m_colorMode == CM_GRAY_HEIGHT)
00426         SceneObject::heightMapGray(v.first.z(), *glColorArray + colorIdx);  // sets r,g,b
00427       else
00428         SceneObject::heightMapColor(v.first.z(), *glColorArray + colorIdx);   // sets r,g,b
00429       // set Alpha value:
00430       (*glColorArray)[colorIdx + 3] = m_alphaOccupied;
00431       colorIdx += 4;
00432     }  
00433     return colorIdx;
00434   }
00435 
00436   unsigned int OcTreeDrawer::setCubeColorRGBA(const unsigned char& r,
00437                                               const unsigned char& g,
00438                                               const unsigned char& b,
00439                                               const unsigned char& a,
00440                                               const unsigned int& current_array_idx,
00441                                               GLfloat** glColorArray) {
00442 
00443     if (glColorArray == NULL) return current_array_idx;
00444     unsigned int colorIdx = current_array_idx;
00445     // set color for next 4 vertices (=one quad)
00446     for (int k = 0; k < 4; ++k) {
00447       (*glColorArray)[colorIdx    ] = (double) r/255.;
00448       (*glColorArray)[colorIdx + 1] = (double) g/255.;
00449       (*glColorArray)[colorIdx + 2] = (double) b/255.;
00450       (*glColorArray)[colorIdx + 3] = (double) a/255.;
00451       colorIdx += 4;
00452     }  
00453     return colorIdx;
00454   }
00455 
00456 
00457   void OcTreeDrawer::clearCubes(GLfloat*** glArray,
00458                                 unsigned int& glArraySize,
00459                                 GLfloat** glColorArray) {
00460     if (glArraySize != 0) {
00461       for (unsigned i = 0; i < 6; ++i) {
00462         delete[] (*glArray)[i];
00463       }
00464       delete[] *glArray;
00465       *glArray = NULL;
00466       glArraySize = 0;
00467     }
00468     if (glColorArray != NULL && *glColorArray != NULL) {
00469       delete[] *glColorArray;
00470       *glColorArray = NULL;
00471     }
00472   }
00473 
00474 
00475   // still used for "selection" nodes
00476   void OcTreeDrawer::generateCubes(const std::list<octomap::OcTreeVolume>& voxels,
00477                                    GLfloat*** glArray, unsigned int& glArraySize,
00478                                    octomath::Pose6D& origin,
00479                                    GLfloat** glColorArray) {
00480     unsigned int i = 0;
00481     unsigned int colorIdx = 0;
00482 
00483     std::vector<octomath::Vector3> cube_template;
00484     initCubeTemplate(origin, cube_template);
00485 
00486     for (std::list<octomap::OcTreeVolume>::const_iterator it=voxels.begin(); 
00487          it != voxels.end(); it++) {
00488       i = generateCube(*it, cube_template, i, glArray);
00489     }
00490 
00491     if (glColorArray != NULL) {
00492       for (std::list<octomap::OcTreeVolume>::const_iterator it=voxels.begin(); 
00493            it != voxels.end(); it++) {
00494         colorIdx = setCubeColorHeightmap(*it, colorIdx, glColorArray);
00495       }
00496     }
00497   }
00498 
00499   void OcTreeDrawer::initOctreeGridVis() {
00500 
00501     if (m_octree_grid_vis_initialized) return;
00502 
00503     clearOcTreeStructure();
00504     // allocate arrays for octree grid visualization
00505     octree_grid_vertex_size = m_grid_voxels.size() * 12 * 2 * 3;
00506     octree_grid_vertex_array = new GLfloat[octree_grid_vertex_size];
00507 
00508     // generate the cubes, 12 lines each
00509     std::list<octomap::OcTreeVolume>::iterator it_rec;
00510     unsigned int i = 0;
00511     double x,y,z;
00512     for (it_rec=m_grid_voxels.begin(); it_rec != m_grid_voxels.end(); it_rec++) {
00513 
00514       x = it_rec->first.x();
00515       y = it_rec->first.y();
00516       z = it_rec->first.z();
00517 
00518       double half_voxel_size = it_rec->second / 2.0;
00519 
00520       // ----
00521       octree_grid_vertex_array[i]   = x + half_voxel_size;
00522       octree_grid_vertex_array[i+1] = y + half_voxel_size;
00523       octree_grid_vertex_array[i+2] = z - half_voxel_size;
00524       i+= 3;
00525       octree_grid_vertex_array[i]   = x - half_voxel_size;
00526       octree_grid_vertex_array[i+1] = y + half_voxel_size;
00527       octree_grid_vertex_array[i+2] = z - half_voxel_size;
00528       i+= 3;
00529       octree_grid_vertex_array[i]   = x - half_voxel_size;
00530       octree_grid_vertex_array[i+1] = y + half_voxel_size;
00531       octree_grid_vertex_array[i+2] = z - half_voxel_size;
00532       i += 3;
00533       octree_grid_vertex_array[i]   = x - half_voxel_size;
00534       octree_grid_vertex_array[i+1] = y + half_voxel_size;
00535       octree_grid_vertex_array[i+2] = z + half_voxel_size;
00536       i+= 3;
00537       octree_grid_vertex_array[i]   = x - half_voxel_size;
00538       octree_grid_vertex_array[i+1] = y + half_voxel_size;
00539       octree_grid_vertex_array[i+2] = z + half_voxel_size;
00540       i+= 3;
00541       octree_grid_vertex_array[i]   = x + half_voxel_size;
00542       octree_grid_vertex_array[i+1] = y + half_voxel_size;
00543       octree_grid_vertex_array[i+2] = z + half_voxel_size;
00544       i+= 3;
00545       octree_grid_vertex_array[i]   = x + half_voxel_size;
00546       octree_grid_vertex_array[i+1] = y + half_voxel_size;
00547       octree_grid_vertex_array[i+2] = z + half_voxel_size;
00548       i+= 3;
00549       octree_grid_vertex_array[i]   = x + half_voxel_size;
00550       octree_grid_vertex_array[i+1] = y + half_voxel_size;
00551       octree_grid_vertex_array[i+2] = z - half_voxel_size;
00552       i+= 3;
00553       // ----
00554       octree_grid_vertex_array[i]   = x + half_voxel_size;
00555       octree_grid_vertex_array[i+1] = y - half_voxel_size;
00556       octree_grid_vertex_array[i+2] = z + half_voxel_size;
00557       i+= 3;
00558       octree_grid_vertex_array[i]   = x - half_voxel_size;
00559       octree_grid_vertex_array[i+1] = y - half_voxel_size;
00560       octree_grid_vertex_array[i+2] = z + half_voxel_size;
00561       i+= 3;
00562       octree_grid_vertex_array[i]   = x - half_voxel_size;
00563       octree_grid_vertex_array[i+1] = y - half_voxel_size;
00564       octree_grid_vertex_array[i+2] = z + half_voxel_size;
00565       i+= 3;
00566       octree_grid_vertex_array[i]   = x - half_voxel_size;
00567       octree_grid_vertex_array[i+1] = y - half_voxel_size;
00568       octree_grid_vertex_array[i+2] = z - half_voxel_size;
00569       i+= 3;
00570       octree_grid_vertex_array[i]   = x - half_voxel_size;
00571       octree_grid_vertex_array[i+1] = y - half_voxel_size;
00572       octree_grid_vertex_array[i+2] = z - half_voxel_size;
00573       i+= 3;
00574       octree_grid_vertex_array[i]   = x + half_voxel_size;
00575       octree_grid_vertex_array[i+1] = y - half_voxel_size;
00576       octree_grid_vertex_array[i+2] = z - half_voxel_size;
00577       i+= 3;
00578       octree_grid_vertex_array[i]   = x + half_voxel_size;
00579       octree_grid_vertex_array[i+1] = y - half_voxel_size;
00580       octree_grid_vertex_array[i+2] = z - half_voxel_size;
00581       i+= 3;
00582       octree_grid_vertex_array[i]   = x + half_voxel_size;
00583       octree_grid_vertex_array[i+1] = y - half_voxel_size;
00584       octree_grid_vertex_array[i+2] = z + half_voxel_size;
00585       i+= 3;
00586       // ----
00587       octree_grid_vertex_array[i]   = x + half_voxel_size;
00588       octree_grid_vertex_array[i+1] = y + half_voxel_size;
00589       octree_grid_vertex_array[i+2] = z - half_voxel_size;
00590       i+= 3;
00591       octree_grid_vertex_array[i]   = x + half_voxel_size;
00592       octree_grid_vertex_array[i+1] = y - half_voxel_size;
00593       octree_grid_vertex_array[i+2] = z - half_voxel_size;
00594       i+= 3;
00595       octree_grid_vertex_array[i]   = x - half_voxel_size;
00596       octree_grid_vertex_array[i+1] = y + half_voxel_size;
00597       octree_grid_vertex_array[i+2] = z - half_voxel_size;
00598       i+= 3;
00599       octree_grid_vertex_array[i]   = x - half_voxel_size;
00600       octree_grid_vertex_array[i+1] = y - half_voxel_size;
00601       octree_grid_vertex_array[i+2] = z - half_voxel_size;
00602       i+= 3;
00603       octree_grid_vertex_array[i]   = x - half_voxel_size;
00604       octree_grid_vertex_array[i+1] = y + half_voxel_size;
00605       octree_grid_vertex_array[i+2] = z + half_voxel_size;
00606       i+= 3;
00607       octree_grid_vertex_array[i]   = x - half_voxel_size;
00608       octree_grid_vertex_array[i+1] = y - half_voxel_size;
00609       octree_grid_vertex_array[i+2] = z + half_voxel_size;
00610       i+= 3;
00611       octree_grid_vertex_array[i]   = x + half_voxel_size;
00612       octree_grid_vertex_array[i+1] = y + half_voxel_size;
00613       octree_grid_vertex_array[i+2] = z + half_voxel_size;
00614       i+= 3;
00615       octree_grid_vertex_array[i]   = x + half_voxel_size;
00616       octree_grid_vertex_array[i+1] = y - half_voxel_size;
00617       octree_grid_vertex_array[i+2] = z + half_voxel_size;
00618       i+= 3;
00619       // ----
00620     }
00621     m_octree_grid_vis_initialized = true;
00622   }
00623 
00624   void OcTreeDrawer::clearOcTreeStructure(){
00625     if (octree_grid_vertex_size != 0) {
00626       delete[] octree_grid_vertex_array;
00627       octree_grid_vertex_size = 0;
00628     }
00629     m_octree_grid_vis_initialized = false;
00630   }
00631 
00632   void OcTreeDrawer::clear() {
00633     //clearOcTree();
00634     clearCubes(&m_occupiedArray, m_occupiedSize, &m_occupiedColorArray);
00635     clearCubes(&m_occupiedThresArray, m_occupiedThresSize, &m_occupiedThresColorArray);
00636     clearCubes(&m_freeArray, m_freeSize);
00637     clearCubes(&m_freeThresArray, m_freeThresSize);
00638     clearCubes(&m_selectionArray, m_selectionSize);
00639     clearOcTreeStructure();
00640   }
00641 
00642 
00643   void OcTreeDrawer::drawOccupiedVoxels() const {
00644 
00645     if (m_colorMode == CM_SEMANTIC) {
00646       // hardcoded mapping id -> colors
00647       if (this->map_id == 0) {  // background
00648         glColor3f(0.784f, 0.66f, 0); // gold
00649       }
00650       else if (this->map_id == 1) {  // table
00651         glColor3f(0.68f, 0., 0.62f); // purple
00652       }
00653       else { // object
00654         glColor3f(0., 0.784f, 0.725f); // cyan
00655       }
00656       drawCubes(m_occupiedThresArray, m_occupiedThresSize, m_occupiedThresColorArray);
00657     }
00658     else {      
00659       // colors for printout mode:
00660       if (m_colorMode == CM_PRINTOUT) {
00661         if (!m_drawFree) { // gray on white background
00662           glColor3f(0.6f, 0.6f, 0.6f);
00663         }
00664         else {
00665           glColor3f(0.1f, 0.1f, 0.1f);
00666         }  
00667       }
00668 
00669       // draw binary occupied cells
00670       if (m_occupiedThresSize != 0) {
00671         if (m_colorMode != CM_PRINTOUT) glColor4f(0.0f, 0.0f, 1.0f, m_alphaOccupied);
00672         drawCubes(m_occupiedThresArray, m_occupiedThresSize, m_occupiedThresColorArray);
00673       }
00674 
00675       // draw delta occupied cells
00676       if (m_occupiedSize != 0) {
00677         if (m_colorMode != CM_PRINTOUT) glColor4f(0.2f, 0.7f, 1.0f, m_alphaOccupied);
00678         drawCubes(m_occupiedArray, m_occupiedSize, m_occupiedColorArray);
00679       }
00680     }
00681   }
00682 
00683 
00684   void OcTreeDrawer::drawFreeVoxels() const {
00685 
00686     if (m_colorMode == CM_PRINTOUT) {
00687       if (!m_drawOccupied) { // gray on white background
00688         glColor3f(0.5f, 0.5f, 0.5f);
00689       }
00690       else {
00691         glColor3f(0.9f, 0.9f, 0.9f);
00692       }
00693     }
00694 
00695     // draw binary freespace cells
00696     if (m_freeThresSize != 0) {
00697       if (m_colorMode != CM_PRINTOUT) glColor4f(0.0f, 1.0f, 0.0f, 0.3f);
00698       drawCubes(m_freeThresArray, m_freeThresSize);
00699     }
00700 
00701     // draw delta freespace cells
00702     if (m_freeSize != 0) {
00703       if (m_colorMode != CM_PRINTOUT) glColor4f(0.5f, 1.0f, 0.1f, 0.3f);
00704       drawCubes(m_freeArray, m_freeSize);
00705     }
00706   }
00707 
00708   void OcTreeDrawer::drawSelection() const {
00709     if (m_selectionSize != 0) {
00710       glColor4f(1.0, 0.0, 0.0, 0.5);
00711       drawCubes(m_selectionArray, m_selectionSize);
00712     }
00713   }
00714 
00715   void OcTreeDrawer::drawCubes(GLfloat** cubeArray, unsigned int cubeArraySize,
00716                                GLfloat* cubeColorArray) const {
00717     if (cubeArraySize == 0 || cubeArray == NULL){
00718       std::cerr << "Warning: GLfloat array to draw cubes appears to be empty, nothing drawn.\n";
00719       return;
00720     }
00721 
00722     // save current color
00723     GLfloat* curcol = new GLfloat[4];
00724     glGetFloatv(GL_CURRENT_COLOR, curcol);
00725 
00726     // enable color pointer when heightColorMode is enabled:
00727 
00728     if ((m_colorMode == CM_COLOR_HEIGHT || m_colorMode == CM_GRAY_HEIGHT) && (cubeColorArray != NULL)){
00729       glEnableClientState(GL_COLOR_ARRAY);
00730       glColorPointer(4, GL_FLOAT, 0, cubeColorArray);
00731     }
00732 
00733     // top surfaces:
00734     glNormal3f(0.0f, 1.0f, 0.0f);
00735     glVertexPointer(3, GL_FLOAT, 0, cubeArray[0]);
00736     glDrawArrays(GL_QUADS, 0, cubeArraySize / 3);
00737     // bottom surfaces:
00738     glNormal3f(0.0f, -1.0f, 0.0f);
00739     glVertexPointer(3, GL_FLOAT, 0, cubeArray[1]);
00740     glDrawArrays(GL_QUADS, 0, cubeArraySize / 3);
00741     // right surfaces:
00742     glNormal3f(1.0f, 0.0f, 0.0f);
00743     glVertexPointer(3, GL_FLOAT, 0, cubeArray[2]);
00744     glDrawArrays(GL_QUADS, 0, cubeArraySize / 3);
00745     // left surfaces:
00746     glNormal3f(-1.0f, 0.0f, 0.0f);
00747     glVertexPointer(3, GL_FLOAT, 0, cubeArray[3]);
00748     glDrawArrays(GL_QUADS, 0, cubeArraySize / 3);
00749     // back surfaces:
00750     glNormal3f(0.0f, 0.0f, -1.0f);
00751     glVertexPointer(3, GL_FLOAT, 0, cubeArray[4]);
00752     glDrawArrays(GL_QUADS, 0, cubeArraySize / 3);
00753     // front surfaces:
00754     glNormal3f(0.0f, 0.0f, 1.0f);
00755     glVertexPointer(3, GL_FLOAT, 0, cubeArray[5]);
00756     glDrawArrays(GL_QUADS, 0, cubeArraySize / 3);
00757 
00758     if ((m_colorMode == CM_COLOR_HEIGHT || m_colorMode == CM_GRAY_HEIGHT)
00759         && (cubeColorArray != NULL)){
00760       glDisableClientState(GL_COLOR_ARRAY);
00761     }
00762 
00763     // draw bounding linies of cubes in printout:
00764     if (m_colorMode == CM_PRINTOUT){
00765       glDisable(GL_LIGHTING);
00766       glHint (GL_LINE_SMOOTH_HINT, GL_NICEST);
00767       glEnable (GL_LINE_SMOOTH);
00768       glPolygonMode (GL_FRONT_AND_BACK, GL_LINE);   // Draw Polygons only as Wireframes
00769       glLineWidth(2.0f);
00770       glColor3f(0.0f, 0.0f, 0.0f);
00771       glCullFace(GL_FRONT_AND_BACK);        // Don't draw any Polygons faces
00772       //glDepthFunc (GL_LEQUAL);
00773 
00774       // top meshes:
00775       glNormal3f(0.0f, 1.0f, 0.0f);
00776       glVertexPointer(3, GL_FLOAT, 0, cubeArray[0]);
00777       glDrawArrays(GL_QUADS, 0, cubeArraySize / 3);
00778       // bottom meshes:
00779       glNormal3f(0.0f, -1.0f, 0.0f);
00780       glVertexPointer(3, GL_FLOAT, 0, cubeArray[1]);
00781       glDrawArrays(GL_QUADS, 0, cubeArraySize / 3);
00782       // right meshes:
00783       glNormal3f(1.0f, 0.0f, 0.0f);
00784       glVertexPointer(3, GL_FLOAT, 0, cubeArray[2]);
00785       glDrawArrays(GL_QUADS, 0, cubeArraySize / 3);
00786       // left meshes:
00787       glNormal3f(-1.0f, 0.0f, 0.0f);
00788       glVertexPointer(3, GL_FLOAT, 0, cubeArray[3]);
00789       glDrawArrays(GL_QUADS, 0, cubeArraySize / 3);
00790 
00791       // restore defaults:
00792       glCullFace(GL_BACK);
00793       //glDepthFunc(GL_LESS);
00794       glDisable(GL_LINE_SMOOTH);
00795       glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
00796       glEnable(GL_LIGHTING);
00797     }
00798     // reset color
00799     glColor4fv(curcol);
00800     delete[] curcol;
00801   }
00802 
00803   void OcTreeDrawer::drawOctreeGrid() const {
00804     if (!m_octree_grid_vis_initialized) return;
00805     if (octree_grid_vertex_size == 0)   return;
00806 
00807     glDisable(GL_LIGHTING);
00808     glEnable(GL_LINE_SMOOTH);
00809 
00810     glLineWidth(1.);
00811     glVertexPointer(3, GL_FLOAT, 0, octree_grid_vertex_array);
00812     glColor3f(0.0, 0.0, 0.0);
00813     glDrawArrays(GL_LINES, 0, octree_grid_vertex_size / 3);
00814 
00815     glDisable(GL_LINE_SMOOTH);
00816     glEnable(GL_LIGHTING);
00817   }
00818 
00819   void OcTreeDrawer::enableOcTree(bool enabled) {
00820     m_drawOcTreeGrid = enabled;
00821     if(m_drawOcTreeGrid && !m_octree_grid_vis_initialized) {
00822       initOctreeGridVis();
00823     }
00824   }
00825 
00826 
00827   void OcTreeDrawer::setOrigin(octomap::pose6d t){
00828     origin = t;  
00829     std::cout << "OcTreeDrawer: setting new global origin: " << t << std::endl;
00830 
00831     octomap::pose6d relative_transform = origin * initial_origin.inv();
00832 
00833     std::cout << "origin        : " << origin << std::endl;
00834     std::cout << "inv init orig : " << initial_origin.inv() << std::endl;
00835     std::cout << "relative trans: " << relative_transform << std::endl;
00836   }
00837 
00838   void OcTreeDrawer::drawAxes() const {
00839 
00840     octomap::pose6d relative_transform = origin * initial_origin.inv();
00841 
00842     glPushMatrix();
00843 
00844     float length = 0.15f; 
00845 
00846     GLboolean lighting, colorMaterial;
00847     glGetBooleanv(GL_LIGHTING, &lighting);
00848     glGetBooleanv(GL_COLOR_MATERIAL, &colorMaterial);
00849 
00850     glDisable(GL_COLOR_MATERIAL);
00851 
00852     double angle= 2 * acos(initial_origin.rot().u());
00853     double scale = sqrt (initial_origin.rot().x()*initial_origin.rot().x() 
00854                          + initial_origin.rot().y()*initial_origin.rot().y() 
00855                          + initial_origin.rot().z()*initial_origin.rot().z());
00856     double ax= initial_origin.rot().x() / scale;
00857     double ay= initial_origin.rot().y() / scale;
00858     double az= initial_origin.rot().z() / scale;
00859 
00860     if (angle > 0) glRotatef(OTD_RAD2DEG*angle, ax, ay, az);
00861 
00862     float color[4];
00863     color[0] = 0.7f;  color[1] = 0.7f;  color[2] = 1.0f;  color[3] = 1.0f;
00864     glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, color);
00865     QGLViewer::drawArrow(length, 0.01*length);
00866 
00867     color[0] = 1.0f;  color[1] = 0.7f;  color[2] = 0.7f;  color[3] = 1.0f;
00868     glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, color);
00869     glPushMatrix();
00870     glRotatef(90.0, 0.0, 1.0, 0.0);
00871     QGLViewer::drawArrow(length, 0.01*length);
00872     glPopMatrix();
00873 
00874     color[0] = 0.7f;  color[1] = 1.0f;  color[2] = 0.7f;  color[3] = 1.0f;
00875     glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, color);
00876     glPushMatrix();
00877     glRotatef(-90.0, 1.0, 0.0, 0.0);
00878     QGLViewer::drawArrow(length, 0.01*length);
00879     glPopMatrix();
00880 
00881     glTranslatef(relative_transform.trans().x(), relative_transform.trans().y(), relative_transform.trans().z());
00882 
00883     if (colorMaterial)
00884       glEnable(GL_COLOR_MATERIAL);
00885     if (!lighting)
00886       glDisable(GL_LIGHTING);
00887 
00888     glPopMatrix();
00889   }
00890 
00891 } // namespace
00892 
00893 


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