Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "vertex_se2.h"
00018 #include <typeinfo>
00019
00020 #ifdef WINDOWS
00021 #include <windows.h>
00022 #endif
00023
00024 #ifdef G2O_HAVE_OPENGL
00025 #ifdef __APPLE__
00026 #include <OpenGL/gl.h>
00027 #else
00028 #include <GL/gl.h>
00029 #endif
00030 #endif
00031
00032 namespace g2o {
00033
00034 VertexSE2::VertexSE2() :
00035 BaseVertex<3, SE2>()
00036 {
00037 }
00038
00039 bool VertexSE2::read(std::istream& is)
00040 {
00041 Eigen::Vector3d p;
00042 is >> p[0] >> p[1] >> p[2];
00043 estimate().fromVector(p);
00044 return true;
00045 }
00046
00047 bool VertexSE2::write(std::ostream& os) const
00048 {
00049 Eigen::Vector3d p = estimate().toVector();
00050 os << p[0] << " " << p[1] << " " << p[2];
00051 return os.good();
00052 }
00053
00054 VertexSE2WriteGnuplotAction::VertexSE2WriteGnuplotAction(): WriteGnuplotAction(typeid(VertexSE2).name()){}
00055
00056 HyperGraphElementAction* VertexSE2WriteGnuplotAction::operator()(HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params_){
00057 if (typeid(*element).name()!=_typeName)
00058 return 0;
00059 WriteGnuplotAction::Parameters* params=static_cast<WriteGnuplotAction::Parameters*>(params_);
00060 if (!params || !params->os){
00061 std::cerr << __PRETTY_FUNCTION__ << ": warning, no valid output stream specified" << std::endl;
00062 return 0;
00063 }
00064
00065 VertexSE2* v = static_cast<VertexSE2*>(element);
00066 *(params->os) << v->estimate().translation().x() << " " << v->estimate().translation().y()
00067 << " " << v->estimate().rotation().angle() << std::endl;
00068 return this;
00069 }
00070
00071 #ifdef G2O_HAVE_OPENGL
00072 VertexSE2DrawAction::VertexSE2DrawAction(): DrawAction(typeid(VertexSE2).name()){}
00073
00074 HyperGraphElementAction* VertexSE2DrawAction::operator()(HyperGraph::HyperGraphElement* element,
00075 HyperGraphElementAction::Parameters* ){
00076 if (typeid(*element).name()!=_typeName)
00077 return 0;
00078 VertexSE2* that = static_cast<VertexSE2*>(element);
00079 glColor3f(0.5,0.5,0.8);
00080 glPushAttrib(GL_ENABLE_BIT);
00081 glDisable(GL_LIGHTING);
00082 glPushMatrix();
00083 glTranslatef(that->estimate().translation().x(),that->estimate().translation().y(),0.);
00084 glRotatef(RAD2DEG(that->estimate().rotation().angle()),0.,0.,1.);
00085 glBegin(GL_TRIANGLE_FAN);
00086 glVertex3f( 0.1 ,0. ,0.);
00087 glVertex3f(-0.1 ,-0.05, 0.);
00088 glVertex3f(-0.1 , 0.05, 0.);
00089 glEnd();
00090 glPopMatrix();
00091 glPopAttrib();
00092 return this;
00093 }
00094 #endif
00095
00096
00097 }