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


octovis
Author(s): Kai M. Wurm , Armin Hornung
autogenerated on Wed Jun 5 2019 19:26:39