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 TUTORIAL_EDGE_SE2_POINT_XY_H 00018 #define TUTORIAL_EDGE_SE2_POINT_XY_H 00019 00020 #include "vertex_se2.h" 00021 #include "vertex_point_xy.h" 00022 #include "g2o/core/base_binary_edge.h" 00023 00024 namespace g2o { 00025 00026 namespace tutorial { 00027 00028 class EdgeSE2PointXY : public BaseBinaryEdge<2, Eigen::Vector2d, VertexSE2, VertexPointXY> 00029 { 00030 public: 00031 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00032 EdgeSE2PointXY(); 00033 00034 void computeError() 00035 { 00036 const VertexSE2* v1 = static_cast<const VertexSE2*>(_vertices[0]); 00037 const VertexPointXY* l2 = static_cast<const VertexPointXY*>(_vertices[1]); 00038 _error = (v1->estimate().inverse() * l2->estimate()) - _measurement; 00039 } 00040 00041 virtual bool read(std::istream& is); 00042 virtual bool write(std::ostream& os) const; 00043 00044 }; 00045 00046 } // end namespace 00047 } // end namespace 00048 00049 #endif