00001 // g2o - General Graph Optimization 00002 // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard 00003 // 00004 // g2o is free software: you can redistribute it and/or modify 00005 // it under the terms of the GNU Lesser General Public License as published 00006 // by the Free Software Foundation, either version 3 of the License, or 00007 // (at your option) any later version. 00008 // 00009 // g2o is distributed in the hope that it will be useful, 00010 // but WITHOUT ANY WARRANTY; without even the implied warranty of 00011 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00012 // GNU Lesser General Public License for more details. 00013 // 00014 // You should have received a copy of the GNU Lesser General Public License 00015 // along with this program. If not, see <http://www.gnu.org/licenses/>. 00016 00017 #include "vertex_point_xy.h" 00018 00019 #ifdef WINDOWS 00020 #include <windows.h> 00021 #endif 00022 00023 #ifdef G2O_HAVE_OPENGL 00024 #ifdef __APPLE__ 00025 #include <OpenGL/gl.h> 00026 #else 00027 #include <GL/gl.h> 00028 #endif 00029 #endif 00030 00031 #include <typeinfo> 00032 00033 #include "g2o/stuff/macros.h" 00034 00035 namespace g2o { 00036 00037 VertexPointXY::VertexPointXY() : 00038 BaseVertex<2, Vector2d>() 00039 { 00040 _estimate.setZero(); 00041 } 00042 00043 bool VertexPointXY::read(std::istream& is) 00044 { 00045 is >> estimate()[0] >> estimate()[1]; 00046 return true; 00047 } 00048 00049 bool VertexPointXY::write(std::ostream& os) const 00050 { 00051 os << estimate()(0) << " " << estimate()(1); 00052 return os.good(); 00053 } 00054 00055 VertexPointXYWriteGnuplotAction::VertexPointXYWriteGnuplotAction(): WriteGnuplotAction(typeid(VertexPointXY).name()){} 00056 00057 HyperGraphElementAction* VertexPointXYWriteGnuplotAction::operator()(HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params_){ 00058 if (typeid(*element).name()!=_typeName) 00059 return 0; 00060 00061 WriteGnuplotAction::Parameters* params=static_cast<WriteGnuplotAction::Parameters*>(params_); 00062 if (!params->os){ 00063 std::cerr << __PRETTY_FUNCTION__ << ": warning, on valid os specified" << std::endl; 00064 return false; 00065 } 00066 00067 VertexPointXY* v = static_cast<VertexPointXY*>(element); 00068 *(params->os) << v->estimate().x() << " " << v->estimate().y() << std::endl; 00069 return this; 00070 } 00071 00072 #ifdef G2O_HAVE_OPENGL 00073 VertexPointXYDrawAction::VertexPointXYDrawAction(): DrawAction(typeid(VertexPointXY).name()){} 00074 00075 HyperGraphElementAction* VertexPointXYDrawAction::operator()(HyperGraph::HyperGraphElement* element, 00076 HyperGraphElementAction::Parameters* /*params_*/ ){ 00077 00078 if (typeid(*element).name()!=_typeName) 00079 return 0; 00080 VertexPointXY* that = static_cast<VertexPointXY*>(element); 00081 glColor3f(0.8,0.5,0.3); 00082 glBegin(GL_POINTS); 00083 glVertex3f(that->estimate().x(),that->estimate().y(),0.); 00084 glEnd(); 00085 return this; 00086 } 00087 #endif 00088 00089 } // end namespace