27 #define OTD_RAD2DEG 57.2957795 32 m_occupiedThresSize(0), m_freeThresSize(0),
33 m_occupiedSize(0), m_freeSize(0), m_selectionSize(0),
34 octree_grid_vertex_size(0), m_alphaOccupied(0.8), map_id(0)
70 glTranslatef(relative_transform.
x(), relative_transform.
y(), relative_transform.
z());
73 float scale = sqrt(q.
x() * q.
x() + q.
y() * q.
y() + q.
z() * q.
z());
75 float axis_x = q.
x() / scale;
76 float axis_y = q.
y() / scale;
77 float axis_z = q.
z() / scale;
79 glRotatef(angle, axis_x, axis_y, axis_z);
82 glEnableClientState(GL_VERTEX_ARRAY);
97 glDisableClientState(GL_VERTEX_ARRAY);
121 bool showAll = (octree.
size() < 5 * 1e6);
122 bool uses_origin = ( (origin.
rot().
x() != 0.) && (origin.
rot().
y() != 0.)
123 && (origin.
rot().
z() != 0.) && (origin.
rot().
u() != 1.) );
128 unsigned int cnt_occupied(0), cnt_occupied_thres(0), cnt_free(0), cnt_free_thres(0);
129 for(OcTree::tree_iterator it = octree.
begin_tree(this->m_max_tree_depth),
130 end=octree.
end_tree(); it!= end; ++it) {
148 double minX, minY, minZ, maxX, maxY, maxZ;
156 std::vector<octomath::Vector3> cube_template;
159 unsigned int idx_occupied(0), idx_occupied_thres(0), idx_free(0), idx_free_thres(0);
160 unsigned int color_idx_occupied(0), color_idx_occupied_thres(0);
164 for(OcTree::tree_iterator it = octree.
begin_tree(this->m_max_tree_depth),
165 end=octree.
end_tree(); it!= end; ++it) {
219 unsigned int& glArraySize,
220 GLfloat*** glArray, GLfloat** glColorArray) {
222 clearCubes(glArray, glArraySize, glColorArray);
225 glArraySize = num_cubes * 4 * 3;
228 *glArray =
new GLfloat* [6];
229 for (
unsigned i = 0; i<6; ++i){
230 (*glArray)[i] =
new GLfloat[glArraySize];
233 if (glColorArray != NULL)
234 *glColorArray =
new GLfloat[glArraySize * 4 *4];
238 std::vector<octomath::Vector3>& cube_template) {
239 cube_template.clear();
240 cube_template.reserve(24);
272 const std::vector<octomath::Vector3>& cube_template,
273 const unsigned int& current_array_idx,
274 GLfloat*** glArray) {
282 double half_cube_size = GLfloat(v.second /2.0 -eps);
283 unsigned int i = current_array_idx;
288 p = v.first + cube_template[0] * half_cube_size;
289 (*glArray)[0][i] = p.
x();
290 (*glArray)[0][i+1] = p.
y();
291 (*glArray)[0][i+2] = p.
z();
293 p = v.first + cube_template[1] * half_cube_size;
294 (*glArray)[1][i] = p.
x();
295 (*glArray)[1][i+1] = p.
y();
296 (*glArray)[1][i+2] = p.
z();
298 p = v.first + cube_template[2] * half_cube_size;
299 (*glArray)[2][i] = p.
x();
300 (*glArray)[2][i+1] = p.
y();
301 (*glArray)[2][i+2] = p.
z();
303 p = v.first + cube_template[3] * half_cube_size;
304 (*glArray)[3][i] = p.
x();
305 (*glArray)[3][i+1] = p.
y();
306 (*glArray)[3][i+2] = p.
z();
308 p = v.first + cube_template[4] * half_cube_size;
309 (*glArray)[4][i] = p.
x();
310 (*glArray)[4][i+1] = p.
y();
311 (*glArray)[4][i+2] = p.
z();
313 p = v.first + cube_template[5] * half_cube_size;
314 (*glArray)[5][i] = p.
x();
315 (*glArray)[5][i+1] = p.
y();
316 (*glArray)[5][i+2] = p.
z();
319 p = v.first + cube_template[6] * half_cube_size;
320 (*glArray)[0][i] = p.
x();
321 (*glArray)[0][i+1] = p.
y();
322 (*glArray)[0][i+2] = p.
z();
324 p = v.first + cube_template[7] * half_cube_size;
325 (*glArray)[1][i] = p.
x();
326 (*glArray)[1][i+1] = p.
y();
327 (*glArray)[1][i+2] = p.
z();
329 p = v.first + cube_template[8] * half_cube_size;
330 (*glArray)[2][i] = p.
x();
331 (*glArray)[2][i+1] = p.
y();
332 (*glArray)[2][i+2] = p.
z();
334 p = v.first + cube_template[9] * half_cube_size;
335 (*glArray)[3][i] = p.
x();
336 (*glArray)[3][i+1] = p.
y();
337 (*glArray)[3][i+2] = p.
z();
339 p = v.first + cube_template[10] * half_cube_size;
340 (*glArray)[4][i] = p.
x();
341 (*glArray)[4][i+1] = p.
y();
342 (*glArray)[4][i+2] = p.
z();
344 p = v.first + cube_template[11] * half_cube_size;
345 (*glArray)[5][i] = p.
x();
346 (*glArray)[5][i+1] = p.
y();
347 (*glArray)[5][i+2] = p.
z();
350 p = v.first + cube_template[12] * half_cube_size;
351 (*glArray)[0][i] = p.
x();
352 (*glArray)[0][i+1] = p.
y();
353 (*glArray)[0][i+2] = p.
z();
355 p = v.first + cube_template[13] * half_cube_size;
356 (*glArray)[1][i] = p.
x();
357 (*glArray)[1][i+1] = p.
y();
358 (*glArray)[1][i+2] = p.
z();
360 p = v.first + cube_template[14] * half_cube_size;
361 (*glArray)[2][i] = p.
x();
362 (*glArray)[2][i+1] = p.
y();
363 (*glArray)[2][i+2] = p.
z();
365 p = v.first + cube_template[15] * half_cube_size;
366 (*glArray)[3][i] = p.
x();
367 (*glArray)[3][i+1] = p.
y();
368 (*glArray)[3][i+2] = p.
z();
370 p = v.first + cube_template[16] * half_cube_size;
371 (*glArray)[4][i] = p.
x();
372 (*glArray)[4][i+1] = p.
y();
373 (*glArray)[4][i+2] = p.
z();
375 p = v.first + cube_template[17] * half_cube_size;
376 (*glArray)[5][i] = p.
x();
377 (*glArray)[5][i+1] = p.
y();
378 (*glArray)[5][i+2] = p.
z();
381 p = v.first + cube_template[18] * half_cube_size;
382 (*glArray)[0][i] = p.
x();
383 (*glArray)[0][i+1] = p.
y();
384 (*glArray)[0][i+2] = p.
z();
386 p = v.first + cube_template[19] * half_cube_size;
387 (*glArray)[1][i] = p.
x();
388 (*glArray)[1][i+1] = p.
y();
389 (*glArray)[1][i+2] = p.
z();
391 p = v.first + cube_template[20] * half_cube_size;
392 (*glArray)[2][i] = p.
x();
393 (*glArray)[2][i+1] = p.
y();
394 (*glArray)[2][i+2] = p.
z();
396 p = v.first + cube_template[21] * half_cube_size;
397 (*glArray)[3][i] = p.
x();
398 (*glArray)[3][i+1] = p.
y();
399 (*glArray)[3][i+2] = p.
z();
401 p = v.first + cube_template[22] * half_cube_size;
402 (*glArray)[4][i] = p.
x();
403 (*glArray)[4][i+1] = p.
y();
404 (*glArray)[4][i+2] = p.
z();
406 p = v.first + cube_template[23] * half_cube_size;
407 (*glArray)[5][i] = p.
x();
408 (*glArray)[5][i+1] = p.
y();
409 (*glArray)[5][i+2] = p.
z();
417 const unsigned int& current_array_idx,
418 GLfloat** glColorArray) {
420 if (glColorArray == NULL)
return current_array_idx;
422 unsigned int colorIdx = current_array_idx;
424 for (
int k = 0; k < 4; ++k) {
437 const unsigned char& g,
438 const unsigned char& b,
439 const unsigned char& a,
440 const unsigned int& current_array_idx,
441 GLfloat** glColorArray) {
443 if (glColorArray == NULL)
return current_array_idx;
444 unsigned int colorIdx = current_array_idx;
446 for (
int k = 0; k < 4; ++k) {
447 (*glColorArray)[colorIdx ] = (double) r/255.;
448 (*glColorArray)[colorIdx + 1] = (double) g/255.;
449 (*glColorArray)[colorIdx + 2] = (double) b/255.;
450 (*glColorArray)[colorIdx + 3] = (double) a/255.;
458 unsigned int& glArraySize,
459 GLfloat** glColorArray) {
460 if (glArraySize != 0) {
461 for (
unsigned i = 0; i < 6; ++i) {
462 delete[] (*glArray)[i];
468 if (glColorArray != NULL && *glColorArray != NULL) {
469 delete[] *glColorArray;
470 *glColorArray = NULL;
477 GLfloat*** glArray,
unsigned int& glArraySize,
479 GLfloat** glColorArray) {
481 unsigned int colorIdx = 0;
483 std::vector<octomath::Vector3> cube_template;
486 for (std::list<octomap::OcTreeVolume>::const_iterator it=voxels.begin();
487 it != voxels.end(); it++) {
491 if (glColorArray != NULL) {
492 for (std::list<octomap::OcTreeVolume>::const_iterator it=voxels.begin();
493 it != voxels.end(); it++) {
509 std::list<octomap::OcTreeVolume>::iterator it_rec;
514 x = it_rec->first.x();
515 y = it_rec->first.y();
516 z = it_rec->first.z();
518 double half_voxel_size = it_rec->second / 2.0;
648 glColor3f(0.784f, 0.66f, 0);
650 else if (this->
map_id == 1) {
651 glColor3f(0.68f, 0., 0.62f);
654 glColor3f(0., 0.784f, 0.725f);
662 glColor3f(0.6f, 0.6f, 0.6f);
665 glColor3f(0.1f, 0.1f, 0.1f);
688 glColor3f(0.5f, 0.5f, 0.5f);
691 glColor3f(0.9f, 0.9f, 0.9f);
710 glColor4f(1.0, 0.0, 0.0, 0.5);
716 GLfloat* cubeColorArray)
const {
717 if (cubeArraySize == 0 || cubeArray == NULL){
718 std::cerr <<
"Warning: GLfloat array to draw cubes appears to be empty, nothing drawn.\n";
723 GLfloat* curcol =
new GLfloat[4];
724 glGetFloatv(GL_CURRENT_COLOR, curcol);
729 glEnableClientState(GL_COLOR_ARRAY);
730 glColorPointer(4, GL_FLOAT, 0, cubeColorArray);
734 glNormal3f(0.0f, 1.0f, 0.0f);
735 glVertexPointer(3, GL_FLOAT, 0, cubeArray[0]);
736 glDrawArrays(GL_QUADS, 0, cubeArraySize / 3);
738 glNormal3f(0.0f, -1.0f, 0.0f);
739 glVertexPointer(3, GL_FLOAT, 0, cubeArray[1]);
740 glDrawArrays(GL_QUADS, 0, cubeArraySize / 3);
742 glNormal3f(1.0f, 0.0f, 0.0f);
743 glVertexPointer(3, GL_FLOAT, 0, cubeArray[2]);
744 glDrawArrays(GL_QUADS, 0, cubeArraySize / 3);
746 glNormal3f(-1.0f, 0.0f, 0.0f);
747 glVertexPointer(3, GL_FLOAT, 0, cubeArray[3]);
748 glDrawArrays(GL_QUADS, 0, cubeArraySize / 3);
750 glNormal3f(0.0f, 0.0f, -1.0f);
751 glVertexPointer(3, GL_FLOAT, 0, cubeArray[4]);
752 glDrawArrays(GL_QUADS, 0, cubeArraySize / 3);
754 glNormal3f(0.0f, 0.0f, 1.0f);
755 glVertexPointer(3, GL_FLOAT, 0, cubeArray[5]);
756 glDrawArrays(GL_QUADS, 0, cubeArraySize / 3);
759 && (cubeColorArray != NULL)){
760 glDisableClientState(GL_COLOR_ARRAY);
765 glDisable(GL_LIGHTING);
766 glHint (GL_LINE_SMOOTH_HINT, GL_NICEST);
767 glEnable (GL_LINE_SMOOTH);
768 glPolygonMode (GL_FRONT_AND_BACK, GL_LINE);
770 glColor3f(0.0f, 0.0f, 0.0f);
771 glCullFace(GL_FRONT_AND_BACK);
775 glNormal3f(0.0f, 1.0f, 0.0f);
776 glVertexPointer(3, GL_FLOAT, 0, cubeArray[0]);
777 glDrawArrays(GL_QUADS, 0, cubeArraySize / 3);
779 glNormal3f(0.0f, -1.0f, 0.0f);
780 glVertexPointer(3, GL_FLOAT, 0, cubeArray[1]);
781 glDrawArrays(GL_QUADS, 0, cubeArraySize / 3);
783 glNormal3f(1.0f, 0.0f, 0.0f);
784 glVertexPointer(3, GL_FLOAT, 0, cubeArray[2]);
785 glDrawArrays(GL_QUADS, 0, cubeArraySize / 3);
787 glNormal3f(-1.0f, 0.0f, 0.0f);
788 glVertexPointer(3, GL_FLOAT, 0, cubeArray[3]);
789 glDrawArrays(GL_QUADS, 0, cubeArraySize / 3);
794 glDisable(GL_LINE_SMOOTH);
795 glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
796 glEnable(GL_LIGHTING);
807 glDisable(GL_LIGHTING);
808 glEnable(GL_LINE_SMOOTH);
812 glColor3f(0.0, 0.0, 0.0);
815 glDisable(GL_LINE_SMOOTH);
816 glEnable(GL_LIGHTING);
829 std::cout <<
"OcTreeDrawer: setting new global origin: " << t << std::endl;
833 std::cout <<
"origin : " <<
origin << std::endl;
835 std::cout <<
"relative trans: " << relative_transform << std::endl;
844 float length = 0.15f;
846 GLboolean lighting, colorMaterial;
847 glGetBooleanv(GL_LIGHTING, &lighting);
848 glGetBooleanv(GL_COLOR_MATERIAL, &colorMaterial);
850 glDisable(GL_COLOR_MATERIAL);
860 if (angle > 0) glRotatef(
OTD_RAD2DEG*angle, ax, ay, az);
863 color[0] = 0.7f; color[1] = 0.7f; color[2] = 1.0f; color[3] = 1.0f;
864 glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, color);
867 color[0] = 1.0f; color[1] = 0.7f; color[2] = 0.7f; color[3] = 1.0f;
868 glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, color);
870 glRotatef(90.0, 0.0, 1.0, 0.0);
874 color[0] = 0.7f; color[1] = 1.0f; color[2] = 0.7f; color[3] = 1.0f;
875 glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, color);
877 glRotatef(-90.0, 1.0, 0.0, 0.0);
881 glTranslatef(relative_transform.
trans().
x(), relative_transform.
trans().
y(), relative_transform.
trans().
z());
884 glEnable(GL_COLOR_MATERIAL);
886 glDisable(GL_LIGHTING);
const tree_iterator end_tree() const
void setOcTree(const AbstractOcTree &octree)
sets a new OcTree that should be drawn by this drawer
static void drawArrow(float length=1.0f, float radius=-1.0f, int nbSubdivisions=12)
unsigned int setCubeColorHeightmap(const octomap::OcTreeVolume &v, const unsigned int ¤t_array_idx, GLfloat **glColorArray)
unsigned int octree_grid_vertex_size
void clearCubes(GLfloat ***glArray, unsigned int &glArraySize, GLfloat **glColorArray=NULL)
clear OpenGL visualization
GLfloat ** m_occupiedArray
GLfloat * octree_grid_vertex_array
OpenGL representation of Octree (grid structure)
void clearOcTreeSelection()
clear the visualization of the OcTree selection
void clearOcTreeStructure()
std::pair< point3d, double > OcTreeVolume
octomap::pose6d initial_origin
GLfloat * m_occupiedColorArray
void enableOcTree(bool enabled=true)
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
void drawFreeVoxels() const
void drawOctreeGrid() const
bool m_octree_grid_vis_initialized
GLfloat ** m_occupiedThresArray
OpenGL representation of Octree cells (cubes)
unsigned int m_selectionSize
void heightMapGray(double h, GLfloat *glArrayPos) const
void drawOccupiedVoxels() const
unsigned int generateCube(const octomap::OcTreeVolume &v, const std::vector< octomath::Vector3 > &cube_template, const unsigned int ¤t_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
std::list< octomap::OcTreeVolume > m_grid_voxels
void drawCubes(GLfloat **cubeArray, unsigned int cubeArraySize, GLfloat *cubeColorArray=NULL) const
unsigned int m_occupiedThresSize
void setAlphaOccupied(double alpha)
sets alpha level for occupied cells
unsigned int m_occupiedSize
unsigned int setCubeColorRGBA(const unsigned char &r, const unsigned char &g, const unsigned char &b, const unsigned char &a, const unsigned int ¤t_array_idx, GLfloat **glColorArray)
GLfloat ** m_selectionArray
GLfloat * m_occupiedThresColorArray
Color array for occupied cells (height)
void setOrigin(octomap::pose6d t)
bool isNodeOccupied(const OcTreeNode *occupancyNode) const
virtual size_t size() const
unsigned int m_freeThresSize