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 #ifndef VERTEX_POINT_XY_H 00018 #define VERTEX_POINT_XY_H 00019 00020 #include "g2o/config.h" 00021 #include "g2o/core/base_vertex.h" 00022 #include "g2o/core/hyper_graph_action.h" 00023 00024 #include <Eigen/Core> 00025 00026 namespace g2o { 00027 00028 class VertexPointXY : public BaseVertex<2, Eigen::Vector2d> 00029 { 00030 public: 00031 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00032 VertexPointXY(); 00033 00034 virtual void setToOrigin() { 00035 _estimate.setZero(); 00036 } 00037 00038 virtual bool setEstimateData(const double* est){ 00039 _estimate[0]=est[0]; 00040 _estimate[1]=est[1]; 00041 return true; 00042 } 00043 00044 virtual bool getEstimateData(double* est) const{ 00045 est[0] = _estimate[0]; 00046 est[0] = _estimate[1]; 00047 return true; 00048 } 00049 00050 virtual int estimateDimension() const { 00051 return 2; 00052 } 00053 00054 virtual bool setMinimalEstimateData(const double* est){ 00055 return setEstimateData(est); 00056 } 00057 00058 virtual bool getMinimalEstimateData(double* est) const{ 00059 return getEstimateData(est); 00060 } 00061 00062 virtual int minimalEstimateDimension() const { 00063 return 2; 00064 } 00065 00066 virtual void oplus(double* update) 00067 { 00068 _estimate[0] += update[0]; 00069 _estimate[1] += update[1]; 00070 } 00071 00072 virtual bool read(std::istream& is); 00073 virtual bool write(std::ostream& os) const; 00074 00075 }; 00076 00077 class VertexPointXYWriteGnuplotAction: public WriteGnuplotAction { 00078 public: 00079 VertexPointXYWriteGnuplotAction(); 00080 virtual HyperGraphElementAction* operator()(HyperGraph::HyperGraphElement* element, 00081 HyperGraphElementAction::Parameters* params_); 00082 }; 00083 00084 #ifdef G2O_HAVE_OPENGL 00085 class VertexPointXYDrawAction: public DrawAction{ 00086 public: 00087 VertexPointXYDrawAction(); 00088 virtual HyperGraphElementAction* operator()(HyperGraph::HyperGraphElement* element, 00089 HyperGraphElementAction::Parameters* params_); 00090 }; 00091 #endif 00092 00093 } 00094 00095 #endif