graph_optimizer.h
Go to the documentation of this file.
00001 // HOG-Man - Hierarchical Optimization for Pose Graphs on Manifolds
00002 // Copyright (C) 2010 G. Grisetti, R. Kümmerle, C. Stachniss
00003 // 
00004 // HOG-Man 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 // HOG-Man 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 AIS_GRAPH_OPTIMIZER_H
00018 #define AIS_GRAPH_OPTIMIZER_H
00019 
00020 #include "graph/posegraph.h"
00021 #include "hogman_minimal/math/transformation.h"
00022 
00023 #ifndef _MY_CAST_
00024 #define _MY_CAST_ reinterpret_cast
00025 #endif
00026 
00027 namespace AISNavigation {
00028 
00032   template <typename PG>
00033   class GraphOptimizer : public PG
00034   {
00035     public:
00036       struct EdgeCmpNodeId {
00037         bool operator()(const typename PG::Edge* e1, const typename PG::Edge* e2) const
00038         {
00039           const typename PG::Vertex* v1from = static_cast<const typename PG::Vertex*>(e1->from());
00040           const typename PG::Vertex* v1to   = static_cast<const typename PG::Vertex*>(e1->to());
00041           const typename PG::Vertex* v2from = static_cast<const typename PG::Vertex*>(e2->from());
00042           const typename PG::Vertex* v2to   = static_cast<const typename PG::Vertex*>(e2->to());
00043           return v1from->id() < v2from->id() || (v1from->id() == v2from->id() && v1to->id() < v2to->id());
00044         }
00045       };
00046       typedef std::map< typename PG::Edge*, double, EdgeCmpNodeId> ChiStatMap;
00047 
00048     public:
00049       GraphOptimizer();
00050       virtual ~GraphOptimizer();
00051 
00052       virtual bool initialize(int rootNode=-1)=0;
00053       virtual int optimize(int iterations, bool online=false)=0;
00054 
00055       double chi2() const;
00056       static double chi2(const typename PG::Edge* e);
00057       static void absChi(double& rotationalError, double& translationalError, typename PG::Edge* e_);
00058       void chiStat(ChiStatMap& emap);
00059       void sqError(double& are, double& ate, double& mte, double& mre, const typename PG::EdgeSet* eset=0) const;
00060 
00061       virtual const bool& verbose() const { return _verbose; }
00062       virtual bool& verbose() { return _verbose; }
00063       virtual const bool& visualizeToStdout() const { return _visualizeToStdout; }
00064       virtual bool& visualizeToStdout() { return _visualizeToStdout; }
00065       virtual const bool& guessOnEdges() const { return _guessOnEdges;}
00066       virtual bool& guessOnEdges() { return _guessOnEdges;}
00067 
00068       virtual void backup();
00069       virtual void restore();
00070     protected:
00071       virtual void backupSubset(typename PG::VertexSet& vset);
00072       virtual void backupSubset(Graph::VertexSet& vset);
00073       virtual void restoreSubset(typename PG::VertexSet& vset);
00074       virtual void restoreSubset(Graph::VertexSet& vset);
00075 
00076       bool _verbose;
00077       bool _visualizeToStdout;
00078       bool _guessOnEdges;
00079 
00080       using PG::_vertices;
00081       using PG::_edges;
00082   };
00083   
00084   #include "graph_optimizer.hpp"
00085 
00086 
00087 } // end namespace
00088 
00089 #endif


hogman_minimal
Author(s): Maintained by Juergen Sturm
autogenerated on Mon Oct 6 2014 00:06:58