posegraph.hh
Go to the documentation of this file.
00001 /**********************************************************************
00002  *
00003  * This source code is part of the Tree-based Network Optimizer (TORO)
00004  *
00005  * TORO Copyright (c) 2007 Giorgio Grisetti, Cyrill Stachniss, 
00006  *                         Slawomir Grzonka, and Wolfram Burgard
00007  *
00008  * TORO is licences under the Common Creative License,
00009  * Attribution-NonCommercial-ShareAlike 3.0
00010  *
00011  * You are free:
00012  *   - to Share - to copy, distribute and transmit the work
00013  *   - to Remix - to adapt the work
00014  *
00015  * Under the following conditions:
00016  *
00017  *   - Attribution. You must attribute the work in the manner specified
00018  *     by the author or licensor (but not in any way that suggests that
00019  *     they endorse you or your use of the work).
00020  *  
00021  *   - Noncommercial. You may not use this work for commercial purposes.
00022  *  
00023  *   - Share Alike. If you alter, transform, or build upon this work,
00024  *     you may distribute the resulting work only under the same or
00025  *     similar license to this one.
00026  *
00027  * Any of the above conditions can be waived if you get permission
00028  * from the copyright holder.  Nothing in this license impairs or
00029  * restricts the author's moral rights.
00030  *
00031  * TORO is distributed in the hope that it will be useful,
00032  * but WITHOUT ANY WARRANTY; without even the implied 
00033  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
00034  * PURPOSE.  
00035  **********************************************************************/
00036 
00043 #ifndef _TREEPOSEGRAPH_HXX_
00044 #define _TREEPOSEGRAPH_HXX_
00045 
00046 #include <iostream>
00047 #include <assert.h>
00048 
00049 #include <set>
00050 #include <list>
00051 #include <map>
00052 #include <deque>
00053 #include <vector>
00054 #include <limits>
00055 #include <algorithm>
00056 
00057 namespace AISNavigation{
00058 
00061 template <class E> 
00062 struct EVComparator{
00064   enum CompareMode {CompareLevel, CompareLength};
00065   CompareMode mode;
00066 
00067   EVComparator(){
00068     mode=CompareLevel;
00069   }
00070   inline bool operator() (const E& e1, const E& e2){
00071     int o1=0, o2=0;
00072     switch (mode){
00073     case CompareLevel:
00074       o1=e1->top->level;
00075       o2=e2->top->level;
00076       break;
00077     case CompareLength:
00078       o1=e1->length;
00079       o2=e2->length;
00080       break;
00081     }
00082     return o1<o2;
00083   }
00084 };
00085 
00090 template <class Ops>
00091 struct TreePoseGraph{
00092   typedef typename Ops::BaseType BaseType;
00093   typedef typename Ops::PoseType Pose;
00094   typedef typename Ops::RotationType Rotation;
00095   typedef typename Ops::TranslationType Translation;
00096   typedef typename Ops::TransformationType Transformation;
00097   typedef typename Ops::CovarianceType Covariance;
00098   typedef typename Ops::InformationType Information;
00099   typedef typename Ops::ParametersType  Parameters;
00100   
00101   struct Vertex;
00102 
00105   struct Edge{
00106     Vertex* v1;   
00107     Vertex* v2;   
00108     Vertex* top;  
00109     int length;   
00110     Transformation transformation;    
00111     Information    informationMatrix; 
00113     bool mark;
00114     
00115     double learningRate;
00116   };
00117 
00118   typedef typename EVComparator<Edge*>::CompareMode EdgeCompareMode;
00119   typedef typename std::list< Edge* > EdgeList;
00120   typedef typename std::map< int, Vertex* > VertexMap;
00121   typedef typename std::set< Vertex* > VertexSet;
00122   typedef typename std::map< Edge*, Edge* > EdgeMap;
00123   typedef typename std::multiset< Edge*, EVComparator<Edge*> > EdgeSet;
00124 
00127   struct Vertex {
00128 
00129     // Graph-related elements
00130     int id;         
00131     EdgeList edges; 
00133     // Tree-related elements
00134     int level;         
00135     Vertex* parent;    
00136     Edge* parentEdge;  
00137     EdgeList children;   
00139     // Parameterization-related elements
00140     Transformation transformation; 
00141     Pose pose;              
00142     Parameters parameters;  
00144     bool mark;
00145   };
00146 
00148   Vertex* vertex(int id);
00149 
00151   const Vertex* vertex (int id) const;
00152 
00154   Edge* edge(int id1, int id2);
00155 
00157   const Edge* edge(int id1, int id2) const;
00158 
00160   Vertex* addVertex(int id, const Pose& pose);
00162   Vertex* removeVertex (int id);
00163 
00165   Edge* addEdge(Vertex* v1, Vertex* v2, const Transformation& t, const Information& i);
00166 
00168   Edge* removeEdge(Edge* eq);
00169 
00184   Edge* addIncrementalEdge(int id1, int id2,  const Transformation& t, const Information& i);
00185 
00189   EdgeSet* affectedEdges(Vertex* v);
00190 
00191   EdgeSet* affectedEdges(VertexSet& vl);
00192 
00194   template <class Action>
00195   void treeBreadthVisit(Action& act);
00196 
00198   template <class Action>
00199   void treeDepthVisit(Action& act, Vertex *v);
00200 
00202   bool buildMST(int id);
00203 
00205   bool buildSimpleTree();
00206 
00208   void revertEdge(Edge* e);
00209 
00211   virtual void revertEdgeInfo(Edge* e) = 0;
00212 
00214   virtual void initializeFromParentEdge(Vertex* v) = 0;
00215 
00217   void clear();
00218 
00220   TreePoseGraph(){
00221     sortedEdges=0; 
00222     edgeCompareMode=EVComparator<Edge*>::CompareLevel;
00223   }
00224 
00226   virtual ~TreePoseGraph();
00227 
00229   EdgeSet* sortEdges();
00230 
00232   int maxPathLength();
00233 
00235   int totalPathLength();
00236 
00238   void compressIndices();
00239 
00241   int maxIndex();
00242 
00245   bool sanityCheck();
00246 
00248   Vertex* root;
00249 
00251   VertexMap vertices;
00252 
00254   EdgeMap edges;
00255 
00260   EdgeSet* sortedEdges;
00261 
00262 protected:
00263   void fillEdgeInfo(Edge* e);
00264   void fillEdgesInfo();
00265   EdgeCompareMode edgeCompareMode;
00266 };
00267 
00268 //include the template implementation part
00269 
00270 #include "posegraph.hxx"
00271 
00272 }; //namespace AISNavigation
00273 
00274 #endif


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jun 6 2019 21:59:21