posegraph.hh

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){
    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