PointcloudDrawer.cpp
Go to the documentation of this file.
1 /*
2  * This file is part of OctoMap - An Efficient Probabilistic 3D Mapping
3  * Framework Based on Octrees
4  * http://octomap.github.io
5  *
6  * Copyright (c) 2009-2014, K.M. Wurm and A. Hornung, University of Freiburg
7  * All rights reserved. License for the viewer octovis: GNU GPL v2
8  * http://www.gnu.org/licenses/old-licenses/gpl-2.0.txt
9  *
10  *
11  * This program is free software; you can redistribute it and/or modify
12  * it under the terms of the GNU General Public License as published by
13  * the Free Software Foundation; either version 2 of the License, or
14  * (at your option) any later version.
15  *
16  * This program is distributed in the hope that it will be useful, but
17  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
18  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
19  * for more details.
20  *
21  * You should have received a copy of the GNU General Public License
22  * along with this program. If not, see http://www.gnu.org/licenses/.
23  */
24 
26 
27 namespace octomap {
28 
30  : ScanGraphDrawer(), m_pointsArray(NULL), m_numberPoints(0)
31  {
32  }
33 
36  {
37  this->setScanGraph(graph);
38 
39  }
40 
42  clear();
43 
44  }
45 
47  clear();
48 
49  // count points first:
50  for (octomap::ScanGraph::const_iterator it = graph.begin(); it != graph.end(); it++) {
51  m_numberPoints += (*it)->scan->size();
52  }
53 
54  m_pointsArray = new GLfloat[3*m_numberPoints];
55 
56  unsigned i = 0;
57  for (octomap::ScanGraph::const_iterator graph_it = graph.begin(); graph_it != graph.end(); graph_it++) {
58  octomap::Pointcloud* scan = new Pointcloud((*graph_it)->scan);
59  scan->transformAbsolute((*graph_it)->pose);
60 
61  for (Pointcloud::iterator pc_it = scan->begin(); pc_it != scan->end(); ++pc_it){
62  m_pointsArray[3*i] = pc_it->x();
63  m_pointsArray[3*i +1] = pc_it->y();
64  m_pointsArray[3*i +2] = pc_it->z();
65 
66  i++;
67  }
68  delete scan;
69  }
70  }
71 
73 
74  if (m_numberPoints != 0) {
75  delete[] m_pointsArray;
76  m_numberPoints = 0;
77  }
78  }
79 
80  void PointcloudDrawer::draw() const{
81  if (m_numberPoints == 0)
82  return;
83 
84  glEnable(GL_POINT_SMOOTH);
85  glEnableClientState(GL_VERTEX_ARRAY);
86 
87  // TODO: find a solution for printout-mode (=> separate drawer?)
88 // if (m_printoutMode){
89 // if (!m_drawFree) {
90 // glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
91 // }
92 // glColor4f(0.0, 0.0, 0.0, 1.);
93 // } else{
94 // glColor4f(1.0, 0.0, 0.0, 1.);
95 // }
96 
97  glPointSize(1.0);
98  glColor4f(1.0, 0.0, 0.0, 1.);
99 
100  glVertexPointer(3, GL_FLOAT, 0, m_pointsArray);
101  glDrawArrays(GL_POINTS, 0, m_numberPoints);
102  glDisableClientState(GL_VERTEX_ARRAY);
103  }
104 
105 }
std::vector< ScanNode * >::const_iterator const_iterator
point3d_collection::iterator iterator
void transformAbsolute(pose6d transform)
virtual void draw() const
virtual void setScanGraph(const ScanGraph &graph)


octovis
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Mon Jun 10 2019 14:00:25