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 EDGE_SE2_POINT_XY_H 00018 #define EDGE_SE2_POINT_XY_H 00019 00020 #include "g2o/config.h" 00021 #include "vertex_se2.h" 00022 #include "vertex_point_xy.h" 00023 #include "g2o/core/base_binary_edge.h" 00024 00025 namespace g2o { 00026 00027 class EdgeSE2PointXY : public BaseBinaryEdge<2, Eigen::Vector2d, VertexSE2, VertexPointXY> 00028 { 00029 public: 00030 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00031 EdgeSE2PointXY(); 00032 00033 void computeError() 00034 { 00035 const VertexSE2* v1 = static_cast<const VertexSE2*>(_vertices[0]); 00036 const VertexPointXY* l2 = static_cast<const VertexPointXY*>(_vertices[1]); 00037 _error = (v1->estimate().inverse() * l2->estimate()) - _measurement; 00038 } 00039 00040 virtual bool setMeasurementData(const double* d){ 00041 _measurement[0]=d[0]; 00042 _measurement[1]=d[1]; 00043 _inverseMeasurement = -_measurement; 00044 return true; 00045 } 00046 00047 virtual bool getMeasurementData(double* d) const{ 00048 d[0] = _measurement[0]; 00049 d[1] = _measurement[1]; 00050 return true; 00051 } 00052 00053 virtual int measurementDimension() const {return 2;} 00054 00055 virtual bool setMeasurementFromState(){ 00056 const VertexSE2* v1 = static_cast<const VertexSE2*>(_vertices[0]); 00057 const VertexPointXY* l2 = static_cast<const VertexPointXY*>(_vertices[1]); 00058 _measurement = v1->estimate().inverse() * l2->estimate(); 00059 _inverseMeasurement = - _measurement; 00060 return true; 00061 } 00062 00063 virtual bool read(std::istream& is); 00064 virtual bool write(std::ostream& os) const; 00065 00066 virtual void initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to); 00067 virtual double initialEstimatePossible(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to) { (void) to; return (from.count(_vertices[0]) == 1 ? 1.0 : -1.0);} 00068 #ifndef NUMERIC_JACOBIAN_TWO_D_TYPES 00069 virtual void linearizeOplus(); 00070 #endif 00071 }; 00072 00073 class EdgeSE2PointXYWriteGnuplotAction: public WriteGnuplotAction { 00074 public: 00075 EdgeSE2PointXYWriteGnuplotAction(); 00076 virtual HyperGraphElementAction* operator()(HyperGraph::HyperGraphElement* element, 00077 HyperGraphElementAction::Parameters* params_); 00078 }; 00079 00080 #ifdef G2O_HAVE_OPENGL 00081 class EdgeSE2PointXYDrawAction: public DrawAction{ 00082 public: 00083 EdgeSE2PointXYDrawAction(); 00084 virtual HyperGraphElementAction* operator()(HyperGraph::HyperGraphElement* element, 00085 HyperGraphElementAction::Parameters* params_); 00086 }; 00087 #endif 00088 00089 } // end namespace 00090 00091 #endif