edge_se2_multi.h
Go to the documentation of this file.
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_MULTI_H
00018 #define EDGE_SE2_MULTI_H
00019 
00020 #include "vertex_se2.h"
00021 #include "g2o/core/base_multi_edge.h"
00022 
00023 namespace g2o {
00024 
00028   class EdgeSE2Multi: public BaseMultiEdge<3, SE2>
00029   {
00030     public:
00031       EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00032       EdgeSE2Multi();
00033 
00034       virtual bool read(std::istream& is);
00035       virtual bool write(std::ostream& os) const;
00036 
00037       void computeError()
00038       {
00039         const VertexSE2* v1 = static_cast<const VertexSE2*>(_vertices[0]);
00040         const VertexSE2* v2 = static_cast<const VertexSE2*>(_vertices[1]);
00041         SE2 delta = _inverseMeasurement * (v1->estimate().inverse()*v2->estimate());
00042         _error = delta.toVector();
00043       }
00044   };
00045 
00046 }
00047 
00048 #endif


re_vision
Author(s): Dorian Galvez-Lopez
autogenerated on Sun Jan 5 2014 11:31:02