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