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


octovis
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Thu Jun 6 2019 17:31:58