$search
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