pose_graph_vis3d.cpp
Go to the documentation of this file.
00001 // HOG-Man - Hierarchical Optimization for Pose Graphs on Manifolds
00002 // Copyright (C) 2010 G. Grisetti, R. Kümmerle, C. Stachniss
00003 //
00004 // This file is part of HOG-Man.
00005 // 
00006 // HOG-Man is free software: you can redistribute it and/or modify
00007 // it under the terms of the GNU General Public License as published by
00008 // the Free Software Foundation, either version 3 of the License, or
00009 // (at your option) any later version.
00010 // 
00011 // HOG-Man is distributed in the hope that it will be useful,
00012 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00013 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014 // GNU General Public License for more details.
00015 // 
00016 // You should have received a copy of the GNU General Public License
00017 // along with HOG-Man.  If not, see <http://www.gnu.org/licenses/>.
00018 
00019 #include "pose_graph_vis3d.h"
00020 
00021 #include "axes.h"
00022 #include "vrml_output.h"
00023 #include "primitives.h"
00024 #include "stuff/macros.h"
00025 #include "graph/posegraph3d.h"
00026 
00027 #define NUM_DRAW_LISTS (2)
00028 
00029 namespace AISNavigation {
00030 
00031 using namespace std;
00032 
00033 PoseGraph3DVis::PoseGraph3DVis() :
00034   _graph(0), _hirarchy(0),
00035   _updateDrawList(false), _useDrawList(false), _drawList(0), _listAllocated(false)
00036 {
00037   _colors.push_back(Color(1, 0, 0)); 
00038   _colors.push_back(Color(0, 1, 0)); 
00039   _colors.push_back(Color(0, 0, 1)); 
00040   _colors.push_back(Color(1, 1, 0)); 
00041   _colors.push_back(Color(1, 0, 1)); 
00042   _colors.push_back(Color(0, 1, 1)); 
00043   for (int i = 0; i < 100; ++i) { // that should be more than enough
00044     double r,g,b;
00045     GET_COLOR(i, 100, r, g, b);
00046     _colors.push_back(Color(r, g, b));
00047   }
00048   _drawOptions.resize(NUM_DRAW_LISTS, true);
00049 }
00050 
00051 PoseGraph3DVis::~PoseGraph3DVis()
00052 {
00053   if (_listAllocated) // clean the draw list if there was one allocated
00054     glDeleteLists(_drawList, NUM_DRAW_LISTS);
00055 }
00056 
00057 void PoseGraph3DVis::writeVrml(std::ostream& os) const
00058 {
00059   if (!_graph)
00060     return;
00061   writeVrmlFileHeader(os);
00062   os << Axes(1.0) << endl;
00063   writeStart(os); 
00064   vrmlLoadIdentity();
00065   if (_drawOptions[0])
00066     writeGraph(os);
00067   if (_drawOptions[1])
00068     writeHirarchy(os);
00069   writeEnd(os);
00070 }
00071 
00072 void PoseGraph3DVis::draw() const
00073 {
00074   if (!_graph)
00075     return;
00076 
00077   // turn off the light for the edges
00078   GLboolean hasLight = glIsEnabled(GL_LIGHTING);
00079   if (hasLight)
00080     glDisable(GL_LIGHTING);
00081 
00082   if (_useDrawList) { // handle update and creation of the draw list
00083     if (!_listAllocated) {
00084       _drawList = glGenLists(NUM_DRAW_LISTS);
00085       _listAllocated = true;
00086       _updateDrawList = true; // force update
00087     }
00088     if (_updateDrawList) {
00089       _updateDrawList = false;
00090       // graph
00091       if (_drawOptions[0])
00092         glNewList(_drawList, GL_COMPILE_AND_EXECUTE);
00093       else
00094         glNewList(_drawList, GL_COMPILE);
00095       drawGraph(); // draw into the list
00096       glEndList();
00097 
00098       // hirarchy
00099       if (_drawOptions[1])
00100         glNewList(_drawList+1, GL_COMPILE_AND_EXECUTE);
00101       else
00102         glNewList(_drawList+1, GL_COMPILE);
00103       drawHirarchy();
00104       glEndList();
00105     } else {
00106       for (GLuint l = 0; l < NUM_DRAW_LISTS; ++l)
00107         if (_drawOptions[l])
00108           glCallList(_drawList + l);
00109     }
00110   } else {
00111     if (_drawOptions[0])
00112       drawGraph(); // normal draw of the graph
00113     if (_drawOptions[1])
00114       drawHirarchy();
00115   }
00116 
00117   if (hasLight)
00118     glEnable(GL_LIGHTING);
00119 }
00120 
00121 void PoseGraph3DVis::setGraph(PoseGraph3D* graph)
00122 {
00123   _graph = graph;
00124   if (_useDrawList)
00125     _updateDrawList = true;
00126 }
00127 
00128 void PoseGraph3DVis::drawGraph() const
00129 {
00130   // display the nodes
00131   for (PoseGraph3D::VertexIDMap::const_iterator it = _graph->vertices().begin();
00132       it != _graph->vertices().end(); ++it) {
00133     const PoseGraph3D::Vertex* vi = static_cast<PoseGraph3D::Vertex*>(it->second);
00134     const Vector3& t = vi->transformation.translation();
00135     AxisAngle axis(vi->transformation.rotation());
00136     double angle = axis.norm();
00137     axis.normalize();
00138     glPushMatrix();
00139     glTranslatef(t.x(), t.y(), t.z());
00140     glRotatef(RAD2DEG(angle), axis.x(), axis.y(), axis.z());
00141     drawPoseBox();
00142     glPopMatrix();
00143   }
00144 
00145   // display the edges
00146   glColor3f(0.7f, 0.7f, 0.7f);
00147   glBegin(GL_LINES);
00148   for (PoseGraph3D::EdgeSet::const_iterator it = _graph->edges().begin();
00149       it != _graph->edges().end(); ++it) {
00150     const PoseGraph3D::Vertex* vi = static_cast<PoseGraph3D::Vertex*>((*it)->from());
00151     const PoseGraph3D::Vertex* vj = static_cast<PoseGraph3D::Vertex*>((*it)->to());
00152     const Vector3& t1 = vi->transformation.translation();
00153     const Vector3& t2 = vj->transformation.translation();
00154     glVertex3f(t1.x(), t1.y(), t1.z());
00155     glVertex3f(t2.x(), t2.y(), t2.z());
00156   }
00157   glEnd();
00158 }
00159 
00160 void PoseGraph3DVis::setHirarchy(std::vector<PoseGraph3DVis::HEdgeVector>* hirarchy)
00161 {
00162   _hirarchy = hirarchy;
00163 }
00164 
00165 void PoseGraph3DVis::drawHirarchy() const
00166 {
00167   if (!_hirarchy || !_graph)
00168     return;
00169 
00170   int l = 0;
00171   glBegin(GL_LINES);
00172   for (std::vector<HEdgeVector>::const_iterator level = _hirarchy->begin(); level != _hirarchy->end(); ++level, ++l) {
00173     const HEdgeVector& hEdges = *level;
00174     glColor3f(_colors[l].r, _colors[l].g, _colors[l].b);
00175     for (HEdgeVector::const_iterator it = hEdges.begin(); it != hEdges.end(); ++it) {
00176       const PoseGraph3D::Vertex* vi = static_cast<PoseGraph3D::Vertex*>(_graph->vertex(it->id1));
00177       const PoseGraph3D::Vertex* vj = static_cast<PoseGraph3D::Vertex*>(_graph->vertex(it->id2));
00178       const Vector3& t1 = vi->transformation.translation();
00179       const Vector3& t2 = vj->transformation.translation();
00180       glVertex3f(t1.x(), t1.y(), t1.z());
00181       glVertex3f(t2.x(), t2.y(), t2.z());
00182     }
00183   }
00184   glEnd();
00185 }
00186 
00187 void PoseGraph3DVis::writeGraph(std::ostream& os) const
00188 {
00189   os << "# PoseGraph3DVis \n";
00190   // display the nodes
00191   for (PoseGraph3D::VertexIDMap::const_iterator it = _graph->vertices().begin();
00192       it != _graph->vertices().end(); ++it) {
00193     const PoseGraph3D::Vertex* vi = static_cast<PoseGraph3D::Vertex*>(it->second);
00194     const Vector3& t = vi->transformation.translation();
00195     AxisAngle axis(vi->transformation.rotation());
00196     double angle = axis.norm();
00197     axis.normalize();
00198     vrmlPushMatrix();
00199     vrmlTranslatef(t.x(), t.y(), t.z());
00200     vrmlRotatef(RAD2DEG(angle), axis.x(), axis.y(), axis.z());
00201     writePoseBox(os);
00202     vrmlPopMatrix();
00203   }
00204 
00205   // display the edges
00206   std::list<Vector3> line_points;
00207   vrmlColor3f(0.5f, 0.5f, 0.5f);
00208   for (PoseGraph3D::EdgeSet::const_iterator it = _graph->edges().begin();
00209       it != _graph->edges().end(); ++it) {
00210     const PoseGraph3D::Vertex* vi = static_cast<PoseGraph3D::Vertex*>((*it)->from());
00211     const PoseGraph3D::Vertex* vj = static_cast<PoseGraph3D::Vertex*>((*it)->to());
00212     const Vector3& t1 = vi->transformation.translation();
00213     const Vector3& t2 = vj->transformation.translation();
00214     line_points.push_back(t1);
00215     line_points.push_back(t2);
00216   }
00217   writeLines(os, line_points.begin(), line_points.end());
00218   os << "# PoseGraph3DVis Nodes end\n";
00219 }
00220 
00221 void PoseGraph3DVis::writeHirarchy(std::ostream& os) const
00222 {
00223   if (!_hirarchy || !_graph)
00224     return;
00225 
00226   int l = 0;
00227   std::list<Vector3> line_points;
00228   for (std::vector<HEdgeVector>::const_iterator level = _hirarchy->begin(); level != _hirarchy->end(); ++level, ++l) {
00229     const HEdgeVector& hEdges = *level;
00230     vrmlColor3f(_colors[l].r, _colors[l].g, _colors[l].b);
00231     for (HEdgeVector::const_iterator it = hEdges.begin(); it != hEdges.end(); ++it) {
00232       const PoseGraph3D::Vertex* vi = static_cast<PoseGraph3D::Vertex*>(_graph->vertex(it->id1));
00233       const PoseGraph3D::Vertex* vj = static_cast<PoseGraph3D::Vertex*>(_graph->vertex(it->id2));
00234       const Vector3& t1 = vi->transformation.translation();
00235       const Vector3& t2 = vj->transformation.translation();
00236       line_points.push_back(t1);
00237       line_points.push_back(t2);
00238     }
00239   }
00240   writeLines(os, line_points.begin(), line_points.end());
00241 }
00242 
00243 void PoseGraph3DVis::setDrawGraph(bool draw)
00244 {
00245   _drawOptions[0] = draw;
00246 }
00247 
00248 void PoseGraph3DVis::setDrawHirarchy(bool draw)
00249 {
00250   _drawOptions[1] = draw;
00251 }
00252 
00253 } // end namespace


hogman_minimal
Author(s): Maintained by Juergen Sturm
autogenerated on Mon Oct 6 2014 00:06:58