graph_optimizer_hchol.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 _GRAPH_OPTIMIZER_HCHOL_HH_
00018 #define _GRAPH_OPTIMIZER_HCHOL_HH_
00019 
00020 #include "graph_optimizer_chol.h"
00021 
00022 namespace AISNavigation{
00023 
00024   template <typename PG>
00025   struct HCholOptimizer: public CholOptimizer<PG>
00026   {
00027     friend struct HVertex;
00028 
00029     struct HVertex: public PG::Vertex
00030     {
00031       friend class HCholOptimizer;
00032       virtual ~HVertex();
00033     protected:
00034       HVertex(int id=-1);
00035       inline typename PG::Vertex* root() {return _root;}
00036       inline const typename PG:: Vertex* root() const {return _root;}
00037       inline std::set<HVertex*>& children() {return _children;}
00038       inline const std::set<HVertex*>& children() const {return _children;}
00039       inline typename PG::Vertex* lowerRoot() {return _lowerRoot;}
00040       inline const typename PG:: Vertex* lowerRoot() const {return _lowerRoot;}
00041 
00042       
00043       inline HVertex* parentVertex() { return _parentVertex;}
00044       inline HCholOptimizer<PG>*  parentOptimizer(){ return _optimizer->_upperOptimizer; }
00045       inline HCholOptimizer<PG>*  optimizer() {return _optimizer; }
00046       inline typename PG::Edge* edgeToRoot() {return _edgeToRoot;}
00047       void taint();
00048       void detachChildren();
00049     protected:
00050       HVertex* _parentVertex;
00051       HCholOptimizer<PG>* _optimizer;
00052       HVertex* _lowerRoot;
00053 
00054       HVertex* _root;
00055       double _distanceToRoot;
00056       typename PG::Edge* _edgeToRoot;
00057 
00058       std::set<HVertex*> _children;
00059       bool _tainted;
00060     };
00061 
00062     typedef std::set<HVertex*> HVertexSet;
00063 
00064     HCholOptimizer(double maxDistance=3.);
00065     HCholOptimizer(HCholOptimizer<PG>* lowerLevel, double maxDistance=3.);
00066     HCholOptimizer(int nLevels, double maxDistance=3.);
00067     virtual ~HCholOptimizer();
00068 
00069     inline bool& online() {return _online;}
00070     inline double& maxDistance() {return _maxDistance;} 
00071     inline bool& propagateDown() {return _propagateDown;}
00072     inline int& edgeAnnotationIterations() {return _edgeAnnotationIncrementalIterations;}
00073     inline int& globalIncrementalIterations() {return _globalIncrementalIterations; }
00074 
00075     virtual typename PG::Vertex* addVertex(const int& k);
00076     virtual typename PG::Vertex* addVertex(int id, const typename PG::TransformationType& pose, const typename PG::InformationType& information);
00077     virtual typename PG::Edge*   addEdge(typename PG::Vertex* from, typename PG::Vertex* to,
00078         const typename PG::TransformationType& mean, const typename PG::InformationType& information);
00079     virtual bool removeEdge(Graph::Edge* e);
00080     virtual bool removeVertex(Graph::Vertex* v);
00081     virtual void refineEdge(typename PG::Edge* _e, const typename PG::TransformationType& mean, const typename PG::InformationType& information);
00082     virtual int optimize(int iterations, bool online=false);
00083 
00084     // for benchmark;
00085     void annotateHiearchicalEdgeOnDenseGraph(typename PG::TransformationType& mean, typename PG::InformationType& info, typename PG::Edge* e, int iterations, double lambda, bool initWithObservations);
00086     void computeTopLevelDenseGraph(CholOptimizer<PG>* chol, int iterations, int lambda, int initWithObservations);
00087     int nLevels() const;
00088     HCholOptimizer<PG>* level(int i);
00089   protected:
00090 
00091     // general functions
00092     //bool updateEdgeStructure(Edge* e);
00093     void annotateHiearchicalEdge(typename PG::Edge* e, int iterations, double lambda, bool initWithObservations);
00094 
00095 
00096     // batch optimization
00097     void annotateHiearchicalEdges(int iterations, double lambda, bool initWithObservations);
00098     void bottomToTop(int iterations, double lambda, bool initWithObservations);
00099     void topToBottom(int iterations, double lambda);
00100     void vCycle(int iterations, double lambdaUp, double lambdaDown, bool initWithObservations);
00101 
00102     // incremental optimization
00103     //void computeHierarchicalEdgesIncremental(HVertex* from, HVertex* to);
00104     bool optimizeLevels(bool propagateDown);
00105     void propagateDownIncremental(HVertex* to);
00106     void propagateDownIncremental(HVertex* to, double lambda);
00107     bool optimizeUpperLevelIncremental();
00108 
00109     void annotateHierarchicalEdgesIncremental(HVertex* v);
00110     double cachedChi() const {return _cachedChi;}
00111     void postprocessIncremental(HVertex* v);
00112     void addVertexToUpperLevels(HVertex* v);
00113     bool optimizePendingIncremental();
00114     void updateStructure(bool incremental);
00115     void detachChildren(HVertex* v);
00116     void cleanupTainted();
00117     virtual void clear();
00118 
00119     HCholOptimizer<PG>* _lowerOptimizer;
00120     HCholOptimizer<PG>* _upperOptimizer;
00121     
00122     bool _online;
00123     double _maxDistance;
00124     double _cachedChi;
00125     double _lastOptChi;
00126     bool _gnuplot; // this overrides the standard gnuplot variable :).
00127     bool _propagateDown;
00128     
00129     double _translationalPropagationError;
00130     double _rotationalPropagationError;
00131     int _globalIncrementalIterations;
00132     int _downIncrementalIterations;
00133     int _edgeAnnotationIncrementalIterations;
00134     int _nVerticesPropagatedDownIncremental;
00135 
00136     std::set<int> _rootIDs;
00137 
00138   };
00139 
00140 } // end namespace
00141 
00142 #include "graph_optimizer_hchol.hpp"
00143 #include "graph_optimizer_hchol_aux.hpp"
00144 #include "graph_optimizer_hchol_batch.hpp"
00145 #include "graph_optimizer_hchol_incremental.hpp"
00146 
00147 #endif


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