Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
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
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
00092
00093 void annotateHiearchicalEdge(typename PG::Edge* e, int iterations, double lambda, bool initWithObservations);
00094
00095
00096
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
00103
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;
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 }
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