PointcloudDrawer.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/PointcloudDrawer.h>
00027 
00028 namespace octomap {
00029 
00030   PointcloudDrawer::PointcloudDrawer()
00031     : ScanGraphDrawer(), m_pointsArray(NULL), m_numberPoints(0)
00032   {
00033   }
00034 
00035   PointcloudDrawer::PointcloudDrawer(const ScanGraph& graph)
00036     : ScanGraphDrawer(), m_pointsArray(NULL), m_numberPoints(0)
00037   {
00038     this->setScanGraph(graph);
00039 
00040   }
00041 
00042   PointcloudDrawer::~PointcloudDrawer() {
00043     clear();
00044 
00045   }
00046 
00047   void PointcloudDrawer::setScanGraph(const ScanGraph& graph){
00048     clear();
00049 
00050     // count points first:
00051     for (octomap::ScanGraph::const_iterator it = graph.begin(); it != graph.end(); it++) {
00052       m_numberPoints += (*it)->scan->size();
00053     }
00054 
00055     m_pointsArray = new GLfloat[3*m_numberPoints];
00056 
00057     unsigned i = 0;
00058     for (octomap::ScanGraph::const_iterator graph_it = graph.begin(); graph_it != graph.end(); graph_it++) {
00059       octomap::Pointcloud* scan = new Pointcloud((*graph_it)->scan);
00060       scan->transformAbsolute((*graph_it)->pose);
00061 
00062       for (Pointcloud::iterator pc_it = scan->begin(); pc_it != scan->end(); ++pc_it){
00063         m_pointsArray[3*i] = pc_it->x();
00064         m_pointsArray[3*i +1] = pc_it->y();
00065         m_pointsArray[3*i +2] = pc_it->z();
00066 
00067         i++;
00068       }
00069       delete scan;
00070     }
00071   }
00072 
00073   void PointcloudDrawer::clear(){
00074 
00075     if (m_numberPoints != 0) {
00076       delete[] m_pointsArray;
00077       m_numberPoints = 0;
00078     }
00079   }
00080 
00081   void PointcloudDrawer::draw() const{
00082     if (m_numberPoints == 0)
00083       return;
00084 
00085     glEnable(GL_POINT_SMOOTH);
00086     glEnableClientState(GL_VERTEX_ARRAY);
00087 
00088    // TODO: find a solution for printout-mode (=> separate drawer?)
00089 //    if (m_printoutMode){
00090 //      if (!m_drawFree) {
00091 //        glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
00092 //      }
00093 //      glColor4f(0.0, 0.0, 0.0, 1.);
00094 //    } else{
00095 //      glColor4f(1.0, 0.0, 0.0, 1.);
00096 //    }
00097 
00098     glPointSize(1.0);
00099     glColor4f(1.0, 0.0, 0.0, 1.);
00100 
00101     glVertexPointer(3, GL_FLOAT, 0, m_pointsArray);
00102     glDrawArrays(GL_POINTS, 0, m_numberPoints);
00103     glDisableClientState(GL_VERTEX_ARRAY);
00104   }
00105 
00106 }


octovis
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Thu Aug 27 2015 14:13:26