OcTreeDrawer.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 
25 #include <octovis/OcTreeDrawer.h>
26 #include <qglviewer.h>
27 
28 #define OTD_RAD2DEG 57.2957795
29 
30 namespace octomap {
31 
32  OcTreeDrawer::OcTreeDrawer() : SceneObject(),
33  m_occupiedThresSize(0), m_freeThresSize(0),
34  m_occupiedSize(0), m_freeSize(0), m_selectionSize(0),
35  octree_grid_vertex_size(0), m_alphaOccupied(0.8), map_id(0)
36  {
37  m_octree_grid_vis_initialized = false;
38  m_drawOccupied = true;
39  m_drawOcTreeGrid = false;
40  m_drawFree = false;
41  m_drawSelection = true;
42  m_displayAxes = false;
43  m_update = true;
44  m_alternativeDrawing = false;
45 
46  m_occupiedArray = NULL;
47  m_freeArray = NULL;
48  m_occupiedThresArray = NULL;
49  m_freeThresArray = NULL;
50  m_occupiedColorArray = NULL;
51  m_occupiedThresColorArray = NULL;
52  m_selectionArray = NULL;
53 
54  // origin and movement
55  initial_origin = octomap::pose6d(0,0,0,0,0,0);
56  origin = initial_origin;
57  }
58 
59  OcTreeDrawer::~OcTreeDrawer() {
60  clear();
61  }
62 
63  void OcTreeDrawer::draw() const {
64  static int gl_list_index = -1;
65  if(m_alternativeDrawing && gl_list_index < 0){
66  gl_list_index = glGenLists(1);
67  m_update = true;
68  }
69  if(!m_alternativeDrawing && gl_list_index != -1){//Free video card memory
70  //std::cerr << "Freeing VRAM\n";
71  glDeleteLists(gl_list_index,1);
72  gl_list_index = -1;
73  }
74  if(m_update || !m_alternativeDrawing)
75  {
76  if(m_alternativeDrawing) {
77  std::cout << "Preparing batch rendering, please wait ...\n";
78  glNewList(gl_list_index, GL_COMPILE_AND_EXECUTE);
79  }
80 
81  // push current status
82  glPushMatrix();
83  // octomap::pose6d relative_transform = origin * initial_origin.inv();
84 
85  octomap::pose6d relative_transform = origin;// * initial_origin;
86 
87  // apply relative transform
88  const octomath::Quaternion& q = relative_transform.rot();
89  glTranslatef(relative_transform.x(), relative_transform.y(), relative_transform.z());
90 
91  // convert quaternion to angle/axis notation
92  float scale = sqrt(q.x() * q.x() + q.y() * q.y() + q.z() * q.z());
93  if (scale) {
94  float axis_x = q.x() / scale;
95  float axis_y = q.y() / scale;
96  float axis_z = q.z() / scale;
97  float angle = acos(q.u()) * 2.0f * OTD_RAD2DEG; // opengl expects DEG
98  glRotatef(angle, axis_x, axis_y, axis_z);
99  }
100 
101  glEnableClientState(GL_VERTEX_ARRAY);
102 
103  if (m_drawOccupied)
104  drawOccupiedVoxels();
105  if (m_drawFree)
106  drawFreeVoxels();
107  if (m_drawOcTreeGrid)
108  drawOctreeGrid();
109  if (m_drawSelection)
110  drawSelection();
111 
112  if (m_displayAxes) {
113  drawAxes();
114  }
115 
116  glDisableClientState(GL_VERTEX_ARRAY);
117 
118  // reset previous status
119  glPopMatrix();
120  if(m_alternativeDrawing) {
121  glEndList();
122  std::cout << "Finished preparation of batch rendering.\n";
123  }
124  m_update = false;
125  }
126  else
127  {
128  glCallList(gl_list_index);
129  }
130 
131  }
132 
133  void OcTreeDrawer::setAlphaOccupied(double alpha){
134  m_update = true;
135  m_alphaOccupied = alpha;
136  }
137 
138 
139  void OcTreeDrawer::setOcTree(const AbstractOcTree& tree, const pose6d& origin, int map_id_) {
140 
141  const OcTree& octree = (const OcTree&) tree;
142  this->map_id = map_id_;
143 
144  // save origin used during cube generation
145  this->initial_origin = octomap::pose6d(octomap::point3d(0,0,0), origin.rot());
146 
147  // origin is in global coords
148  this->origin = origin;
149 
150  // maximum size to prevent crashes on large maps: (should be checked in a better way than a constant)
151  bool showAll = (octree.size() < 5 * 1e6);
152  bool uses_origin = ( (origin.rot().x() != 0.) && (origin.rot().y() != 0.)
153  && (origin.rot().z() != 0.) && (origin.rot().u() != 1.) );
154 
155  // walk the tree one to find the number of nodes in each category
156  // (this is used to set up the OpenGL arrays)
157  // TODO: this step may be left out, if we maintained the GLArrays in std::vectors instead...
158  unsigned int cnt_occupied(0), cnt_occupied_thres(0), cnt_free(0), cnt_free_thres(0);
159  for(OcTree::tree_iterator it = octree.begin_tree(this->m_max_tree_depth),
160  end=octree.end_tree(); it!= end; ++it) {
161  if (it.isLeaf()) {
162  if (octree.isNodeOccupied(*it)){ // occupied voxels
163  if (octree.isNodeAtThreshold(*it)) ++cnt_occupied_thres;
164  else ++cnt_occupied;
165  }
166  else if (showAll) { // freespace voxels
167  if (octree.isNodeAtThreshold(*it)) ++cnt_free_thres;
168  else ++cnt_free;
169  }
170  }
171  }
172  // setup GL arrays for cube quads and cube colors
173  initGLArrays(cnt_occupied , m_occupiedSize , &m_occupiedArray , &m_occupiedColorArray);
174  initGLArrays(cnt_occupied_thres, m_occupiedThresSize, &m_occupiedThresArray, &m_occupiedThresColorArray);
175  initGLArrays(cnt_free , m_freeSize , &m_freeArray, NULL);
176  initGLArrays(cnt_free_thres , m_freeThresSize , &m_freeThresArray, NULL);
177 
178  double minX, minY, minZ, maxX, maxY, maxZ;
179  octree.getMetricMin(minX, minY, minZ);
180  octree.getMetricMax(maxX, maxY, maxZ);
181 
182  // set min/max Z for color height map
183  m_zMin = minZ;
184  m_zMax = maxZ;
185 
186  std::vector<octomath::Vector3> cube_template;
187  initCubeTemplate(origin, cube_template);
188 
189  unsigned int idx_occupied(0), idx_occupied_thres(0), idx_free(0), idx_free_thres(0);
190  unsigned int color_idx_occupied(0), color_idx_occupied_thres(0);
191 
192  m_grid_voxels.clear();
193  OcTreeVolume voxel; // current voxel, possibly transformed
194  for(OcTree::tree_iterator it = octree.begin_tree(this->m_max_tree_depth),
195  end=octree.end_tree(); it!= end; ++it) {
196 
197  if (it.isLeaf()) { // voxels for leaf nodes
198  if (uses_origin)
199  voxel = OcTreeVolume(origin.rot().rotate(it.getCoordinate()), it.getSize());
200  else
201  voxel = OcTreeVolume(it.getCoordinate(), it.getSize());
202 
203  if (octree.isNodeOccupied(*it)){ // occupied voxels
204  if (octree.isNodeAtThreshold(*it)) {
205  idx_occupied_thres = generateCube(voxel, cube_template, idx_occupied_thres, &m_occupiedThresArray);
206  color_idx_occupied_thres = setCubeColorHeightmap(voxel, color_idx_occupied_thres, &m_occupiedThresColorArray);
207  }
208  else {
209  idx_occupied = generateCube(voxel, cube_template, idx_occupied, &m_occupiedArray);
210  color_idx_occupied = setCubeColorHeightmap(voxel, color_idx_occupied, &m_occupiedColorArray);
211  }
212  }
213  else if (showAll) { // freespace voxels
214  if (octree.isNodeAtThreshold(*it)) {
215  idx_free_thres = generateCube(voxel, cube_template, idx_free_thres, &m_freeThresArray);
216  }
217  else {
218  idx_free = generateCube(voxel, cube_template, idx_free, &m_freeArray);
219  }
220  }
221  }
222 
223  else { // inner node voxels (for grid structure only)
224  if (showAll) {
225  if (uses_origin)
226  voxel = OcTreeVolume(origin.rot().rotate(it.getCoordinate()), it.getSize());
227  else
228  voxel = OcTreeVolume(it.getCoordinate(), it.getSize());
229  m_grid_voxels.push_back(voxel);
230  }
231  }
232  } // end for all voxels
233 
234  m_octree_grid_vis_initialized = false;
235 
236  if(m_drawOcTreeGrid)
237  initOctreeGridVis();
238  }
239 
240  void OcTreeDrawer::setOcTreeSelection(const std::list<octomap::OcTreeVolume>& selectedVoxels){
241  m_update = true;
242  generateCubes(selectedVoxels, &m_selectionArray, m_selectionSize, this->origin);
243  }
244 
245  void OcTreeDrawer::clearOcTreeSelection(){
246  m_update = true;
247  clearCubes(&m_selectionArray, m_selectionSize);
248  }
249 
250  void OcTreeDrawer::initGLArrays(const unsigned int& num_cubes,
251  unsigned int& glArraySize,
252  GLfloat*** glArray, GLfloat** glColorArray) {
253 
254  clearCubes(glArray, glArraySize, glColorArray);
255 
256  // store size of GL arrays for drawing
257  glArraySize = num_cubes * 4 * 3;
258 
259  // allocate cube arrays, 6 quads per cube
260  *glArray = new GLfloat* [6];
261  for (unsigned i = 0; i<6; ++i){
262  (*glArray)[i] = new GLfloat[glArraySize];
263  }
264  // setup quad color array, if given
265  if (glColorArray != NULL)
266  *glColorArray = new GLfloat[glArraySize * 4 *4];
267  }
268 
269  void OcTreeDrawer::initCubeTemplate(const octomath::Pose6D& origin,
270  std::vector<octomath::Vector3>& cube_template) {
271  cube_template.clear();
272  cube_template.reserve(24);
273 
274  cube_template.push_back(octomath::Vector3( 1, 1,-1));
275  cube_template.push_back(octomath::Vector3( 1,-1,-1));
276  cube_template.push_back(octomath::Vector3( 1, 1,-1));
277  cube_template.push_back(octomath::Vector3(-1, 1,-1));
278  cube_template.push_back(octomath::Vector3( 1, 1,-1));
279  cube_template.push_back(octomath::Vector3( 1, 1, 1));
280 
281  cube_template.push_back(octomath::Vector3(-1, 1,-1));
282  cube_template.push_back(octomath::Vector3(-1,-1,-1));
283  cube_template.push_back(octomath::Vector3( 1, 1, 1));
284  cube_template.push_back(octomath::Vector3(-1, 1, 1));
285  cube_template.push_back(octomath::Vector3( 1,-1,-1));
286  cube_template.push_back(octomath::Vector3( 1,-1, 1));
287 
288  cube_template.push_back(octomath::Vector3(-1, 1, 1));
289  cube_template.push_back(octomath::Vector3(-1,-1, 1));
290  cube_template.push_back(octomath::Vector3( 1,-1, 1));
291  cube_template.push_back(octomath::Vector3(-1,-1, 1));
292  cube_template.push_back(octomath::Vector3(-1,-1,-1));
293  cube_template.push_back(octomath::Vector3(-1,-1, 1));
294 
295  cube_template.push_back(octomath::Vector3( 1, 1, 1));
296  cube_template.push_back(octomath::Vector3( 1,-1, 1));
297  cube_template.push_back(octomath::Vector3( 1,-1,-1));
298  cube_template.push_back(octomath::Vector3(-1,-1,-1));
299  cube_template.push_back(octomath::Vector3(-1, 1,-1));
300  cube_template.push_back(octomath::Vector3(-1, 1, 1));
301  }
302 
303  unsigned int OcTreeDrawer::generateCube(const octomap::OcTreeVolume& v,
304  const std::vector<octomath::Vector3>& cube_template,
305  const unsigned int& current_array_idx,
306  GLfloat*** glArray) {
307 
308  // epsilon to be substracted from cube size so that neighboring planes don't overlap
309  // seems to introduce strange artifacts nevertheless...
310  double eps = 1e-5;
311 
312  octomath::Vector3 p;
313 
314  double half_cube_size = GLfloat(v.second /2.0 -eps);
315  unsigned int i = current_array_idx;
316  // Cube surfaces are in gl_array in order: back, front, top, down, left, right.
317  // Arrays are filled in parallel (increasing i for all at once)
318  // One color array for all surfaces is filled when requested
319 
320  p = v.first + cube_template[0] * half_cube_size;
321  (*glArray)[0][i] = p.x();
322  (*glArray)[0][i+1] = p.y();
323  (*glArray)[0][i+2] = p.z();
324 
325  p = v.first + cube_template[1] * half_cube_size;
326  (*glArray)[1][i] = p.x();
327  (*glArray)[1][i+1] = p.y();
328  (*glArray)[1][i+2] = p.z();
329 
330  p = v.first + cube_template[2] * half_cube_size;
331  (*glArray)[2][i] = p.x();
332  (*glArray)[2][i+1] = p.y();
333  (*glArray)[2][i+2] = p.z();
334 
335  p = v.first + cube_template[3] * half_cube_size;
336  (*glArray)[3][i] = p.x();
337  (*glArray)[3][i+1] = p.y();
338  (*glArray)[3][i+2] = p.z();
339 
340  p = v.first + cube_template[4] * half_cube_size;
341  (*glArray)[4][i] = p.x();
342  (*glArray)[4][i+1] = p.y();
343  (*glArray)[4][i+2] = p.z();
344 
345  p = v.first + cube_template[5] * half_cube_size;
346  (*glArray)[5][i] = p.x();
347  (*glArray)[5][i+1] = p.y();
348  (*glArray)[5][i+2] = p.z();
349  i+= 3; //-------------------
350 
351  p = v.first + cube_template[6] * half_cube_size;
352  (*glArray)[0][i] = p.x();
353  (*glArray)[0][i+1] = p.y();
354  (*glArray)[0][i+2] = p.z();
355 
356  p = v.first + cube_template[7] * half_cube_size;
357  (*glArray)[1][i] = p.x();
358  (*glArray)[1][i+1] = p.y();
359  (*glArray)[1][i+2] = p.z();
360 
361  p = v.first + cube_template[8] * half_cube_size;
362  (*glArray)[2][i] = p.x();
363  (*glArray)[2][i+1] = p.y();
364  (*glArray)[2][i+2] = p.z();
365 
366  p = v.first + cube_template[9] * half_cube_size;
367  (*glArray)[3][i] = p.x();
368  (*glArray)[3][i+1] = p.y();
369  (*glArray)[3][i+2] = p.z();
370 
371  p = v.first + cube_template[10] * half_cube_size;
372  (*glArray)[4][i] = p.x();
373  (*glArray)[4][i+1] = p.y();
374  (*glArray)[4][i+2] = p.z();
375 
376  p = v.first + cube_template[11] * half_cube_size;
377  (*glArray)[5][i] = p.x();
378  (*glArray)[5][i+1] = p.y();
379  (*glArray)[5][i+2] = p.z();
380  i+= 3; //-------------------
381 
382  p = v.first + cube_template[12] * half_cube_size;
383  (*glArray)[0][i] = p.x();
384  (*glArray)[0][i+1] = p.y();
385  (*glArray)[0][i+2] = p.z();
386 
387  p = v.first + cube_template[13] * half_cube_size;
388  (*glArray)[1][i] = p.x();
389  (*glArray)[1][i+1] = p.y();
390  (*glArray)[1][i+2] = p.z();
391 
392  p = v.first + cube_template[14] * half_cube_size;
393  (*glArray)[2][i] = p.x();
394  (*glArray)[2][i+1] = p.y();
395  (*glArray)[2][i+2] = p.z();
396 
397  p = v.first + cube_template[15] * half_cube_size;
398  (*glArray)[3][i] = p.x();
399  (*glArray)[3][i+1] = p.y();
400  (*glArray)[3][i+2] = p.z();
401 
402  p = v.first + cube_template[16] * half_cube_size;
403  (*glArray)[4][i] = p.x();
404  (*glArray)[4][i+1] = p.y();
405  (*glArray)[4][i+2] = p.z();
406 
407  p = v.first + cube_template[17] * half_cube_size;
408  (*glArray)[5][i] = p.x();
409  (*glArray)[5][i+1] = p.y();
410  (*glArray)[5][i+2] = p.z();
411  i+= 3; //-------------------
412 
413  p = v.first + cube_template[18] * half_cube_size;
414  (*glArray)[0][i] = p.x();
415  (*glArray)[0][i+1] = p.y();
416  (*glArray)[0][i+2] = p.z();
417 
418  p = v.first + cube_template[19] * half_cube_size;
419  (*glArray)[1][i] = p.x();
420  (*glArray)[1][i+1] = p.y();
421  (*glArray)[1][i+2] = p.z();
422 
423  p = v.first + cube_template[20] * half_cube_size;
424  (*glArray)[2][i] = p.x();
425  (*glArray)[2][i+1] = p.y();
426  (*glArray)[2][i+2] = p.z();
427 
428  p = v.first + cube_template[21] * half_cube_size;
429  (*glArray)[3][i] = p.x();
430  (*glArray)[3][i+1] = p.y();
431  (*glArray)[3][i+2] = p.z();
432 
433  p = v.first + cube_template[22] * half_cube_size;
434  (*glArray)[4][i] = p.x();
435  (*glArray)[4][i+1] = p.y();
436  (*glArray)[4][i+2] = p.z();
437 
438  p = v.first + cube_template[23] * half_cube_size;
439  (*glArray)[5][i] = p.x();
440  (*glArray)[5][i+1] = p.y();
441  (*glArray)[5][i+2] = p.z();
442  i += 3; //-------------------
443 
444  return i; // updated array idx
445  }
446 
447 
448  unsigned int OcTreeDrawer::setCubeColorHeightmap(const octomap::OcTreeVolume& v,
449  const unsigned int& current_array_idx,
450  GLfloat** glColorArray) {
451 
452  if (glColorArray == NULL) return current_array_idx;
453 
454  unsigned int colorIdx = current_array_idx;
455  // color for all 4 vertices (same height)
456  for (int k = 0; k < 4; ++k) {
457  if (m_colorMode == CM_GRAY_HEIGHT)
458  SceneObject::heightMapGray(v.first.z(), *glColorArray + colorIdx); // sets r,g,b
459  else
460  SceneObject::heightMapColor(v.first.z(), *glColorArray + colorIdx); // sets r,g,b
461  // set Alpha value:
462  (*glColorArray)[colorIdx + 3] = m_alphaOccupied;
463  colorIdx += 4;
464  }
465  return colorIdx;
466  }
467 
468  unsigned int OcTreeDrawer::setCubeColorRGBA(const unsigned char& r,
469  const unsigned char& g,
470  const unsigned char& b,
471  const unsigned char& a,
472  const unsigned int& current_array_idx,
473  GLfloat** glColorArray) {
474 
475  if (glColorArray == NULL) return current_array_idx;
476  unsigned int colorIdx = current_array_idx;
477  // set color for next 4 vertices (=one quad)
478  for (int k = 0; k < 4; ++k) {
479  (*glColorArray)[colorIdx ] = (double) r/255.;
480  (*glColorArray)[colorIdx + 1] = (double) g/255.;
481  (*glColorArray)[colorIdx + 2] = (double) b/255.;
482  (*glColorArray)[colorIdx + 3] = (double) a/255.;
483  colorIdx += 4;
484  }
485  return colorIdx;
486  }
487 
488 
489  void OcTreeDrawer::clearCubes(GLfloat*** glArray,
490  unsigned int& glArraySize,
491  GLfloat** glColorArray) {
492  if (glArraySize != 0) {
493  for (unsigned i = 0; i < 6; ++i) {
494  delete[] (*glArray)[i];
495  }
496  delete[] *glArray;
497  *glArray = NULL;
498  glArraySize = 0;
499  }
500  if (glColorArray != NULL && *glColorArray != NULL) {
501  delete[] *glColorArray;
502  *glColorArray = NULL;
503  }
504  }
505 
506 
507  // still used for "selection" nodes
508  void OcTreeDrawer::generateCubes(const std::list<octomap::OcTreeVolume>& voxels,
509  GLfloat*** glArray, unsigned int& glArraySize,
510  octomath::Pose6D& origin,
511  GLfloat** glColorArray) {
512  unsigned int i = 0;
513  unsigned int colorIdx = 0;
514 
515  std::vector<octomath::Vector3> cube_template;
516  initCubeTemplate(origin, cube_template);
517 
518  for (std::list<octomap::OcTreeVolume>::const_iterator it=voxels.begin();
519  it != voxels.end(); it++) {
520  i = generateCube(*it, cube_template, i, glArray);
521  }
522 
523  if (glColorArray != NULL) {
524  for (std::list<octomap::OcTreeVolume>::const_iterator it=voxels.begin();
525  it != voxels.end(); it++) {
526  colorIdx = setCubeColorHeightmap(*it, colorIdx, glColorArray);
527  }
528  }
529  }
530 
531  void OcTreeDrawer::initOctreeGridVis() {
532 
533  if (m_octree_grid_vis_initialized) return;
534 
535  clearOcTreeStructure();
536  // allocate arrays for octree grid visualization
537  octree_grid_vertex_size = m_grid_voxels.size() * 12 * 2 * 3;
538  octree_grid_vertex_array = new GLfloat[octree_grid_vertex_size];
539 
540  // generate the cubes, 12 lines each
541  std::list<octomap::OcTreeVolume>::iterator it_rec;
542  unsigned int i = 0;
543  double x,y,z;
544  for (it_rec=m_grid_voxels.begin(); it_rec != m_grid_voxels.end(); it_rec++) {
545 
546  x = it_rec->first.x();
547  y = it_rec->first.y();
548  z = it_rec->first.z();
549 
550  double half_voxel_size = it_rec->second / 2.0;
551 
552  // ----
553  octree_grid_vertex_array[i] = x + half_voxel_size;
554  octree_grid_vertex_array[i+1] = y + half_voxel_size;
555  octree_grid_vertex_array[i+2] = z - half_voxel_size;
556  i+= 3;
557  octree_grid_vertex_array[i] = x - half_voxel_size;
558  octree_grid_vertex_array[i+1] = y + half_voxel_size;
559  octree_grid_vertex_array[i+2] = z - half_voxel_size;
560  i+= 3;
561  octree_grid_vertex_array[i] = x - half_voxel_size;
562  octree_grid_vertex_array[i+1] = y + half_voxel_size;
563  octree_grid_vertex_array[i+2] = z - half_voxel_size;
564  i += 3;
565  octree_grid_vertex_array[i] = x - half_voxel_size;
566  octree_grid_vertex_array[i+1] = y + half_voxel_size;
567  octree_grid_vertex_array[i+2] = z + half_voxel_size;
568  i+= 3;
569  octree_grid_vertex_array[i] = x - half_voxel_size;
570  octree_grid_vertex_array[i+1] = y + half_voxel_size;
571  octree_grid_vertex_array[i+2] = z + half_voxel_size;
572  i+= 3;
573  octree_grid_vertex_array[i] = x + half_voxel_size;
574  octree_grid_vertex_array[i+1] = y + half_voxel_size;
575  octree_grid_vertex_array[i+2] = z + half_voxel_size;
576  i+= 3;
577  octree_grid_vertex_array[i] = x + half_voxel_size;
578  octree_grid_vertex_array[i+1] = y + half_voxel_size;
579  octree_grid_vertex_array[i+2] = z + half_voxel_size;
580  i+= 3;
581  octree_grid_vertex_array[i] = x + half_voxel_size;
582  octree_grid_vertex_array[i+1] = y + half_voxel_size;
583  octree_grid_vertex_array[i+2] = z - half_voxel_size;
584  i+= 3;
585  // ----
586  octree_grid_vertex_array[i] = x + half_voxel_size;
587  octree_grid_vertex_array[i+1] = y - half_voxel_size;
588  octree_grid_vertex_array[i+2] = z + half_voxel_size;
589  i+= 3;
590  octree_grid_vertex_array[i] = x - half_voxel_size;
591  octree_grid_vertex_array[i+1] = y - half_voxel_size;
592  octree_grid_vertex_array[i+2] = z + half_voxel_size;
593  i+= 3;
594  octree_grid_vertex_array[i] = x - half_voxel_size;
595  octree_grid_vertex_array[i+1] = y - half_voxel_size;
596  octree_grid_vertex_array[i+2] = z + half_voxel_size;
597  i+= 3;
598  octree_grid_vertex_array[i] = x - half_voxel_size;
599  octree_grid_vertex_array[i+1] = y - half_voxel_size;
600  octree_grid_vertex_array[i+2] = z - half_voxel_size;
601  i+= 3;
602  octree_grid_vertex_array[i] = x - half_voxel_size;
603  octree_grid_vertex_array[i+1] = y - half_voxel_size;
604  octree_grid_vertex_array[i+2] = z - half_voxel_size;
605  i+= 3;
606  octree_grid_vertex_array[i] = x + half_voxel_size;
607  octree_grid_vertex_array[i+1] = y - half_voxel_size;
608  octree_grid_vertex_array[i+2] = z - half_voxel_size;
609  i+= 3;
610  octree_grid_vertex_array[i] = x + half_voxel_size;
611  octree_grid_vertex_array[i+1] = y - half_voxel_size;
612  octree_grid_vertex_array[i+2] = z - half_voxel_size;
613  i+= 3;
614  octree_grid_vertex_array[i] = x + half_voxel_size;
615  octree_grid_vertex_array[i+1] = y - half_voxel_size;
616  octree_grid_vertex_array[i+2] = z + half_voxel_size;
617  i+= 3;
618  // ----
619  octree_grid_vertex_array[i] = x + half_voxel_size;
620  octree_grid_vertex_array[i+1] = y + half_voxel_size;
621  octree_grid_vertex_array[i+2] = z - half_voxel_size;
622  i+= 3;
623  octree_grid_vertex_array[i] = x + half_voxel_size;
624  octree_grid_vertex_array[i+1] = y - half_voxel_size;
625  octree_grid_vertex_array[i+2] = z - half_voxel_size;
626  i+= 3;
627  octree_grid_vertex_array[i] = x - half_voxel_size;
628  octree_grid_vertex_array[i+1] = y + half_voxel_size;
629  octree_grid_vertex_array[i+2] = z - half_voxel_size;
630  i+= 3;
631  octree_grid_vertex_array[i] = x - half_voxel_size;
632  octree_grid_vertex_array[i+1] = y - half_voxel_size;
633  octree_grid_vertex_array[i+2] = z - half_voxel_size;
634  i+= 3;
635  octree_grid_vertex_array[i] = x - half_voxel_size;
636  octree_grid_vertex_array[i+1] = y + half_voxel_size;
637  octree_grid_vertex_array[i+2] = z + half_voxel_size;
638  i+= 3;
639  octree_grid_vertex_array[i] = x - half_voxel_size;
640  octree_grid_vertex_array[i+1] = y - half_voxel_size;
641  octree_grid_vertex_array[i+2] = z + half_voxel_size;
642  i+= 3;
643  octree_grid_vertex_array[i] = x + half_voxel_size;
644  octree_grid_vertex_array[i+1] = y + half_voxel_size;
645  octree_grid_vertex_array[i+2] = z + half_voxel_size;
646  i+= 3;
647  octree_grid_vertex_array[i] = x + half_voxel_size;
648  octree_grid_vertex_array[i+1] = y - half_voxel_size;
649  octree_grid_vertex_array[i+2] = z + half_voxel_size;
650  i+= 3;
651  // ----
652  }
653  m_octree_grid_vis_initialized = true;
654  }
655 
656  void OcTreeDrawer::clearOcTreeStructure(){
657  if (octree_grid_vertex_size != 0) {
658  delete[] octree_grid_vertex_array;
659  octree_grid_vertex_size = 0;
660  }
661  m_octree_grid_vis_initialized = false;
662  }
663 
664  void OcTreeDrawer::clear() {
665  //clearOcTree();
666  clearCubes(&m_occupiedArray, m_occupiedSize, &m_occupiedColorArray);
667  clearCubes(&m_occupiedThresArray, m_occupiedThresSize, &m_occupiedThresColorArray);
668  clearCubes(&m_freeArray, m_freeSize);
669  clearCubes(&m_freeThresArray, m_freeThresSize);
670  clearCubes(&m_selectionArray, m_selectionSize);
671  clearOcTreeStructure();
672  }
673 
674 
675  void OcTreeDrawer::drawOccupiedVoxels() const {
676 
677  if (m_colorMode == CM_SEMANTIC) {
678  // hardcoded mapping id -> colors
679  if (this->map_id == 0) { // background
680  glColor3f(0.784f, 0.66f, 0); // gold
681  }
682  else if (this->map_id == 1) { // table
683  glColor3f(0.68f, 0., 0.62f); // purple
684  }
685  else { // object
686  glColor3f(0., 0.784f, 0.725f); // cyan
687  }
688  drawCubes(m_occupiedThresArray, m_occupiedThresSize, m_occupiedThresColorArray);
689  }
690  else {
691  // colors for printout mode:
692  if (m_colorMode == CM_PRINTOUT) {
693  if (!m_drawFree) { // gray on white background
694  glColor3f(0.6f, 0.6f, 0.6f);
695  }
696  else {
697  glColor3f(0.1f, 0.1f, 0.1f);
698  }
699  }
700 
701  // draw binary occupied cells
702  if (m_occupiedThresSize != 0) {
703  if (m_colorMode != CM_PRINTOUT) glColor4f(0.0f, 0.0f, 1.0f, m_alphaOccupied);
704  drawCubes(m_occupiedThresArray, m_occupiedThresSize, m_occupiedThresColorArray);
705  }
706 
707  // draw delta occupied cells
708  if (m_occupiedSize != 0) {
709  if (m_colorMode != CM_PRINTOUT) glColor4f(0.2f, 0.7f, 1.0f, m_alphaOccupied);
710  drawCubes(m_occupiedArray, m_occupiedSize, m_occupiedColorArray);
711  }
712  }
713  }
714 
715 
716  void OcTreeDrawer::drawFreeVoxels() const {
717 
718  if (m_colorMode == CM_PRINTOUT) {
719  if (!m_drawOccupied) { // gray on white background
720  glColor3f(0.5f, 0.5f, 0.5f);
721  }
722  else {
723  glColor3f(0.9f, 0.9f, 0.9f);
724  }
725  }
726 
727  // draw binary freespace cells
728  if (m_freeThresSize != 0) {
729  if (m_colorMode != CM_PRINTOUT) glColor4f(0.0f, 1.0f, 0.0f, 0.3f);
730  drawCubes(m_freeThresArray, m_freeThresSize);
731  }
732 
733  // draw delta freespace cells
734  if (m_freeSize != 0) {
735  if (m_colorMode != CM_PRINTOUT) glColor4f(0.5f, 1.0f, 0.1f, 0.3f);
736  drawCubes(m_freeArray, m_freeSize);
737  }
738  }
739 
740  void OcTreeDrawer::drawSelection() const {
741  if (m_selectionSize != 0) {
742  glColor4f(1.0, 0.0, 0.0, 0.5);
743  drawCubes(m_selectionArray, m_selectionSize);
744  }
745  }
746 
747  void OcTreeDrawer::drawCubes(GLfloat** cubeArray, unsigned int cubeArraySize,
748  GLfloat* cubeColorArray) const {
749  if (cubeArraySize == 0 || cubeArray == NULL){
750  std::cerr << "Warning: GLfloat array to draw cubes appears to be empty, nothing drawn.\n";
751  return;
752  }
753 
754  // save current color
755  GLfloat* curcol = new GLfloat[4];
756  glGetFloatv(GL_CURRENT_COLOR, curcol);
757 
758  // enable color pointer when heightColorMode is enabled:
759 
760  if ((m_colorMode == CM_COLOR_HEIGHT || m_colorMode == CM_GRAY_HEIGHT) && (cubeColorArray != NULL)){
761  glEnableClientState(GL_COLOR_ARRAY);
762  glColorPointer(4, GL_FLOAT, 0, cubeColorArray);
763  }
764 
765  // top surfaces:
766  glNormal3f(0.0f, 1.0f, 0.0f);
767  glVertexPointer(3, GL_FLOAT, 0, cubeArray[0]);
768  glDrawArrays(GL_QUADS, 0, cubeArraySize / 3);
769  // bottom surfaces:
770  glNormal3f(0.0f, -1.0f, 0.0f);
771  glVertexPointer(3, GL_FLOAT, 0, cubeArray[1]);
772  glDrawArrays(GL_QUADS, 0, cubeArraySize / 3);
773  // right surfaces:
774  glNormal3f(1.0f, 0.0f, 0.0f);
775  glVertexPointer(3, GL_FLOAT, 0, cubeArray[2]);
776  glDrawArrays(GL_QUADS, 0, cubeArraySize / 3);
777  // left surfaces:
778  glNormal3f(-1.0f, 0.0f, 0.0f);
779  glVertexPointer(3, GL_FLOAT, 0, cubeArray[3]);
780  glDrawArrays(GL_QUADS, 0, cubeArraySize / 3);
781  // back surfaces:
782  glNormal3f(0.0f, 0.0f, -1.0f);
783  glVertexPointer(3, GL_FLOAT, 0, cubeArray[4]);
784  glDrawArrays(GL_QUADS, 0, cubeArraySize / 3);
785  // front surfaces:
786  glNormal3f(0.0f, 0.0f, 1.0f);
787  glVertexPointer(3, GL_FLOAT, 0, cubeArray[5]);
788  glDrawArrays(GL_QUADS, 0, cubeArraySize / 3);
789 
790  if ((m_colorMode == CM_COLOR_HEIGHT || m_colorMode == CM_GRAY_HEIGHT)
791  && (cubeColorArray != NULL)){
792  glDisableClientState(GL_COLOR_ARRAY);
793  }
794 
795  // draw bounding linies of cubes in printout:
796  if (m_colorMode == CM_PRINTOUT){
797  glDisable(GL_LIGHTING);
798  glHint (GL_LINE_SMOOTH_HINT, GL_NICEST);
799  glEnable (GL_LINE_SMOOTH);
800  glPolygonMode (GL_FRONT_AND_BACK, GL_LINE); // Draw Polygons only as Wireframes
801  glLineWidth(2.0f);
802  glColor3f(0.0f, 0.0f, 0.0f);
803  glCullFace(GL_FRONT_AND_BACK); // Don't draw any Polygons faces
804  //glDepthFunc (GL_LEQUAL);
805 
806  // top meshes:
807  glNormal3f(0.0f, 1.0f, 0.0f);
808  glVertexPointer(3, GL_FLOAT, 0, cubeArray[0]);
809  glDrawArrays(GL_QUADS, 0, cubeArraySize / 3);
810  // bottom meshes:
811  glNormal3f(0.0f, -1.0f, 0.0f);
812  glVertexPointer(3, GL_FLOAT, 0, cubeArray[1]);
813  glDrawArrays(GL_QUADS, 0, cubeArraySize / 3);
814  // right meshes:
815  glNormal3f(1.0f, 0.0f, 0.0f);
816  glVertexPointer(3, GL_FLOAT, 0, cubeArray[2]);
817  glDrawArrays(GL_QUADS, 0, cubeArraySize / 3);
818  // left meshes:
819  glNormal3f(-1.0f, 0.0f, 0.0f);
820  glVertexPointer(3, GL_FLOAT, 0, cubeArray[3]);
821  glDrawArrays(GL_QUADS, 0, cubeArraySize / 3);
822 
823  // restore defaults:
824  glCullFace(GL_BACK);
825  //glDepthFunc(GL_LESS);
826  glDisable(GL_LINE_SMOOTH);
827  glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
828  glEnable(GL_LIGHTING);
829  }
830  // reset color
831  glColor4fv(curcol);
832  delete[] curcol;
833  }
834 
835  void OcTreeDrawer::drawOctreeGrid() const {
836  if (!m_octree_grid_vis_initialized) return;
837  if (octree_grid_vertex_size == 0) return;
838 
839  glDisable(GL_LIGHTING);
840  glEnable(GL_LINE_SMOOTH);
841 
842  glLineWidth(1.);
843  glVertexPointer(3, GL_FLOAT, 0, octree_grid_vertex_array);
844  glColor3f(0.0, 0.0, 0.0);
845  glDrawArrays(GL_LINES, 0, octree_grid_vertex_size / 3);
846 
847  glDisable(GL_LINE_SMOOTH);
848  glEnable(GL_LIGHTING);
849  }
850 
851  void OcTreeDrawer::enableOcTree(bool enabled) {
852  m_update = true;
853  m_drawOcTreeGrid = enabled;
854  if(m_drawOcTreeGrid && !m_octree_grid_vis_initialized) {
855  initOctreeGridVis();
856  }
857  }
858 
859 
860  void OcTreeDrawer::setOrigin(octomap::pose6d t){
861  origin = t;
862  std::cout << "OcTreeDrawer: setting new global origin: " << t << std::endl;
863 
864  octomap::pose6d relative_transform = origin * initial_origin.inv();
865 
866  std::cout << "origin : " << origin << std::endl;
867  std::cout << "inv init orig : " << initial_origin.inv() << std::endl;
868  std::cout << "relative trans: " << relative_transform << std::endl;
869  }
870 
871  void OcTreeDrawer::drawAxes() const {
872 
873  octomap::pose6d relative_transform = origin * initial_origin.inv();
874 
875  glPushMatrix();
876 
877  float length = 0.15f;
878 
879  GLboolean lighting, colorMaterial;
880  glGetBooleanv(GL_LIGHTING, &lighting);
881  glGetBooleanv(GL_COLOR_MATERIAL, &colorMaterial);
882 
883  glDisable(GL_COLOR_MATERIAL);
884 
885  double angle= 2 * acos(initial_origin.rot().u());
886  double scale = sqrt (initial_origin.rot().x()*initial_origin.rot().x()
887  + initial_origin.rot().y()*initial_origin.rot().y()
888  + initial_origin.rot().z()*initial_origin.rot().z());
889  double ax= initial_origin.rot().x() / scale;
890  double ay= initial_origin.rot().y() / scale;
891  double az= initial_origin.rot().z() / scale;
892 
893  if (angle > 0) glRotatef(OTD_RAD2DEG*angle, ax, ay, az);
894 
895  float color[4];
896  color[0] = 0.7f; color[1] = 0.7f; color[2] = 1.0f; color[3] = 1.0f;
897  glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, color);
898  QGLViewer::drawArrow(length, 0.01*length);
899 
900  color[0] = 1.0f; color[1] = 0.7f; color[2] = 0.7f; color[3] = 1.0f;
901  glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, color);
902  glPushMatrix();
903  glRotatef(90.0, 0.0, 1.0, 0.0);
904  QGLViewer::drawArrow(length, 0.01*length);
905  glPopMatrix();
906 
907  color[0] = 0.7f; color[1] = 1.0f; color[2] = 0.7f; color[3] = 1.0f;
908  glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, color);
909  glPushMatrix();
910  glRotatef(-90.0, 1.0, 0.0, 0.0);
911  QGLViewer::drawArrow(length, 0.01*length);
912  glPopMatrix();
913 
914  glTranslatef(relative_transform.trans().x(), relative_transform.trans().y(), relative_transform.trans().z());
915 
916  if (colorMaterial)
917  glEnable(GL_COLOR_MATERIAL);
918  if (!lighting)
919  glDisable(GL_LIGHTING);
920 
921  glPopMatrix();
922  }
923 
924 } // namespace
925 
926 
OcTreeBaseImpl< OcTreeNode, AbstractOccupancyOcTree >::begin_tree
tree_iterator begin_tree(unsigned char maxDepth=0) const
octomap::AbstractOccupancyOcTree::isNodeAtThreshold
bool isNodeAtThreshold(const OcTreeNode *occupancyNode) const
octomap::OcTreeDrawer::setOcTree
void setOcTree(const AbstractOcTree &octree)
sets a new OcTree that should be drawn by this drawer
Definition: OcTreeDrawer.h:43
octomap::OcTreeDrawer::setCubeColorHeightmap
unsigned int setCubeColorHeightmap(const octomap::OcTreeVolume &v, const unsigned int &current_array_idx, GLfloat **glColorArray)
Definition: OcTreeDrawer.cpp:448
octomap::OcTreeDrawer::octree_grid_vertex_size
unsigned int octree_grid_vertex_size
Definition: OcTreeDrawer.h:140
octomap::OcTreeDrawer::drawFreeVoxels
void drawFreeVoxels() const
Definition: OcTreeDrawer.cpp:716
octomap::OcTreeDrawer::clearCubes
void clearCubes(GLfloat ***glArray, unsigned int &glArraySize, GLfloat **glColorArray=NULL)
clear OpenGL visualization
Definition: OcTreeDrawer.cpp:489
octomap::OcTreeDrawer::m_occupiedArray
GLfloat ** m_occupiedArray
Definition: OcTreeDrawer.h:126
octomap::OcTreeDrawer::octree_grid_vertex_array
GLfloat * octree_grid_vertex_array
OpenGL representation of Octree (grid structure)
Definition: OcTreeDrawer.h:139
octomap::SceneObject::heightMapGray
void heightMapGray(double h, GLfloat *glArrayPos) const
Definition: SceneObject.cpp:92
octomap::OcTreeDrawer::clearOcTreeSelection
void clearOcTreeSelection()
clear the visualization of the OcTree selection
Definition: OcTreeDrawer.cpp:245
octomap::SceneObject::m_colorMode
ColorMode m_colorMode
Definition: SceneObject.h:87
octomap::OcTreeDrawer::drawOccupiedVoxels
void drawOccupiedVoxels() const
Definition: OcTreeDrawer.cpp:675
octomap::OcTreeVolume
std::pair< point3d, double > OcTreeVolume
octomap::OcTreeDrawer::initial_origin
octomap::pose6d initial_origin
Definition: OcTreeDrawer.h:157
octomap::OcTreeDrawer::m_occupiedColorArray
GLfloat * m_occupiedColorArray
Definition: OcTreeDrawer.h:135
octomap::OcTreeDrawer::enableOcTree
void enableOcTree(bool enabled=true)
Definition: OcTreeDrawer.cpp:851
octomap::OcTreeDrawer::initCubeTemplate
void initCubeTemplate(const octomath::Pose6D &origin, std::vector< octomath::Vector3 > &cube_template)
setup cube template
Definition: OcTreeDrawer.cpp:269
octomap::OcTreeDrawer::drawSelection
void drawSelection() const
Definition: OcTreeDrawer.cpp:740
octomap::OcTreeDrawer::setOcTreeSelection
void setOcTreeSelection(const std::list< octomap::OcTreeVolume > &selectedPoints)
sets a new selection of the current OcTree to be drawn
Definition: OcTreeDrawer.cpp:240
octomap::OcTreeDrawer::drawOctreeGrid
void drawOctreeGrid() const
Definition: OcTreeDrawer.cpp:835
OcTreeBaseImpl< OcTreeNode, AbstractOccupancyOcTree >::getMetricMin
virtual void getMetricMin(double &x, double &y, double &z)
octomap::OcTreeDrawer::generateCubes
void generateCubes(const std::list< octomap::OcTreeVolume > &voxels, GLfloat ***glArray, unsigned int &glArraySize, octomath::Pose6D &origin, GLfloat **glColorArray=NULL)
Definition: OcTreeDrawer.cpp:508
octomap::OcTreeDrawer::m_freeThresArray
GLfloat ** m_freeThresArray
Definition: OcTreeDrawer.h:124
octomap::OcTreeDrawer::m_occupiedThresArray
GLfloat ** m_occupiedThresArray
OpenGL representation of Octree cells (cubes)
Definition: OcTreeDrawer.h:122
octomap::OcTreeDrawer::m_selectionSize
unsigned int m_selectionSize
Definition: OcTreeDrawer.h:131
octomath::Pose6D::trans
Vector3 & trans()
octomap::OcTreeDrawer::generateCube
unsigned int generateCube(const octomap::OcTreeVolume &v, const std::vector< octomath::Vector3 > &cube_template, const unsigned int &current_array_idx, GLfloat ***glArray)
add one cube to arrays
Definition: OcTreeDrawer.cpp:303
OcTreeBaseImpl< OcTreeNode, AbstractOccupancyOcTree >::getMetricMax
virtual void getMetricMax(double &x, double &y, double &z)
octomap::OcTreeDrawer::initGLArrays
void initGLArrays(const unsigned int &num_cubes, unsigned int &glArraySize, GLfloat ***glArray, GLfloat **glColorArray)
setup OpenGL arrays
Definition: OcTreeDrawer.cpp:250
octomap::OcTreeDrawer::m_grid_voxels
std::list< octomap::OcTreeVolume > m_grid_voxels
Definition: OcTreeDrawer.h:142
octomath::Quaternion::rotate
Vector3 rotate(const Vector3 &v) const
octomap::OcTreeDrawer::m_occupiedThresSize
unsigned int m_occupiedThresSize
Definition: OcTreeDrawer.h:123
octomap::OcTreeDrawer::setAlphaOccupied
void setAlphaOccupied(double alpha)
sets alpha level for occupied cells
Definition: OcTreeDrawer.cpp:133
octomap::OcTreeDrawer::origin
octomap::pose6d origin
Definition: OcTreeDrawer.h:156
octomath::Pose6D::rot
Quaternion & rot()
octomap::pose6d
octomath::Pose6D pose6d
octomap::OcTreeDrawer::m_occupiedSize
unsigned int m_occupiedSize
Definition: OcTreeDrawer.h:127
octomap::OcTreeDrawer::setCubeColorRGBA
unsigned int setCubeColorRGBA(const unsigned char &r, const unsigned char &g, const unsigned char &b, const unsigned char &a, const unsigned int &current_array_idx, GLfloat **glColorArray)
Definition: OcTreeDrawer.cpp:468
octomap::OcTreeDrawer::m_selectionArray
GLfloat ** m_selectionArray
Definition: OcTreeDrawer.h:130
octomap::AbstractOccupancyOcTree::isNodeOccupied
bool isNodeOccupied(const OcTreeNode *occupancyNode) const
octomap::OcTreeDrawer::m_freeSize
unsigned int m_freeSize
Definition: OcTreeDrawer.h:129
octomap::OcTreeDrawer::m_occupiedThresColorArray
GLfloat * m_occupiedThresColorArray
Color array for occupied cells (height)
Definition: OcTreeDrawer.h:134
octomap::OcTreeDrawer::setOrigin
void setOrigin(octomap::pose6d t)
Definition: OcTreeDrawer.cpp:860
octomap::OcTreeDrawer::drawCubes
void drawCubes(GLfloat **cubeArray, unsigned int cubeArraySize, GLfloat *cubeColorArray=NULL) const
Definition: OcTreeDrawer.cpp:747
octomath::Pose6D::inv
Pose6D inv() const
octomap::SceneObject::heightMapColor
void heightMapColor(double h, GLfloat *glArrayPos) const
Definition: SceneObject.cpp:38
octomap::OcTreeDrawer::m_freeThresSize
unsigned int m_freeThresSize
Definition: OcTreeDrawer.h:125
QGLViewer::drawArrow
static void drawArrow(qreal length=1.0, qreal radius=-1.0, int nbSubdivisions=12)
Definition: qglviewer.cpp:3207
OTD_RAD2DEG
#define OTD_RAD2DEG
Definition: OcTreeDrawer.cpp:28

octovis
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Mon Feb 28 2022 22:58:16