posegraph.h

Go to the documentation of this file.

/**********************************************************************
 *
 * This source code is part of the Tree-based Network Optimizer (TORO)
 *
 * TORO Copyright (c) 2007 Giorgio Grisetti, Cyrill Stachniss, 
 *                         Slawomir Grzonka, and Wolfram Burgard
 *
 * TORO is licences under the Common Creative License,
 * Attribution-NonCommercial-ShareAlike 3.0
 *
 * You are free:
 *   - to Share - to copy, distribute and transmit the work
 *   - to Remix - to adapt the work
 *
 * Under the following conditions:
 *
 *   - Attribution. You must attribute the work in the manner specified
 *     by the author or licensor (but not in any way that suggests that
 *     they endorse you or your use of the work).
 *  
 *   - Noncommercial. You may not use this work for commercial purposes.
 *  
 *   - Share Alike. If you alter, transform, or build upon this work,
 *     you may distribute the resulting work only under the same or
 *     similar license to this one.
 *
 * Any of the above conditions can be waived if you get permission
 * from the copyright holder.  Nothing in this license impairs or
 * restricts the author's moral rights.
 *
 * TORO is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied 
 * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
 * PURPOSE.  
 **********************************************************************/
 
#ifndef _TREEPOSEGRAPH_HXX_
#define _TREEPOSEGRAPH_HXX_
 
#include <iostream>
#include <assert.h>
 
#include <set>
#include <list>
#include <map>
#include <deque>
#include <vector>
#include <limits>
#include <algorithm>
 
namespace AISNavigation{
 
template <class E> 
struct EVComparator{
  enum CompareMode {CompareLevel, CompareLength};
  CompareMode mode;
 
  EVComparator(){
    mode=CompareLevel;
  }
  inline bool operator() (const E& e1, const E& e2) const {
    int o1=0, o2=0;
    switch (mode){
    case CompareLevel:
      o1=e1->top->level;
      o2=e2->top->level;
      break;
    case CompareLength:
      o1=e1->length;
      o2=e2->length;
      break;
    }
    return o1<o2;
  }
};
 
template <class Ops>
struct TreePoseGraph{
  typedef typename Ops::BaseType BaseType;
  typedef typename Ops::PoseType Pose;
  typedef typename Ops::RotationType Rotation;
  typedef typename Ops::TranslationType Translation;
  typedef typename Ops::TransformationType Transformation;
  typedef typename Ops::CovarianceType Covariance;
  typedef typename Ops::InformationType Information;
  typedef typename Ops::ParametersType  Parameters;
  
  struct Vertex;
 
  struct Edge{
    Vertex* v1;   
    Vertex* v2;   
    Vertex* top;  
    int length;   
    Transformation transformation;    
    Information    informationMatrix; 
    bool mark;
    
    double learningRate;
  };
 
  typedef typename EVComparator<Edge*>::CompareMode EdgeCompareMode;
  typedef typename std::list< Edge* > EdgeList;
  typedef typename std::map< int, Vertex* > VertexMap;
  typedef typename std::set< Vertex* > VertexSet;
  typedef typename std::map< Edge*, Edge* > EdgeMap;
  typedef typename std::multiset< Edge*, EVComparator<Edge*> > EdgeSet;
 
  struct Vertex {
 
    // Graph-related elements
    int id;         
    EdgeList edges; 
    // Tree-related elements
    int level;         
    Vertex* parent;    
    Edge* parentEdge;  
    EdgeList children;   
    // Parameterization-related elements
    Transformation transformation; 
    Pose pose;              
    Parameters parameters;  
    bool mark;
  };
 
  Vertex* vertex(int id);
 
  const Vertex* vertex (int id) const;
 
  Edge* edge(int id1, int id2);
 
  const Edge* edge(int id1, int id2) const;
 
  Vertex* addVertex(int id, const Pose& pose);
  Vertex* removeVertex (int id);
 
  Edge* addEdge(Vertex* v1, Vertex* v2, const Transformation& t, const Information& i);
 
  Edge* removeEdge(Edge* eq);
 
  Edge* addIncrementalEdge(int id1, int id2,  const Transformation& t, const Information& i);
 
  EdgeSet* affectedEdges(Vertex* v);
 
  EdgeSet* affectedEdges(VertexSet& vl);
 
  template <class Action>
  void treeBreadthVisit(Action& act);
 
  template <class Action>
  void treeDepthVisit(Action& act, Vertex *v);
 
  bool buildMST(int id);
 
  bool buildSimpleTree();
 
  void revertEdge(Edge* e);
 
  virtual void revertEdgeInfo(Edge* e) = 0;
 
  virtual void initializeFromParentEdge(Vertex* v) = 0;
 
  void clear();
 
  TreePoseGraph(){
    sortedEdges=0; 
    edgeCompareMode=EVComparator<Edge*>::CompareLevel;
  }
 
  virtual ~TreePoseGraph();
 
  EdgeSet* sortEdges();
 
  int maxPathLength();
 
  int totalPathLength();
 
  void compressIndices();
 
  int maxIndex();
 
  bool sanityCheck();
 
  Vertex* root;
 
  VertexMap vertices;
 
  EdgeMap edges;
 
  EdgeSet* sortedEdges;
 
protected:
  void fillEdgeInfo(Edge* e);
  void fillEdgesInfo();
  EdgeCompareMode edgeCompareMode;
};
 
//include the template implementation part
 
#include "posegraph.hxx"
 
}; //namespace AISNavigation
 
#endif