Merged pcl_integration branch to trunk

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1014 f169173b-cf89-36c8-b27e-44dbe73f0c83

corelib/src/toro3d/dmatrix.hh

@@ -0,0 +1,94 @@
+#ifndef DMATRIX_HXX 
+#define DMATRIX_HXX
+
+#include <iostream>
+#include <exception>
+
+class DNotInvertibleMatrixException: public std::exception {};
+class DIncompatibleMatrixException: public std::exception {};
+class DNotSquareMatrixException: public std::exception {};
+
+
+template <class X> struct DVector{
+public:
+  DVector(int n=0);
+  ~DVector();
+  
+  DVector(const DVector&);
+  DVector& operator=(const DVector&);
+  
+  X&  operator[](int i) {
+    if ((*shares)>1) detach();
+    return elems[i];
+  }
+  
+  const X&  operator[](int i) const { return elems[i]; }
+  
+  X operator*(const DVector&) const;
+  DVector operator+(const DVector&) const;
+  DVector operator-(const DVector&) const;
+  DVector operator*(const X&) const;
+  
+  int dim() const { return size; }
+  
+  void detach();
+  
+  static DVector<X> I(int);
+  
+protected:
+  X * elems;
+  int size;
+  int * shares;
+};
+
+
+
+template <class X> class DMatrix {
+public:
+  DMatrix(int n=0,int m=0);
+  ~DMatrix();
+  
+  DMatrix(const DMatrix&);
+  DMatrix& operator=(const DMatrix&);
+  
+  X * operator[](int i) {
+    if ((*shares)>1) detach();
+    return mrows[i];
+  }
+  
+  const X * operator[](int i) const { return mrows[i]; }
+  
+  const X det() const;
+  DMatrix inv() const;
+  DMatrix transpose() const;
+  DMatrix operator*(const DMatrix&) const;
+  DMatrix operator+(const DMatrix&) const;
+  DMatrix operator-(const DMatrix&) const;
+  DMatrix operator*(const X&) const;
+  
+  int rows() const { return nrows; }
+  int columns() const { return ncols; }
+  
+  void detach();
+  
+  static DMatrix I(int);
+  
+protected:
+  X * elems;
+  int nrows,ncols;
+  X ** mrows;
+  
+  int * shares;
+};
+
+
+template <class X> DVector<X> operator * (const DMatrix<X> m, const DVector<X> v);
+
+template <class X> DVector<X> operator * (const DVector<X> v, const DMatrix<X> m);
+
+
+/*************** IMPLEMENTATION ***************/
+
+#include "dmatrix.hxx"
+
+#endif

corelib/src/toro3d/dmatrix.hxx

@@ -0,0 +1,289 @@
+
+template <class X> DVector<X>::DVector(int n) {
+  if (n<1) n=1;
+  size=n;
+  elems=new X[size];
+  for (int i=0;i<size; i++) 
+    elems[i]=X(0);
+  shares=new int;
+  (*shares)=1;
+}
+
+template <class X> DVector<X>::~DVector() {
+  if (--(*shares)) return;
+  delete [] elems;
+  delete shares;
+}
+
+template <class X> DVector<X>::DVector(const DVector<X>& m) {
+  shares=m.shares;
+  elems=m.elems;
+  size=m.size;
+  (*shares)++;
+}
+
+template <class X> DVector<X>& DVector<X>::operator=(const DVector<X>& m) {
+  if (shares==m.shares)
+    return *this;
+  if (!--(*shares)) {
+    delete [] elems;
+    delete shares;
+  }
+  shares=m.shares;
+  elems=m.elems;
+  size=m.size;
+  (*shares)++;
+  return *this;
+}
+
+template <class X> X DVector<X>::operator*(const DVector<X>& v) const{
+  if (size!=v.size) throw DIncompatibleMatrixException();
+  X p=X(0);
+  for (int i=0; i<size; i++)
+    p+=elems[i]*v.elems[i];
+  return p;
+}
+
+template <class X> void DVector<X>::detach() {
+  DVector<X> aux(size);
+  for (int i=0;i<size;i++) aux.elems[i]=elems[i];
+  operator=(aux);
+}
+
+template <class X> DVector<X> DVector<X>::operator+(const DVector<X>& v) const{
+  if (size!=v.size) throw DIncompatibleMatrixException();
+  DVector<X> r(size);
+  for (int i=0; i<size; i++){
+    r.elems[i]=elems[i]+v.elems[i];
+  }
+  return r;
+}
+
+template <class X> DVector<X> DVector<X>::operator-(const DVector<X>& v) const{
+  if (size!=v.size) throw DIncompatibleMatrixException();
+  DVector<X> r(size);
+  for (int i=0; i<size; i++){
+    r.elems[i]=elems[i]-v.elems[i];
+  }
+  return r;
+}
+
+template <class X> DVector<X> DVector<X>::operator*(const X& d) const{
+  DVector<X> r(size);
+  for (int i=0; i<size; i++){
+    r.elems[i]=elems[i]*d;
+  }
+  return r;
+}
+
+
+
+template <class X> DMatrix<X>::DMatrix(int n,int m) {
+  if (n<1) n=1;
+  if (m<1) m=1;
+  nrows=n;
+  ncols=m;
+  elems=new X[nrows*ncols];
+  mrows=new X* [nrows];
+  for (int i=0;i<nrows;i++) mrows[i]=elems+ncols*i;
+  for (int i=0;i<nrows*ncols;i++) elems[i]=X(0);
+  shares=new int;
+  (*shares)=1;
+}
+
+template <class X> DMatrix<X>::~DMatrix() {
+  if (--(*shares)) return;
+  delete [] elems;
+  delete [] mrows;
+  delete shares;
+}
+
+template <class X> DMatrix<X>::DMatrix(const DMatrix& m) {
+  shares=m.shares;
+  elems=m.elems;
+  nrows=m.nrows;
+  ncols=m.ncols;
+  mrows=m.mrows;
+  (*shares)++;
+}
+
+template <class X> DMatrix<X>& DMatrix<X>::operator=(const DMatrix& m) {
+  if (shares==m.shares)
+    return *this;
+  if (!--(*shares)) {
+    delete [] elems;
+    delete [] mrows;
+    delete shares;
+  }
+  shares=m.shares;
+  elems=m.elems;
+  nrows=m.nrows;
+  ncols=m.ncols;
+  mrows=m.mrows;
+  (*shares)++;
+  return *this;
+}
+
+template <class X> DMatrix<X> DMatrix<X>::inv() const {
+  if (nrows!=ncols) throw DNotInvertibleMatrixException();
+  DMatrix<X> aux1(*this),aux2(I(nrows));
+  aux1.detach();
+  for (int i=0;i<nrows;i++) {
+    int k=i;
+    for (;k<nrows&&aux1.mrows[k][i]==X(0);k++){};
+    if (k>=nrows) throw DNotInvertibleMatrixException();
+    X val=aux1.mrows[k][i];
+    for (int j=0;j<nrows;j++) {
+      aux1.mrows[k][j]=aux1.mrows[k][j]/val;
+      aux2.mrows[k][j]=aux2.mrows[k][j]/val;
+    }
+    if (k!=i) {
+      for (int j=0;j<nrows;j++) {
+	X tmp=aux1.mrows[k][j];
+	aux1.mrows[k][j]=aux1.mrows[i][j];
+	aux1.mrows[i][j]=tmp;
+	tmp=aux2.mrows[k][j];
+	aux2.mrows[k][j]=aux2.mrows[i][j];
+	aux2.mrows[i][j]=tmp;
+      }
+    }
+    for (int j=0;j<nrows;j++)
+      if (j!=i) {
+	X tmp=aux1.mrows[j][i];
+	for (int l=0;l<nrows;l++) {
+	  aux1.mrows[j][l]=aux1.mrows[j][l]-tmp*aux1.mrows[i][l];
+	  aux2.mrows[j][l]=aux2.mrows[j][l]-tmp*aux2.mrows[i][l];
+	}
+      }
+  }
+  return aux2;
+}
+
+template <class X> const X DMatrix<X>::det() const {
+  if (nrows!=ncols) throw DNotSquareMatrixException();
+  DMatrix<X> aux(*this);
+  X d=X(1);
+  aux.detach();
+  for (int i=0;i<nrows;i++) {
+    int k=i;
+    for (;k<nrows&&aux.mrows[k][i]==X(0);k++){};
+    if (k>=nrows) return X(0);
+    X val=aux.mrows[k][i];
+    for (int j=0;j<nrows;j++) {
+      aux.mrows[k][j]/=val;
+    }
+    d=d*val;
+    if (k!=i) {
+      for (int j=0;j<nrows;j++) {
+	X tmp=aux.mrows[k][j];
+	aux.mrows[k][j]=aux.mrows[i][j];
+	aux.mrows[i][j]=tmp;
+      }
+      d=-d;	
+    }
+    for (int j=i+1;j<nrows;j++){
+      X tmp=aux.mrows[j][i];
+      if (!(tmp==X(0)) ){
+	for (int l=0;l<nrows;l++) {
+	  aux.mrows[j][l]=aux.mrows[j][l]-tmp*aux.mrows[i][l];
+	}
+	//d=d*tmp;
+      }		
+		}
+  }
+  return d;
+}
+
+template <class X> DMatrix<X> DMatrix<X>::transpose() const {
+  DMatrix<X> aux(ncols, nrows);
+  for (int i=0; i<nrows; i++)
+    for (int j=0; j<ncols; j++)
+      aux[j][i]=mrows[i][j];
+  return aux;
+}
+
+template <class X> DMatrix<X> DMatrix<X>::operator*(const DMatrix<X>& m) const {
+  if (ncols!=m.nrows) throw DIncompatibleMatrixException();
+  DMatrix<X> aux(nrows,m.ncols);
+  for (int i=0;i<nrows;i++)
+    for (int j=0;j<m.ncols;j++){
+      X a=0;
+      for (int k=0;k<ncols;k++)
+	a+=mrows[i][k]*m.mrows[k][j];
+      aux.mrows[i][j]=a;
+    }
+  return aux;
+}
+
+template <class X> DMatrix<X> DMatrix<X>::operator+(const DMatrix<X>& m) const {
+  if (ncols!=m.ncols||nrows!=m.nrows) throw DIncompatibleMatrixException();
+  DMatrix<X> aux(nrows,ncols);
+  for (int i=0;i<nrows*ncols;i++) aux.elems[i]=elems[i]+m.elems[i];
+  return aux;
+}
+
+template <class X> DMatrix<X> DMatrix<X>::operator-(const DMatrix<X>& m) const {
+  if (ncols!=m.ncols||nrows!=m.nrows) throw DIncompatibleMatrixException();
+  DMatrix<X> aux(nrows,ncols);
+  for (int i=0;i<nrows*ncols;i++) aux.elems[i]=elems[i]-m.elems[i];
+  return aux;
+}
+
+template <class X> DMatrix<X> DMatrix<X>::operator*(const X& e) const {
+  DMatrix<X> aux(nrows,ncols);
+  for (int i=0;i<nrows*ncols;i++) aux.elems[i]=elems[i]*e;
+  return aux;
+}
+
+template <class X> void DMatrix<X>::detach() {
+  DMatrix<X> aux(nrows,ncols);
+  for (int i=0;i<nrows*ncols;i++) aux.elems[i]=elems[i];
+  operator=(aux);
+}
+
+template <class X> DMatrix<X> DMatrix<X>::I(int n) {
+  DMatrix<X> aux(n,n);
+  for (int i=0;i<n;i++) aux[i][i]=X(1);
+  return aux;
+}
+
+template <class X> std::ostream& operator<<(std::ostream& os, const DMatrix<X> &m) {
+  os << "{";
+  for (int i=0;i<m.rows();i++) {
+    if (i>0) os << ",";
+    os << "{";
+    for (int j=0;j<m.columns();j++) {
+      if (j>0) os << ",";
+      os << m[i][j];
+    }
+    os << "}";
+  }
+  return os << "}";
+}
+
+
+template <class X> DVector<X> operator * (const DMatrix<X> m, const DVector<X> v){
+    if (v.dim()!=m.columns()) throw DIncompatibleMatrixException();
+    DVector<X> r(m.rows());
+    for (int i=0; i<m.rows(); i++){
+      X a=X(0);
+      for (int j=0; j<m.columns(); j++){
+	a+=m[i][j]*v[j];
+      }
+      r[i]=a;
+    }
+    return r;
+}
+
+template <class X> DVector<X> operator * (const DVector<X> v, const DMatrix<X> m){
+    if (v.dim()!=m.rows()) throw DIncompatibleMatrixException();
+    DVector<X> r(m.columns());
+    for (int i=0; i<m.columns(); i++){
+      X a=X(0);
+      for (int j=0; j<m.rows(); j++){
+	a+=m[j][i]*v[j];
+      }
+      r[i]=a;
+    }
+    return r;
+}

corelib/src/toro3d/posegraph.hh

@@ -0,0 +1,274 @@
+/**********************************************************************
+ *
+ * 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.  
+ **********************************************************************/
+
+/** \file posegraph.hh 
+ * 
+ * \brief The template class for the node parameters. The graph of
+ * poses with support to tree construction functionalities.
+ **/
+
+#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{
+
+/** \brief A comparator class (struct) that compares the level
+    of two vertices if edges **/
+template <class E> 
+struct EVComparator{
+  /** Comparison operator for the level **/
+  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;
+  }
+};
+
+/** \brief The template class for representing an abstract tree
+    without specifing the dimensionality of the exact parameterization
+    of the nodes. This definition is passed in via the Operation (Ops)
+    template class **/
+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;
+
+  /** \brief Definition of an edge in the graph based on the template
+      input from Ops **/
+  struct Edge{
+    Vertex* v1;   /**< The constraint is defined between v1 and v2 **/
+    Vertex* v2;   /**< The constraint is defined between v1 and v2 **/
+    Vertex* top;  /**< The node with the smallest level in the path **/
+    int length;   /**< Length of the path on the tree (number of vertieces involved) **/
+    Transformation transformation;    /**< Transformation describing the constraint (relative mapping) **/
+    Information    informationMatrix; /**< Uncertainty encoded in the information matrix **/
+
+    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;
+
+  /** \brief Definition of a vertex in the graph based on the
+      template input from Ops **/
+  struct Vertex {
+
+    // Graph-related elements
+    int id;         /**< Id of the vertex in the graph **/
+    EdgeList edges; /**< The edges related to this vertex **/
+
+    // Tree-related elements
+    int level;         /**< level in the tree. It is the distance on the tree to the root **/
+    Vertex* parent;    /**< Parent vertex **/
+    Edge* parentEdge;  /**< Constraint between the parent and the current vertex in the tree **/
+    EdgeList children;   /**< All constraints involving the children of this vertex **/
+
+    // Parameterization-related elements
+    Transformation transformation; /**< redundant representation of the vertex, without gymbal locks **/
+    Pose pose;              /**< The pose of the vertex **/
+    Parameters parameters;  /**< The parameter representation **/
+
+    bool mark;
+  };
+
+  /** Returns the vertex with the given id **/
+  Vertex* vertex(int id);
+
+  /** Returns a const pointer to the vertex with the given id **/
+  const Vertex* vertex (int id) const;
+
+  /** Returns the edge between the two vertices **/
+  Edge* edge(int id1, int id2);
+
+  /** Returns a const pointer tothe edge between the two vertices **/
+  const Edge* edge(int id1, int id2) const;
+
+  /** Add a vertex to the graph **/
+  Vertex* addVertex(int id, const Pose& pose);
+  /** Remove a vertex from the graph **/
+  Vertex* removeVertex (int id);
+
+  /** Add an edge/constraint to the graph **/
+  Edge* addEdge(Vertex* v1, Vertex* v2, const Transformation& t, const Information& i);
+
+  /** Remove an edge/constraint from the graph **/
+  Edge* removeEdge(Edge* eq);
+
+   /** Adds en edge incrementally to the tree. 
+      It builds a simple tree and initializes the structures for the optimization.
+
+      This function is for online processing.
+      It requires that at least one vertex is already present in the graph.
+      The vertices are represented by their ids.
+     
+      Once the edge is introduced in the structure:
+      - the parent of the node with the higher ID is computed.
+      - the top node is assigned
+      - the edge is inserted in the
+
+      @returns A pointer to the added edge, if the insertion was succesfull. 0 otherwise.
+  **/
+  Edge* addIncrementalEdge(int id1, int id2,  const Transformation& t, const Information& i);
+
+  /** Returns a set of edges which are accected by the mofification of the vertex v.
+      The set is ordered according to the level of their top node.
+   **/
+  EdgeSet* affectedEdges(Vertex* v);
+
+  EdgeSet* affectedEdges(VertexSet& vl);
+
+  /** Function to perform a breadth-first visit of the nodes in the tree to carry out a specific action act**/
+  template <class Action>
+  void treeBreadthVisit(Action& act);
+
+  /** Function to perform a depth-first visit of the nodes in the tree to carry out a specific action act **/
+  template <class Action>
+  void treeDepthVisit(Action& act, Vertex *v);
+
+  /** Constructs the tree be computing a minimal spanning tree **/
+  bool buildMST(int id);
+
+  /** Constructs the incremental tree according to the input trajectory **/
+  bool buildSimpleTree();
+
+  /** Trun around an edge (used to ensure a certain oder on the vertexes) **/
+  void revertEdge(Edge* e);
+
+  /** Revert edge info. This function needs to be implemented by a subclass **/
+  virtual void revertEdgeInfo(Edge* e) = 0;
+
+  /** Revert edge info. This function needs to be implemented by a subclass **/
+  virtual void initializeFromParentEdge(Vertex* v) = 0;
+
+  /** Delete all edges and vertices **/
+  void clear();
+
+  /**constructor*/
+  TreePoseGraph(){
+    sortedEdges=0; 
+    edgeCompareMode=EVComparator<Edge*>::CompareLevel;
+  }
+
+  /** Destructor **/
+  virtual ~TreePoseGraph();
+
+  /** Sort constraints for correct processing order **/
+  EdgeSet* sortEdges();
+
+  /** Determines the length of the longest path in the tree **/
+  int maxPathLength();
+
+  /** Determines the path length of all pathes in the tree **/
+  int totalPathLength();
+
+  /** remove gaps in the indices of the vertex ids **/
+  void compressIndices();
+
+  /** compute the highest index of an vertex **/
+  int maxIndex();
+
+  /** performs a consistency check on the tree and the graph structure.
+      @returns false on failure.*/
+  bool sanityCheck();
+
+  /** The root node of the tree **/
+  Vertex* root;
+
+  /** All vertices **/
+  VertexMap vertices;
+
+  /** All edges **/
+  EdgeMap edges;
+
+  /** The constraints/edges sorted according to the level in the tree
+      in order to allow us the efficient update (pose computation) of
+      the nodes in the tree (see the RSS07 paper for further
+      details) **/
+  EdgeSet* sortedEdges;
+
+protected:
+  void fillEdgeInfo(Edge* e);
+  void fillEdgesInfo();
+  EdgeCompareMode edgeCompareMode;
+};
+
+//include the template implementation part
+
+#include "posegraph.hxx"
+
+}; //namespace AISNavigation
+
+#endif

corelib/src/toro3d/posegraph.hxx

@@ -0,0 +1,693 @@
+/**********************************************************************
+ *
+ * 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.  
+ **********************************************************************/
+
+/** \file posegraph.hxx
+ * 
+ * \brief The implementation of the template class for the node
+ * parameters. 
+ **/
+
+
+
+/*********************** IMPLEMENTATION PART ***********************/
+template <typename Ops>
+typename TreePoseGraph<Ops>::Vertex* TreePoseGraph<Ops>::vertex(int id){
+  typename VertexMap::iterator it=vertices.find(id);
+  if (it==vertices.end())
+    return 0;
+  return it->second;
+}
+
+template <typename Ops>
+const typename TreePoseGraph<Ops>::Vertex * TreePoseGraph<Ops>::vertex (int id) const{
+  typename VertexMap::const_iterator it=vertices.find(id);
+  if (it==edges.end())
+    return 0;
+  return it->second;
+}
+
+template <class Ops>
+typename TreePoseGraph<Ops>::Edge* TreePoseGraph<Ops>::edge(int id1, int id2){
+  Vertex* v1=vertex(id1);
+  if (!v1)
+    return false;
+  typename EdgeList::iterator it=v1->edges.begin();
+  while(it!=v1->edges.end()){
+    if ((*it)->v1->id==id1 && (*it)->v2->id==id2)
+      return *it;
+    it++;
+  }
+  return 0;
+}
+
+template <class Ops>
+const typename TreePoseGraph<Ops>::Edge * TreePoseGraph<Ops>::edge(int id1, int id2) const{
+  const Vertex* v1=vertex(id1);
+  if (!v1)
+    return false;
+  typename EdgeList::const_iterator it=v1->edges.begin();
+  while(it!=v1->edges.end()){
+    if ((*it)->v1->id==id1 && (*it)->v2->id==id2)
+      return *it;
+    it++;
+  }
+  return 0;
+}
+
+template <class Ops>
+void TreePoseGraph<Ops>::revertEdge(typename TreePoseGraph<Ops>::Edge * e){
+  revertEdgeInfo(e);
+  Vertex* ap=e->v2;
+  e->v2=e->v1;
+  e->v1=ap;
+}
+
+template <class Ops>
+typename TreePoseGraph<Ops>::Vertex* TreePoseGraph<Ops>::addVertex(int id, const typename TreePoseGraph<Ops>::Pose& pose){
+  Vertex* v=vertex(id);
+  if (v)
+    return 0;
+  v=new Vertex;
+  v->id=id;
+  v->pose=pose;
+  v->parent=0;
+  v->mark=false;
+  vertices.insert(std::make_pair(id,v));
+  return v;
+}
+
+template <class Ops>
+typename TreePoseGraph<Ops>::Vertex* TreePoseGraph<Ops>::removeVertex (int id){
+  typename VertexMap::iterator it=vertices.find(id);
+  if (it==vertices.end())
+    return 0;
+  Vertex* v=it->second;
+
+  if (v==0)
+    return false;
+
+  typename TreePoseGraph<Ops>::EdgeList el=v->edges;
+  for(typename EdgeList::iterator it=el.begin(); it!=el.end(); it++){
+    removeEdge(*it);
+  }
+  delete v;
+  vertices.erase(it);
+  return v;
+}
+
+template <class Ops>
+typename TreePoseGraph<Ops>::Edge* TreePoseGraph<Ops>::addEdge(typename TreePoseGraph<Ops>::Vertex* v1, typename TreePoseGraph<Ops>::Vertex* v2,
+								 const typename TreePoseGraph<Ops>::Transformation& t, const typename TreePoseGraph<Ops>::Information& i){
+  if (v1==v2)
+    return 0;
+  Edge* e=edge(v1->id, v2->id);
+  if (e)
+    return 0;
+
+  e=new Edge;
+  e->mark=false;
+  e->v1=v1;
+  e->v2=v2;
+  e->top=0;
+  e->transformation=t;
+  e->informationMatrix=i;
+  v1->edges.push_back(e);
+  v2->edges.push_back(e);
+  edges.insert(std::make_pair(e,e));
+  return e;
+}
+
+template <class Ops>
+typename TreePoseGraph<Ops>::Edge* TreePoseGraph<Ops>::addIncrementalEdge(int id1, int id2,
+							       const typename TreePoseGraph<Ops>::Transformation& t, const typename TreePoseGraph<Ops>::Information& i){
+  EVComparator<Edge*> comp;
+  comp.mode=edgeCompareMode;
+  if (! sortedEdges)
+    sortedEdges=new EdgeSet(comp);
+
+
+  typename VertexMap::iterator it1=vertices.find(id1);
+  typename VertexMap::iterator it2=vertices.find(id2);
+  Vertex* v1, *v2, *addedVertex=0;
+  if (it1==vertices.end() && it2==vertices.end()){
+    return 0;
+  }
+
+  if (it1==vertices.end()){
+    typename TreePoseGraph<Ops>::Pose p;
+    v1=addedVertex=addVertex(id1,p);
+  } else {
+    v1=it1->second;
+  }
+  if (it2==vertices.end()){
+    typename TreePoseGraph<Ops>::Pose p;
+    v2=addedVertex=addVertex(id2,p);
+  } else {
+    v2=it2->second;
+  }
+
+  if (v1->id==v2->id){
+    assert(0);
+  }
+
+  Edge* e=addEdge(v1,v2,t,i);
+  if (!e){
+    return 0;
+  }
+  if (v1->id>v2->id)
+    revertEdge(e);
+
+  if (addedVertex){
+    Vertex* otherVertex= (addedVertex==v1)? v2:v1;
+    addedVertex->parent=otherVertex;
+    addedVertex->parentEdge=e;
+    addedVertex->level=otherVertex->level+1;
+    otherVertex->children.push_back(e);
+  }
+
+  fillEdgeInfo(e);
+  sortedEdges->insert(e);
+
+  if (addedVertex){
+    initializeFromParentEdge(addedVertex);
+  }
+  return e;
+}
+
+
+template <class Ops>
+typename TreePoseGraph<Ops>::Edge* TreePoseGraph<Ops>::removeEdge(typename TreePoseGraph<Ops>::Edge* e){
+  {
+    typename EdgeMap::iterator it=edges.find(e);
+    if (it==edges.end()){
+      return 0;
+    }
+    edges.erase(it);
+  }
+
+
+  Vertex* v1=e->v1;
+  Vertex* v2=e->v2;
+
+  {
+    typename EdgeList::iterator it=v1->edges.begin();
+    while(it!=v1->edges.end()){
+      if (*it==e){
+	v1->edges.erase(it);
+	break;
+      }
+      it++;
+    }
+  }
+
+  {
+    typename EdgeList::iterator it=v2->edges.begin();
+    while(it!=v2->edges.end()){
+      if ((*it)==e){
+	delete *it;
+	v2->edges.erase(it);
+	break;
+      }
+      it++;
+    }
+  }
+
+  return e;
+}
+
+template <class Ops>
+template <class Action>
+void TreePoseGraph<Ops>::treeBreadthVisit(Action& act){
+  typedef std::deque<Vertex*> VertexDeque;
+  static VertexDeque q;
+  q.push_back(root);
+  while (!q.empty()){
+    Vertex* current=q.front();
+    act.perform(current);
+    q.pop_front();
+    typename EdgeList::iterator it=current->children.begin();
+    while(it!=current->children.end()){
+      typename TreePoseGraph::Edge* e=(*it);
+      q.push_back(e->v2);
+      if(e->v2==current){
+	std::cerr << "error in the link direction v=" << current->id << std::endl;
+	std::cerr << " v1=" << e->v1->id << " v2=" << e->v2->id <<  std::endl;
+	assert(0);
+      }
+      it++;
+    }
+  }
+  q.clear();
+}
+
+template <class Ops>
+template <class Action>
+void TreePoseGraph<Ops>::treeDepthVisit(Action& act, Vertex* v){
+  act.perform(v);
+  typename EdgeList::iterator it=v->children.begin();
+  while(it!=v->children.end()){
+    treeDepthVisit(act, (*it)->v2);
+    it++;
+  }
+}
+
+template <class Ops>
+bool TreePoseGraph<Ops>::buildMST(int id){
+  typedef std::deque<Vertex*> VertexDeque;
+  typename VertexMap::iterator it=vertices.begin();
+  while (it!=vertices.end()){
+    it->second->parent=0;
+    it->second->parentEdge=0;
+    it->second->children.clear();
+    it++;
+  }
+  Vertex* v=vertex(id);
+  if (!v)
+    return false;
+  root=v;
+  root->level=0;
+  VertexDeque q;
+  q.push_back(v);
+  //std::cerr << "v=" << v->id << std::endl;
+  while (!q.empty()){
+    v=q.front();
+    typename EdgeList::iterator it=v->edges.begin();
+    while (it!=v->edges.end()){
+      Edge* e=(*it);
+      bool invertedEdge=false;
+      Vertex* other=e->v2;
+      if (other==v){
+	other=e->v1;
+	invertedEdge=true;
+      }
+      if (other!=root && other->parent==0){
+	if (invertedEdge){
+	  revertEdge(e);
+	}
+	//std::cerr << "INSERT v=" << v->id<< " " << "e=(" << e->v1->id << "," << e->v2->id << ")" << std::endl;
+	other->parent=v;
+	other->parentEdge=e;
+	other->level=v->level+1;
+	q.push_back(other);
+	v->children.push_back(e);
+	//std::cerr << "v=" << other->id << std::endl;
+      }
+      it++;
+    }
+    q.pop_front();
+  }
+  fillEdgesInfo();
+  return true;
+}
+
+/** \brief A class (struct) to dermine the level of a vertex in the tree **/
+template <class TPG>
+struct LevelAssigner{
+  /** Dermines the level of the vertex v in the tree **/
+  void perform(typename TPG::Vertex* v){
+    if (v->parent)
+      v->level=v->parent->level+1;
+    else
+      v->level=0;
+  }
+};
+
+
+
+
+template <class Ops>
+bool TreePoseGraph<Ops>::buildSimpleTree(){
+  root=0;
+  //rectify all the constraints, so that the v1<v2
+  for (typename EdgeMap::iterator it=edges.begin(); it!=edges.end(); it++){
+    Edge* e=it->second;
+    if (e->v1->id > e->v2->id)
+      revertEdge(e);
+  }
+
+  //clear the tree data
+  for (typename VertexMap::iterator it=vertices.begin(); it!=vertices.end(); it++){
+    Vertex* v=it->second;
+    v->parent=0;
+    v->parentEdge=0;
+    v->children.clear();
+  }
+
+  //fill the structure
+  for (typename VertexMap::iterator it=vertices.begin(); it!=vertices.end(); it++){
+    Vertex* v=it->second;
+    if (v->edges.empty()){
+      assert(0);
+      continue;
+    }
+    Edge* bestEdge=v->edges.front();
+    int bestId=std::numeric_limits<int>::max();
+    bool found=false;
+    typename EdgeList::iterator li=v->edges.begin();
+    while(li!=v->edges.end()){
+      Edge* e =*li;
+      if (e->v2==v && e->v1->id<bestId){ //consider only the entering edges
+	bestId=e->v1->id;
+	bestEdge=e;
+	found=true;
+      }
+      li++;
+    }
+    if (found){
+      v->parentEdge=bestEdge;
+      v->parent=bestEdge->v1;
+      v->parent->children.push_back(bestEdge);
+    } else {
+      assert(! root);
+      root=v;
+    }
+  }
+//   std::cerr << "root="  << root << std::endl;
+  assert(root);
+
+  //assign the level
+  LevelAssigner< TreePoseGraph<Ops> > oa;
+  treeDepthVisit(oa, root);
+
+  fillEdgesInfo();
+  return true;
+}
+
+template <class Ops>
+TreePoseGraph<Ops>::~TreePoseGraph(){
+  clear();
+}
+
+template <class Ops>
+void TreePoseGraph<Ops>::clear(){
+  for (typename VertexMap::iterator it=vertices.begin(); it!=vertices.end(); it++){
+    delete it->second;
+    it->second=0;
+  }
+  for (typename EdgeMap::iterator it=edges.begin(); it!=edges.end(); it++){
+    delete it->second;
+    it->second=0;
+  }
+  vertices.clear();
+  edges.clear();
+  if ( sortedEdges )
+    delete sortedEdges;
+  sortedEdges=0;
+}
+
+template <class Ops>
+void TreePoseGraph<Ops>::fillEdgeInfo(Edge* e){
+  Vertex* v1=e->v1;
+  Vertex* v2=e->v2;
+  int length=0;
+  while (v1!=v2) {
+    if (v1->level > v2->level){
+      v1=v1->parent;
+      length++;
+    } else if (v2->level > v1->level){
+      v2=v2->parent;
+      length++;
+    } else if (v1->level==v2->level){
+      v1=v1->parent;
+      v2=v2->parent;
+      length+=2;
+    }
+  }
+  e->length=length;
+  e->top=v1;
+}
+
+template <class Ops>
+void TreePoseGraph<Ops>::fillEdgesInfo(){
+  typename TreePoseGraph<Ops>::EdgeMap em=edges;
+  for(typename EdgeMap::iterator it=em.begin(); it!=em.end(); it++){
+    fillEdgeInfo(it->second);
+  }
+}
+
+
+template <class Ops>
+typename TreePoseGraph<Ops>::EdgeSet* TreePoseGraph<Ops>::sortEdges(){
+  EVComparator<Edge*> comp;
+  comp.mode=edgeCompareMode;
+  EdgeSet * el=new EdgeSet(comp);
+  typename EdgeMap::iterator it=edges.begin();
+  while(it!=edges.end()){
+    el->insert(it->second);
+    it++;
+  }
+  return el;
+}
+
+template <class Ops>
+typename TreePoseGraph<Ops>::EdgeSet* TreePoseGraph<Ops>::affectedEdges(Vertex* v){
+  EVComparator<Edge*> comp;
+  comp.mode=edgeCompareMode;
+  EdgeSet * es=new EdgeSet(comp);
+  std::deque<Vertex*> frontier;
+  std::list<Vertex*> markedVertices;
+
+  //frontier.push_back(v);
+  //v->mark=true;
+
+  for (typename EdgeList::iterator it=v->children.begin(); it!=v->children.end(); it++){
+    Edge* e=*it;
+    Vertex* other=(e->v1==v)?e->v2:e->v1;
+    frontier.push_back(other);
+    other->mark=true;
+    markedVertices.push_back(other);
+    e->mark=true;
+    es->insert(e);
+   }
+
+  while (! frontier.empty()){
+    Vertex* c=frontier.front();
+    frontier.pop_front();
+    markedVertices.push_back(c);
+    EdgeList& el=c->edges;
+    for (typename EdgeList::iterator it=el.begin(); it!=el.end(); it++){
+      Edge* e=*it;
+      if (e->mark)
+	continue;
+      Vertex* other= (e->v1==c)?e->v2:e->v1;
+      if (other==c->parent)
+	continue;
+      if (other!=e->top && ! e->top->mark){
+	e->top->mark=true;
+	frontier.push_back(e->top);
+      }
+      e->mark=true;
+      es->insert(e);
+      if (!other->mark){
+	other->mark=true;
+	frontier.push_back(other);
+      }
+    }
+  }
+  for (typename std::list<Vertex*>::iterator it=markedVertices.begin(); it!=markedVertices.end(); it++){
+    (*it)->mark=false;
+  }
+
+  for (typename EdgeSet::iterator it=es->begin(); it!=es->end(); it++){
+    (*it)->mark=false;
+  }
+  return es;
+}
+
+template <class Ops>
+typename TreePoseGraph<Ops>::EdgeSet* TreePoseGraph<Ops>::affectedEdges(typename TreePoseGraph<Ops>::VertexSet& vl){
+  EVComparator<Edge*> comp;
+  comp.mode=edgeCompareMode;
+  EdgeSet * es=new EdgeSet(comp);
+  std::deque<Vertex*> frontier;
+  std::list<Vertex*> markedVertices;
+
+//   for (typename VertexSet::iterator it=vl.begin(); it!=vl.end(); it++){
+//     frontier.push_back(*it);
+//     (*it)->mark=true;
+//   }
+
+  for (typename VertexSet::iterator it=vl.begin(); it!=vl.end(); it++){
+    Vertex* v=*it;
+    for (typename EdgeList::iterator it=v->children.begin(); it!=v->children.end(); it++){
+      Edge* e=*it;
+      Vertex* other=(e->v1==v)?e->v2:e->v1;
+      frontier.push_back(other);
+      other->mark=true;
+      markedVertices.push_back(other);
+      e->mark=true;
+      es->insert(e);
+    }
+  }
+
+
+
+
+  while (! frontier.empty()){
+    Vertex* c=frontier.front();
+    frontier.pop_front();
+    markedVertices.push_back(c);
+    EdgeList& el=c->edges;
+    for (typename EdgeList::iterator it=el.begin(); it!=el.end(); it++){
+      Edge* e=*it;
+      if (e->mark)
+	continue;
+      Vertex* other= (e->v1==c)?e->v2:e->v1;
+      if (other==c->parent)
+	continue;
+      if (other!=e->top && ! e->top->mark){
+	e->top->mark=true;
+	frontier.push_back(e->top);
+      }
+      e->mark=true;
+      es->insert(e);
+      if (!other->mark){
+	other->mark=true;
+	frontier.push_back(other);
+      }
+    }
+  }
+  for (typename std::list<Vertex*>::iterator it=markedVertices.begin(); it!=markedVertices.end(); it++){
+    (*it)->mark=false;
+  }
+
+  for (typename EdgeSet::iterator it=es->begin(); it!=es->end(); it++){
+    (*it)->mark=false;
+  }
+  return es;
+}
+
+
+
+template <class Ops>
+int TreePoseGraph<Ops>::maxPathLength(){
+  int max=0;
+  typename EdgeMap::const_iterator it=edges.begin();
+  while(it!=edges.end()){
+    int l=it->second->length;
+    max=l>max?l:max;
+    it++;
+  }
+  return max;
+}
+
+template <class Ops>
+int TreePoseGraph<Ops>::totalPathLength(){
+  int t=0;
+  typename EdgeMap::const_iterator it=edges.begin();
+  while(it!=edges.end()){
+    t+=it->second->length;
+    it++;
+  }
+  return t;
+}
+
+
+template <class Ops>
+void TreePoseGraph<Ops>::compressIndices(){
+  VertexMap vmap;
+  int i=0;
+  for (typename VertexMap::iterator it=vertices.begin(); it!=vertices.end(); it++){
+    Vertex* v=it->second;
+    v->id=i;
+    vmap.insert(std::make_pair(i,v));
+    i++;
+  }
+  vertices=vmap;
+}
+
+template <class Ops>
+int TreePoseGraph<Ops>::maxIndex(){
+  typename VertexMap::reverse_iterator it=vertices.rbegin();
+  if (it!=vertices.rend())
+    return it->second->id;
+  return -1;
+}
+
+
+
+template <class TPG>
+struct LoopChecker{
+  bool noloops;
+  void perform(typename TPG::Vertex* v){
+    if (!noloops)
+      return;
+    if (!v->mark)
+      v->mark=true;
+    else
+      noloops=false;
+  }
+};
+
+
+template <class Ops>
+bool TreePoseGraph<Ops>::sanityCheck(){
+  //check that each node has exactly one parent
+  for (typename VertexMap::iterator it=vertices.begin(); it!=vertices.end(); it++){
+    Vertex* v=it->second;
+    v->mark=false;
+    Vertex* vp=v->parent;
+
+    if (! vp){
+      if (v!=root){
+	std::cerr << "root not found in the graph" << std::endl;
+	return false;
+      }
+    }
+
+    const EdgeList& children=it->second->children;
+    for (typename EdgeList::const_iterator lt=children.begin(); lt!=children.end(); lt++){
+      if ((*lt)->v1!=v){
+	std::cerr << "wrong direction of the edges" << std::cerr;
+	return false;
+      }
+    }
+  }
+  //check that there are no loops in the tree
+
+  LoopChecker< TreePoseGraph<Ops> > lc;
+  lc.noloops=true;
+  treeBreadthVisit(lc);
+  if (!lc.noloops){
+    std::cerr << "the tree contains loops" << std::endl;
+    return false;
+  }
+  for (typename VertexMap::iterator it=vertices.begin(); it!=vertices.end(); it++){
+    Vertex* v=it->second;
+    v->mark=false;
+  }
+  return true;
+}

corelib/src/toro3d/posegraph3.cpp

@@ -0,0 +1,405 @@
+/**********************************************************************
+ *
+ * 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.  
+ **********************************************************************/
+
+/** \file posegraph3.cpp
+ * 
+ * \brief Defines the graph of 3D poses, with specific functionalities
+ * such as loading, saving, merging constraints, and etc.
+ **/
+
+#include "posegraph3.hh"
+#include <fstream>
+#include <sstream>
+#include <string>
+
+using namespace std;
+
+namespace AISNavigation {
+
+#define LINESIZE 81920
+
+
+#define DEBUG(i) \
+        if (verboseLevel>i) cerr
+
+
+bool TreePoseGraph3::load(const char* filename, bool overrideCovariances, bool twoDimensions){
+  clear();
+  ifstream is(filename);
+  if (!is)
+    return false;
+
+  while(is){
+    char buf[LINESIZE];
+    is.getline(buf,LINESIZE);
+    istringstream ls(buf);
+    string tag;
+    ls >> tag;
+    
+    if (twoDimensions){
+      if (tag=="VERTEX"){
+	int id;
+	Pose p(0.,0.,0.,0.,0.,0.);
+	ls >> id >> p.x() >> p.y() >> p.yaw(); 
+	TreePoseGraph3::Vertex* v=addVertex(id,p);
+	if (v){
+	  v->transformation=Transformation(p);
+	}
+      }
+    } else {
+      if (tag=="VERTEX3"){
+	int id;
+	Pose p;
+	ls >> id >> p.x() >> p.y() >> p.z() >> p.roll() >> p.pitch() >> p.yaw(); 
+	TreePoseGraph3::Vertex* v=addVertex(id,p);
+	if (v){
+	  v->transformation=Transformation(p);
+	}
+      }
+    }      
+  }
+  is.clear(); /* clears the end-of-file and error flags */
+  is.seekg(0, ios::beg);
+
+  bool edgesOk=true;
+  while(is){
+    char buf[LINESIZE];
+    is.getline(buf,LINESIZE);
+    istringstream ls(buf);
+    string tag;
+    ls >> tag;
+    
+    if (twoDimensions){
+      if (tag=="EDGE"){
+	int id1, id2;
+	Pose p(0.,0.,0.,0.,0.,0.);
+	InformationMatrix m;
+	ls >> id1 >> id2 >> p.x() >> p.y() >> p.yaw();
+	m=DMatrix<double>::I(6);
+	if (! overrideCovariances){
+	  ls >> m[0][0] >> m[0][1] >> m[1][1] >> m[2][2] >> m[0][2] >> m[1][2];
+	  m[2][0]=m[0][2]; m[2][1]=m[1][2]; m[1][0]=m[0][1];
+	}
+      
+	TreePoseGraph3::Vertex* v1=vertex(id1);
+	TreePoseGraph3::Vertex* v2=vertex(id2);
+	Transformation t(p);
+	if (!addEdge(v1, v2,t ,m)){
+	  cerr << "Fatal, attempting to insert an edge between non existing nodes, skipping";
+	  cerr << "edge=" << id1 <<" -> " << id2 << endl;
+	  edgesOk=false;
+	} 
+      }
+    } else {
+      if (tag=="EDGE3"){
+	int id1, id2;
+	Pose p;
+	InformationMatrix m;
+	ls >> id1 >> id2 >> p.x() >> p.y() >> p.z() >> p.roll() >> p.pitch() >> p.yaw();
+	m=DMatrix<double>::I(6);
+	if (! overrideCovariances){
+	  for (int i=0; i<6; i++)
+	    for (int j=i; j<6; j++)
+	      ls >> m[i][j];
+	}
+	TreePoseGraph3::Vertex* v1=vertex(id1);
+	TreePoseGraph3::Vertex* v2=vertex(id2);
+	Transformation t(p);
+	if (!addEdge(v1, v2,t ,m)){
+	  cerr << "Fatal, attempting to insert an edge between non existing nodes, skipping";
+	  cerr << "edge=" << id1 <<" -> " << id2 << endl;
+	  edgesOk=false;
+	}
+      }
+    }
+  }
+  return true;
+  //return edgesOk;
+}
+
+bool TreePoseGraph3::loadEquivalences(const char* filename){
+  ifstream is(filename);
+  if (!is)
+    return false;
+  EdgeList suppressed;
+  uint equivCount=0;
+  while (is){
+    char buf[LINESIZE];
+    is.getline(buf, LINESIZE);
+    istringstream ls(buf);
+    string tag;
+    ls >> tag;
+    if (tag=="EQUIV"){
+      int id1, id2;
+      ls >> id1 >> id2;
+      Edge* e=edge(id1,id2);
+      if (!e)
+	e=edge(id2,id1);
+      if (e){
+	suppressed.push_back(e);
+	equivCount++;
+      }
+    }
+  }
+  for (EdgeList::iterator it=suppressed.begin(); it!=suppressed.end(); it++){
+    Edge* e=*it;
+    if (e->v1->id > e->v2->id)
+      revertEdge(e);
+    collapseEdge(e);
+  }
+  return true;
+}
+
+bool TreePoseGraph3::saveGnuplot(const char* filename){
+  ofstream os(filename);
+  if (!os)
+    return false;
+  for (TreePoseGraph3::VertexMap::iterator it=vertices.begin(); it!=vertices.end(); it++){
+    TreePoseGraph3::Vertex* v=it->second;
+    v->pose=v->transformation.toPoseType();
+  }
+  for (TreePoseGraph3::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
+    const TreePoseGraph3::Edge * e=it->second;
+    const Vertex* v1=e->v1;
+    const Vertex* v2=e->v2;
+    
+    os << v1->pose.x() << " " << v1->pose.y() << " " << v1->pose.z() << " "
+       << v1->pose.roll() << " " << v1->pose.pitch() << " " << v1->pose.yaw() <<endl;
+    os << v2->pose.x() << " " << v2->pose.y() << " " << v2->pose.z() << " "
+       << v2->pose.roll() << " " << v2->pose.pitch() << " " << v2->pose.yaw() <<endl;
+    os << endl << endl;
+  }
+  return true;
+
+}
+
+bool TreePoseGraph3::save(const char* filename){
+  ofstream os(filename);
+  if (!os)
+    return false;
+  
+  for (TreePoseGraph3::VertexMap::iterator it=vertices.begin(); it!=vertices.end(); it++){
+    TreePoseGraph3::Vertex* v=it->second;
+    v->pose=v->transformation.toPoseType();
+
+    os << "VERTEX3 " 
+       << v->id << " " 
+       << v->pose.x() << " " 
+       << v->pose.y() << " " 
+       << v->pose.z() << " "
+       << v->pose.roll() << " " 
+       << v->pose.pitch() << " " 
+       << v->pose.yaw() << endl; 
+  }
+  for (TreePoseGraph3::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
+    const TreePoseGraph3::Edge * e=it->second;
+    os << "EDGE3 " << e->v1->id << " " << e->v2->id << " ";
+    Pose p=e->transformation.toPoseType();
+    os << p.x() << " " << p.y() << " " << p.z() << " " << p.roll() << " " << p.pitch() << " " << p.yaw() << " ";
+    for (int i=0; i<6; i++)
+      for (int j=i; j<6; j++)
+	os << e->informationMatrix[i][j] << " ";
+    os << endl;
+  }
+  return true;
+}
+
+/** \brief A class (struct) used to print vertex information to a
+    stream. Needed for debugging. **/
+struct IdPrinter{
+  IdPrinter(std::ostream& _os):os(_os){}
+  std::ostream& os;
+  void perform(TreePoseGraph3::Vertex* v){
+    std::cout << "(" << v->id << "," << v->level << ")" << endl;
+  }
+};
+
+void TreePoseGraph3::printDepth( std::ostream& os ){
+  IdPrinter ip(os);
+  treeDepthVisit(ip, root);
+}
+
+void TreePoseGraph3::printWidth( std::ostream& os ){
+  IdPrinter ip(os);
+  treeBreadthVisit(ip);
+}
+
+/** \brief A class (struct) for realizing the pose update of the
+    individual nodes. Assumes the correct order of constraint updates
+    (according to the tree level, see RSS07 paper)**/
+
+struct PosePropagator{
+  void perform(TreePoseGraph3::Vertex* v){
+    if (!v->parent)
+      return;
+    TreePoseGraph3::Transformation tParent(v->parent->transformation);
+    TreePoseGraph3::Transformation tNode=tParent*v->parentEdge->transformation;
+
+    assert(v->parentEdge->v1==v->parent);
+    assert(v->parentEdge->v2==v);
+    v->transformation=tNode;
+  }
+};
+
+void TreePoseGraph3::initializeOnTree(){
+  PosePropagator pp;
+  treeDepthVisit(pp, root);
+}
+
+
+void TreePoseGraph3::printEdgesStat(std::ostream& os){
+  for (TreePoseGraph3::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
+    const TreePoseGraph3::Edge * e=it->second;
+    os << "EDGE " << e->v1->id << " " << e->v2->id << " ";
+    Pose p=e->transformation.toPoseType();
+    os << p.x() << " " << p.y() << " " << p.z() << " " << p.roll() << " " << p.pitch() << " " << p.yaw() << endl;
+    os << "   top=" << e->top->id << " length=" << e->length << endl;
+  }
+}
+
+void TreePoseGraph3::revertEdgeInfo(Edge* e){
+  // here we assume uniform covariances, and we neglect the transofrmation
+  // induced by the Jacobian when reverting the link
+  e->transformation=e->transformation.inv();
+};
+
+void TreePoseGraph3::initializeFromParentEdge(Vertex* v){
+  Transformation tp=Transformation(v->parent->pose)*v->parentEdge->transformation;
+  v->transformation=tp;
+  v->pose=tp.toPoseType();
+  v->parameters=v->parentEdge->transformation;
+}
+
+
+void TreePoseGraph3::collapseEdge(Edge* e){
+  Vertex* v1=e->v1;
+  Vertex* v2=e->v2;
+  
+  // all the edges of v2 become outgoing
+  for (EdgeList::iterator it=v2->edges.begin(); it!=v2->edges.end(); it++){
+    if ( (*it)->v1!=v2 )
+      revertEdge(*it);
+  }
+
+ // all the edges of v1 become outgoing
+  for (EdgeList::iterator it=v1->edges.begin(); it!=v1->edges.end(); it++){
+    if ( (*it)->v1!=v1 )
+      revertEdge(*it);
+  }
+
+  assert(e->v1==v1);
+
+  InformationMatrix I12=e->informationMatrix;
+  CovarianceMatrix  C12=I12.inv();
+  Transformation    T12=e->transformation;
+  Pose              p12=T12.toPoseType();
+
+  Transformation iT12=T12.inv();
+
+  //compute the marginal information of the nodes in the path v1-v2-v*
+  for (EdgeList::iterator it2=v2->edges.begin(); it2!=v2->edges.end(); it2++){
+    Edge* e2=*it2;
+    if (e2->v1==v2){ //edge leaving v2
+      Transformation T2x=e2->transformation;
+      Pose           p2x=T2x.toPoseType();
+      InformationMatrix I2x=e2->informationMatrix;
+      CovarianceMatrix  C2x=I2x.inv();
+      
+      //compute the estimate of the vertex based on the path v1-v2-vx
+      
+      Transformation tr=iT12*T2x;
+      
+      CovarianceMatrix CM=C2x;
+      
+      
+      Transformation T1x_pred=T12*e2->transformation;
+      Covariance C1x_pred=C12+C2x;
+      InformationMatrix I1x_pred=C1x_pred.inv();
+
+      e2->transformation=T1x_pred;
+      e2->informationMatrix=I1x_pred;
+    }
+  }
+
+  //all the edges leaving v1 and leaving v2 and leading to the same point are merged
+  std::list<Transformation> tList;
+  std::list<InformationMatrix> iList;
+  std::list<Vertex*> vList;
+
+  //others are transformed and added to v1
+  for (EdgeList::iterator it2=v2->edges.begin(); it2!=v2->edges.end(); it2++){
+    Edge* e1x=0;
+    Edge* e2x=0;
+    if ( ((*it2)->v1!=v1)){
+      e2x=*it2;
+      for (EdgeList::iterator it1=v1->edges.begin(); it1!=v1->edges.end(); it1++){
+	if ((*it1)->v2==(*it2)->v2)
+	  e1x=*it1;
+      }
+
+    }
+    // FIXME
+    // edges leading to the same node are ignored
+    // should be merged
+    if (e1x && e2x){
+      // here goes something for mergin the constraints, according to the information matrices.
+      // in 3D it is a nightmare, so i postpone this, and i simply ignore the redundant constraints.
+      // the resultng system is overconfident
+    }
+    if (!e1x && e2x){
+      tList.push_back(e2x->transformation);
+      iList.push_back(e2x->informationMatrix);
+      vList.push_back(e2x->v2);
+    }
+  }
+  removeVertex(v2->id);
+
+  std::list<Transformation>::iterator t=tList.begin();
+  std::list<InformationMatrix>::iterator i=iList.begin();
+  std::list<Vertex*>::iterator v=vList.begin();
+  while (i!=iList.end()){
+    addEdge(v1,*v,*t,*i);
+    i++;
+    t++;
+    v++;
+  }
+}
+
+void TreePoseGraph3::recomputeAllTransformations(){
+  TransformationPropagator tp;
+  treeDepthVisit(tp,root);
+}
+
+}; //namespace AISNavigation

corelib/src/toro3d/posegraph3.hh

@@ -0,0 +1,145 @@
+/**********************************************************************
+ *
+ * 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.  
+ **********************************************************************/
+
+/** \file posegraph3.hh
+ *
+ * \brief Defines the graph of 3D poses, with specific functionalities
+ * such as loading, saving, merging constraints, and etc.
+ **/
+
+#ifndef _POSEGRAPH3_HH_
+#define _POSEGRAPH3_HH_
+
+#include "posegraph.hh"
+#include "transformation3.hh"
+#include <iostream>
+#include <vector>
+
+typedef unsigned int uint;
+#ifndef M_PI
+	#define M_PI 3.14159265359
+#endif
+
+namespace AISNavigation {
+
+
+/** \brief The class (struct) that contains 2D graph related functions
+    such as loading, saving, merging, etc. **/
+struct TreePoseGraph3: public TreePoseGraph<Operations3D<double> >{
+  typedef Operations3D<double> Ops;
+  
+  typedef Ops::PoseType           Pose;
+  typedef Ops::RotationType       Rotation;
+  typedef Ops::TranslationType    Translation;
+  typedef Ops::TransformationType Transformation;
+  typedef Ops::CovarianceType     CovarianceMatrix;
+  typedef Ops::InformationType    InformationMatrix;
+
+
+  /** Load a graph from a file ignoring the equivalence constraints
+      @param filename the graph file
+      @param overrideCovariances ignore the covariances from the file, and use identities instead
+   **/
+  bool load( const char* filename, bool overrideCovariances=false, bool twoDimensions=false); 
+
+  /** Load only the equivalence constraints from a  graph file (call load before)  **/
+  bool loadEquivalences( const char* filename); 
+
+  /** Saves the graph in the graph-format**/
+  bool save( const char* filename);
+
+  /** Saved the graph for visualizing it using gnuplot **/
+  bool saveGnuplot( const char* filename);
+
+  /** Debug function  **/
+  void printDepth( std::ostream& os );
+
+  /** Debug function  **/
+  void printWidth( std::ostream& os );
+
+  /** Debug function  **/
+  void printEdgesStat( std::ostream& os);
+
+  /** Initializes the parameters based on the topology of the tree and the actual transformation*/
+  void initializeOnTree();
+  
+  /** Recomputes all the transformations based on the parameters and the tree*/
+  void recomputeAllTransformations();
+
+  virtual void initializeFromParentEdge(Vertex* v);
+
+  /** Turn around the edge (<i,j>  => <j,i>)  **/
+  virtual void revertEdgeInfo(Edge* e);
+
+  /** Function to compress a graph. Needed if, for example, equivalence
+      constraints are used to build a graoh structure with indices
+      without gaps.  **/
+  virtual void collapseEdge(Edge* e);
+
+  /** Specifies the verbose level for debugging **/  
+  int verboseLevel;
+
+protected:
+
+  /** \brief A class (struct) to compute the parameterization of the vertex v **/
+  struct ParameterPropagator{
+    inline void perform(TreePoseGraph3::Vertex* v){
+      if (!v->parent){
+	v->parameters=TreePoseGraph3::Transformation(0.,0.,0.,0.,0.,0.);
+	return;
+      }
+      v->parameters=v->parent->transformation.inv()*v->transformation;
+    }
+  };
+
+  /** \brief A class (struct) to compute the parameterization of the vertex v **/
+  struct TransformationPropagator{
+    inline void perform(TreePoseGraph3::Vertex* v){
+      if (!v->parent){
+	return;
+      }
+      v->transformation=v->parent->transformation*v->parameters;
+    }
+  };
+
+};
+
+
+}; //namespace AISNavigation
+#endif
+
+
+

corelib/src/toro3d/transformation3.hh

@@ -0,0 +1,275 @@
+/**********************************************************************
+ *
+ * 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 _TRANSFORMATION3_HXX_
+#define _TRANSFORMATION3_HXX_
+
+#include <assert.h>
+#include <cmath>
+#include "dmatrix.hh"
+
+namespace AISNavigation {
+
+template <class T>
+struct Vector3 {
+  T elems[3] ;
+
+  Vector3(T x, T y, T z) {elems[0]=x; elems[1]=y; elems[2]=z;}
+  Vector3()              {elems[0]=0.; elems[1]=0.; elems[2]=0.;}
+  Vector3(const DVector<T>& t){}
+
+  // translational view
+  inline const T& x()     const  {return elems[0];}
+  inline const T& y()     const  {return elems[1];}
+  inline const T& z()     const  {return elems[2];}
+  inline T& x()            {return elems[0];}
+  inline T& y()            {return elems[1];}
+  inline T& z()            {return elems[2];}
+
+  // rotational view
+  inline const T& roll()   const  {return elems[0];}
+  inline const T& pitch() const  {return elems[1];}
+  inline const T& yaw()   const  {return elems[2];}
+  inline T& roll()          {return elems[0];}
+  inline T& pitch()        {return elems[1];}
+  inline T& yaw()          {return elems[2];}
+
+};
+
+template <class T>
+struct Pose3 : public DVector<T>{
+  Pose3();
+  Pose3(const Vector3<T>& rot, const Vector3<T>& trans);
+  Pose3(const T& x, const T& y, const T& z, const T& roll, const T& pitch, const T& yaw);
+  Pose3(const DVector<T>& v): DVector<T>(v) {assert(v.dim()==6);} 
+    
+  inline operator const DVector<T>&  () {return (const DVector<T>)*this;}
+  inline operator DVector<T>&        () {return *this;}
+
+  inline const T& roll()  const  {return DVector<T>::elems[0];}
+  inline const T& pitch() const  {return DVector<T>::elems[1];}
+  inline const T& yaw()   const  {return DVector<T>::elems[2];}
+  inline const T& x()     const  {return DVector<T>::elems[3];}
+  inline const T& y()     const  {return DVector<T>::elems[4];}
+  inline const T& z()     const  {return DVector<T>::elems[5];}
+
+  inline T& roll()               {return DVector<T>::elems[0];}
+  inline T& pitch()              {return DVector<T>::elems[1];}
+  inline T& yaw()                {return DVector<T>::elems[2];}
+  inline T& x()                  {return DVector<T>::elems[3];}
+  inline T& y()                  {return DVector<T>::elems[4];}
+  inline T& z()                  {return DVector<T>::elems[5];}
+
+};
+
+
+/*!
+ * A Quaternion can be used to either represent a rotational axis
+ * and a Rotation, or, the point which will be rotated
+ */
+
+template <class T>
+struct Quaternion{
+
+  /*!
+   * Default Constructor: w=x=y=z=0;
+   */
+  Quaternion();
+  
+  /*!
+   * The Quaternion representation of the point "pose"
+   */
+  Quaternion(const Vector3<T>& pose);
+  
+  /*!
+   * create a Quaternion by scalar w and the imaginery parts x,y, and z.
+   */
+  Quaternion(const T _w, const T _x, const T _y, const T _z);
+  
+  /*!
+   * create a rotational Quaternion, roll along x-axis, pitch along y-axis and yaw along z-axis
+   */
+  Quaternion(const T _roll_x_phi, const T _pitch_y_theta, const T _yaw_z_psi);
+  
+  /*!
+   * @return the conjugated version of this quaternion
+   */
+  inline Quaternion<T> conjugated() const;
+  
+  /*!
+   * @return this quaternion, but normalized
+   */
+  inline Quaternion<T> normalized() const;
+  
+  /*!
+   * @return the inverse of this Quaternion
+   */
+  inline Quaternion<T> inverse() const;
+
+  /*construct a quaternion on the axis/angle representation*/
+  inline Quaternion(const Vector3<T>& axis, const T& angle);
+
+  /*!
+   * if this Quaternion represents a point, use this function
+   * to rotate the point along <axis> with angle <alpha>
+   * @param axis the rotational axis
+   * @param alpha rotational angle
+   */
+  inline Quaternion<T> rotateThisAlong (const Vector3<T>& axis, const T alpha) const;
+  
+	
+  /*!
+   * if this Quaternion represents a rotational axis + rotation,
+   * use this function to rotate another point represented as a Quaternion p
+   * @param p the point to be rotated by <this>. Point is represented as a Quaternion
+   * @return rotated Point (represented as a Quaternion)
+   */
+  inline Quaternion<T> rotatePoint(const Quaternion& p) const;
+  
+  /*!
+   * if this Quaternion represents a rotational axis + rotation,
+   * use this function to rotate another point
+   * @param p the point to be rotated by <this>. 
+   * @return rotated Point 
+   */
+  inline Vector3<T> rotatePoint(const Vector3<T>& p) const;
+  
+  /*!
+   * if this Quaternion represents a rotational axis, add a rotation of angle <alpha>
+   * along <this> axis to the Quaternion
+   * @param alpha rotational value
+   * @return this Quaternion with included information about the rotation along <this> axis
+   */
+  inline Quaternion withRotation (const T alpha) const;
+	
+  /*!
+   * Given rotational axis x,y,z, get the rotation along these axis encoded in this Quaternion
+   * @return rotation along x,y,z axis encoded in <this> Quaternion
+	*/
+  inline Vector3<T> toAngles() const;
+  
+  inline Vector3<T> axis() const;
+  inline T angle() const;
+
+	
+  /*!
+   * @return the norm of this Quaternion
+   */
+  inline T norm() const;
+  
+  /*!
+   * @return the real part (==w) of this Quaternion
+   */
+  inline T re() const;
+  
+  /*!
+   * @return the imaginery part (== (x,y,z)) of this Quaternion
+   */
+  inline Vector3<T> im() const;
+	
+  T w,x,y,z;
+};
+
+
+
+template <class T> inline Quaternion<T> operator + (const Quaternion<T> & left, const Quaternion<T>& right);
+template <class T> inline Quaternion<T> operator - (const Quaternion<T> & left, const Quaternion<T>& right);
+template <class T> inline Quaternion<T> operator * (const Quaternion<T> & left, const Quaternion<T>& right);
+template <class T> inline Quaternion<T> operator * (const Quaternion<T> & left, const T scalar);
+template <class T> inline Quaternion<T> operator * (const T scalar, const Quaternion<T>& right);
+template <class T> std::ostream& operator << (std::ostream& os, const Quaternion<T>& q);
+
+template <class T> inline T innerproduct(const Quaternion<T>& left, const Quaternion<T>& right);
+template <class T> inline Quaternion<T> slerp(const Quaternion<T>& from, const Quaternion<T>& to, const T lambda);
+
+
+
+template <class T>
+struct Transformation3{
+  Quaternion<T> rotationQuaternion;
+  Vector3<T>    translationVector;
+
+  Transformation3(){}
+
+  inline static Transformation3<T> identity();
+
+  Transformation3 (const Vector3<T>& trans, const Quaternion<T>& rot);
+  Transformation3 (const Pose3<T>& v);
+  Transformation3 (const T& x, const T& y, const T& z, const T& roll, const T& pitch, const T& yaw);
+
+  inline Vector3<T> translation() const;
+  inline Quaternion <T> rotation() const;
+
+  inline Pose3<T> toPoseType() const;
+  
+  inline void setTranslation(const Vector3<T>& t);
+  inline void setTranslation(const T& x, const T& y, const T& z);
+
+  inline void setRotation(const Vector3<T>& r);
+  inline void setRotation(const T& roll, const T& pitch, const T& yaw);
+  inline void setRotation(const Quaternion<T>& q);
+
+
+  inline Transformation3<T> inv() const;
+  inline bool validRotation(const T& epsilon=0.001) const;
+
+};
+
+template <class T>
+inline Vector3<T> operator * (const Transformation3<T>& m, const Vector3<T>& v);
+
+template <class T>
+inline Transformation3<T> operator * (const Transformation3<T>& m1, const Transformation3<T>& m2);
+
+template <class T>
+struct Operations3D{
+  typedef T                  BaseType;
+  typedef Pose3<T>           PoseType;
+  typedef Quaternion<T>      RotationType;
+  typedef Vector3<T>         TranslationType;
+  typedef Transformation3<T> TransformationType;
+  typedef DMatrix<T>         CovarianceType;
+  typedef DMatrix<T>         InformationType;
+  typedef Transformation3<T> ParametersType;
+};
+
+} // namespace AISNavigation
+/**************************** IMPLEMENTATION ****************************/
+
+#include "transformation3.hxx"
+
+#endif
+

corelib/src/toro3d/transformation3.hxx

@@ -0,0 +1,451 @@
+/**********************************************************************
+ *
+ * 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.  
+ **********************************************************************/
+
+#include <limits>
+
+namespace AISNavigation {
+
+template <class T>
+inline Vector3<T> operator * (const T& d, const Vector3<T>& v) {
+  return Vector3<T>(v.elems[0]*d, v.elems[1]*d, v.elems[2]*d);
+}
+
+template <class T>
+inline Vector3<T> operator * (const Vector3<T>& v, const T& d) {
+  return Vector3<T>(v.elems[0]*d, v.elems[1]*d, v.elems[2]*d);
+}
+
+template <class T>
+inline T operator * (const Vector3<T>& v1, const Vector3<T>& v2){
+  return v1.elems[0]*v2.elems[0] 
+    + v1.elems[1]*v2.elems[1] 
+    + v1.elems[2]*v2.elems[2];
+}
+
+template <class T>
+inline Vector3<T> operator + (const Vector3<T>& v1, const Vector3<T>& v2){
+  return Vector3<T>(v1.elems[0]+v2.elems[0], 
+		    v1.elems[1]+v2.elems[1], 
+		    v1.elems[2]+v2.elems[2]);
+}
+
+template <class T>
+Vector3<T> operator - (const Vector3<T>& v1, const Vector3<T>& v2){
+  return Vector3<T>(v1.elems[0]-v2.elems[0], 
+		    v1.elems[1]-v2.elems[1],
+		    v1.elems[2]-v2.elems[2]);
+}
+
+
+
+template <class T>
+Pose3<T>::Pose3(): DVector<T>(6){
+}
+
+template <class T>
+Pose3<T>::Pose3(const Vector3<T>& trans, const Vector3<T>& rot): DVector<T>(6){
+  DVector<T>::elems[0]=rot.roll(); 
+  DVector<T>::elems[1]=rot.pitch(); 
+  DVector<T>::elems[2]=rot.yaw(); 
+  DVector<T>::elems[3]=trans.x(); 
+  DVector<T>::elems[4]=trans.y(); 
+  DVector<T>::elems[5]=trans.z();
+}
+
+template <class T>
+Pose3<T>::Pose3(const T& x, const T& y, const T& z, const T& r, const T& p, const T& yw): DVector<T>(6){
+  DVector<T>::elems[0]=r; 
+  DVector<T>::elems[1]=p; 
+  DVector<T>::elems[2]=yw; 
+  DVector<T>::elems[3]=x; 
+  DVector<T>::elems[4]=y; 
+  DVector<T>::elems[5]=z;
+}
+
+
+#define MY_MAX(a,b) (((a)>(b))?(a):(b))
+
+
+template<class T>
+Quaternion<T>::Quaternion(){
+	w = 1;
+	x = 0;
+	y = 0;
+	z = 0;
+}
+
+template<class T>
+Quaternion<T>::Quaternion(const Vector3<T>& pose){
+	w = 0;
+	x = pose.x();
+	y = pose.y();
+	z = pose.z();
+}
+
+template<class T>
+Quaternion<T>::Quaternion(const Vector3<T>& axis, const T& angle){
+  T sa=sin(angle/2);
+  T ca=cos(angle/2);
+  w=ca;
+  x=axis.x()*sa;
+  y=axis.y()*sa;
+  z=axis.z()*sa;
+}
+
+template<class T>
+Quaternion<T>::Quaternion(const T _w, const T _x, const T _y, const T _z){
+	w = _w;
+	x = _x;
+	y = _y;
+	z = _z;
+}
+
+template<class T>
+Quaternion<T>::Quaternion(const T phi, const T theta, const T psi){
+	T sphi   = sin(phi);
+	T stheta = sin(theta);
+	T spsi   = sin(psi);
+	T cphi   = cos(phi);
+	T ctheta = cos(theta);
+	T cpsi   = cos(psi);
+	
+	T _r[3][3] = { //create rotational Matrix
+		{cpsi*ctheta, cpsi*stheta*sphi - spsi*cphi, cpsi*stheta*cphi + spsi*sphi},
+		{spsi*ctheta, spsi*stheta*sphi + cpsi*cphi, spsi*stheta*cphi - cpsi*sphi},
+		{    -stheta,                  ctheta*sphi,                  ctheta*cphi}
+	};
+	
+	T _w = sqrt(MY_MAX(0, 1 + _r[0][0] + _r[1][1] + _r[2][2]))/2.0;
+	T _x = sqrt(MY_MAX(0, 1 + _r[0][0] - _r[1][1] - _r[2][2]))/2.0;
+	T _y = sqrt(MY_MAX(0, 1 - _r[0][0] + _r[1][1] - _r[2][2]))/2.0;
+	T _z = sqrt(MY_MAX(0, 1 - _r[0][0] - _r[1][1] + _r[2][2]))/2.0;
+	this->w = _w;
+	this->x = (_r[2][1] - _r[1][2])>=0?fabs(_x):-fabs(_x);
+	this->y = (_r[0][2] - _r[2][0])>=0?fabs(_y):-fabs(_y);
+	this->z = (_r[1][0] - _r[0][1])>=0?fabs(_z):-fabs(_z);
+}
+
+
+template<class T>
+inline Quaternion<T> Quaternion<T>::conjugated() const{
+	return Quaternion<T>(w,-x,-y,-z);
+}
+
+template<class T>
+inline Quaternion<T> Quaternion<T>::normalized() const{
+	T n = this->norm();
+	if (n > 0)
+		return ((1./n) * (*this));
+	else
+		return Quaternion<T>(0.,0.,0.,0.);
+}
+
+template<class T>
+inline Quaternion<T> Quaternion<T>::inverse() const{
+	return ((1./this->norm()) * this->conjugated());
+}
+
+template<class T>
+inline Quaternion<T> Quaternion<T>::rotateThisAlong(const Vector3<T>& axis, const T alpha) const{
+	Quaternion<T> q(axis);
+	q = q.normalized();
+	q = q.withRotation(alpha);
+	return q.rotatePoint(*this);
+}
+
+template<class T>
+inline Quaternion<T> Quaternion<T>::rotatePoint(const Quaternion<T>& p) const{
+	return (*this)*p*(this->conjugated());
+}
+
+template<class T>
+inline Vector3<T> Quaternion<T>::rotatePoint(const Vector3<T>& point) const{
+	Quaternion<T> p(point);
+	Quaternion<T> q = this->rotatePoint(p);
+	return q.im();
+}
+
+template<class T>
+inline Quaternion<T> Quaternion<T>::withRotation(const T alpha) const{
+	Quaternion<T> q = normalized();
+	T salpha = sin(alpha/2.);
+	T calpha = cos(alpha/2.);
+	q.w = calpha;
+	q.x = salpha * q.x;
+	q.y = salpha * q.y;
+	q.z = salpha * q.z;
+	return q;
+}
+
+
+template<class T>
+inline Vector3<T> Quaternion<T>::toAngles() const{
+	T n = this->norm();
+	T s = n > 0?2./(n*n):0.;
+	
+	T m00, m01, m02, m10, m11, m12, m20, m21, m22;
+	T phi,theta,psi;
+	
+	
+	T xs = this->x*s;
+	T ys = this->y*s;
+	T zs = this->z*s;
+	
+	T wx = this->w*xs;
+	T wy = this->w*ys;
+	T wz = this->w*zs;
+	
+	T xx = this->x*xs;
+	T xy = this->x*ys;
+	T xz = this->x*zs;
+	
+	T yy = this->y*ys;
+	T yz = this->y*zs;
+	
+	T zz = this->z*zs;
+	
+	m00 = 1.0 - (yy + zz);
+	m11 = 1.0 - (xx + zz);
+	m22 = 1.0 - (xx + yy);
+	
+	
+	m10 = xy + wz;
+	m01 = xy - wz;
+	
+	m20 = xz - wy;
+	m02 = xz + wy;
+	m21 = yz + wx;
+	m12 = yz - wx;
+	
+	phi   = atan2(m21,m22);
+	theta = atan2(-m20,sqrt(m21*m21 + m22*m22));
+	psi 	= atan2(m10,m00);
+	
+	return Vector3<T>(phi, theta, psi);
+}
+
+
+template<class T>
+inline Vector3<T> Quaternion<T>::axis() const {
+  double imNorm=sqrt(x*x+y*y+z*z);
+  if (imNorm<std::numeric_limits<double>::min()){
+    return Vector3<T>(0.,0.,1.);
+  }
+  return Vector3<T>(x/imNorm, y/imNorm, z/imNorm);
+}
+
+template<class T>
+inline T Quaternion<T>::angle() const{
+  Quaternion<T> q=normalized();
+  double a=2*atan2(sqrt(q.x*q.x + q.y*q.y  + q.z*q.z), q.w);
+  return atan2(sin(a), cos(a));
+}
+
+template<class T>
+inline T Quaternion<T>::norm() const{
+	return sqrt(w*w + x*x + y*y + z*z);
+}
+
+
+template<class T>
+inline T Quaternion<T>::re() const{
+	return w;
+}
+
+template<class T>
+inline Vector3<T> Quaternion<T>::im() const{
+	return Vector3<T>(x, y, z);
+}
+
+
+template<class T>
+inline Quaternion<T> operator + (const Quaternion<T>& left, const Quaternion<T>& right){
+	return Quaternion<T>(left.w + right.w, left.x + right.x, left.y + right.y, left.z + right.z);
+}
+
+template<class T>
+inline Quaternion<T> operator - (const Quaternion<T>& left, const Quaternion<T>& right){
+	return Quaternion<T>(left.w - right.w, left.x - right.x, left.y - right.y, left.z - right.z);
+}
+
+template<class T>
+inline Quaternion<T> operator * (const Quaternion<T>& q1, const Quaternion<T>& q2){
+  return Quaternion<T> (q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z,
+			q1.y*q2.z - q2.y*q1.z + q1.w*q2.x + q2.w*q1.x,
+			q1.z*q2.x - q2.z*q1.x + q1.w*q2.y + q2.w*q1.y,
+			q1.x*q2.y - q2.x*q1.y + q1.w*q2.z + q2.w*q1.z);
+}
+
+template<class T>
+inline Quaternion<T> operator * (const Quaternion<T>& q, const T s){
+	return Quaternion<T>(s*q.w, s*q.x, s*q.y, s*q.z);
+}
+
+template<class T>
+inline Quaternion<T> operator * (const T s, const Quaternion<T>& q){
+	return Quaternion<T>(q.w*s, q.x*s, q.y*s, q.z*s);
+}
+
+template<class T>
+std::ostream& operator << (std::ostream& os, const Quaternion<T>& q){
+	os << q.w << " " << q.x << " " << q.y << " " << q.z << " ";
+	return os;
+}
+
+template<class T>
+inline T innerproduct(const Quaternion<T>& q1, const Quaternion<T>& q2){
+	return q1.w*q2.w + q1.x*q2.x + q1.y*q2.y + q1.z*q2.z;
+}
+
+template<class T>
+inline Quaternion<T> slerp(const Quaternion<T>& from, const Quaternion<T>& to, const T lambda){
+	Quaternion<T> _from = from.normalized();
+	Quaternion<T> _to = to.normalized();
+	T _cos_omega = innerproduct(_from,_to);
+	_cos_omega = (_cos_omega>1)?1:_cos_omega;
+	_cos_omega = (_cos_omega<-1)?-1:_cos_omega;
+	T _omega = acos(_cos_omega);
+	assert (!isnan(_cos_omega));
+	if (fabs(_omega) < 1e-6)
+		return to;
+	
+	//determine right direction of slerp:
+	Quaternion<T> _pq = _from - _to;
+	Quaternion<T> _pmq = _from + _to;
+	T _first = _pq.norm();
+	T _alternativ = _pmq.norm();
+	
+	Quaternion<T> q1 = _from;
+	Quaternion<T> q2 = (_first < _alternativ)? (Quaternion<T>) _to: -1.*(Quaternion<T>)_to;
+	//now calculate intermediate quaternion.
+	
+	Quaternion<T> ret = q1*(sin((1-lambda)*_omega)/(sin(_omega))) + q2*(sin(lambda*_omega)/sin(_omega));
+	assert (!(isnan(ret.w) || isnan(ret.x) || isnan(ret.y) || isnan(ret.z)));
+	return ret;
+}
+
+
+template <class T>
+inline Transformation3<T> Transformation3<T>::identity(){
+  Transformation3<T> m;
+  m.rotationQuaternion=Quaternion<T>();
+  m.translationVector(0.,0.,0.);
+  return m;
+}
+
+
+
+template <class T>
+inline Transformation3<T>::Transformation3 (const T& x, const T& y, const T& z, const T& roll, const T& pitch, const T& yaw){
+  rotationQuaternion=Quaternion<T>(roll,pitch,yaw);
+  translationVector=Vector3<T>(x,y,z);
+}
+
+template <class T>
+inline Transformation3<T>::Transformation3 (const Pose3<T>& v){
+  rotationQuaternion=Quaternion<T>(v.roll(),v.pitch(),v.yaw());
+  translationVector=Vector3<T>(v.x(),v.y(),v.z());
+
+}
+
+template <class T>
+inline Vector3<T> Transformation3<T>::translation() const {
+  return translationVector;
+}
+  
+template <class T>
+inline Quaternion<T> Transformation3<T>::rotation()    const {
+  return rotationQuaternion;
+}
+  
+template <class T>
+inline Pose3<T> Transformation3<T>::toPoseType()   const {
+  Vector3<T> t=translation();
+  Vector3<T> r=rotationQuaternion.toAngles();
+  Pose3<T> rv(t.x(), t.y(), t.z(), r.roll(), r.pitch(), r.yaw() );
+  return rv;
+}
+  
+template <class T>
+inline void Transformation3<T>::setTranslation(const Vector3<T>& t){
+  translationVector=t;
+}
+
+template <class T>
+inline void Transformation3<T>::setRotation(const Quaternion<T>& q){
+  rotationQuaternion=q.normalized();
+}
+
+template <class T>
+inline void Transformation3<T>::setRotation(const Vector3<T>& r){
+  setRotation(r.roll(),r.pitch(), r.yaw());
+}
+
+template <class T>
+inline void Transformation3<T>::setRotation(const T& roll_phi, const T& pitch_theta, const T& yaw_psi){
+  rotationQuaternion=Quaternion<T>(roll_phi, pitch_theta, yaw_psi);
+}
+
+template <class T>
+inline void Transformation3<T>::setTranslation(const T& x, const T& y, const T& z){
+  translationVector=Vector3<T>(x,y,z);
+}
+
+
+template <class T>
+inline Transformation3<T> Transformation3<T>::inv() const {
+
+  Transformation3<T> rv(*this);
+  rv.rotationQuaternion=rotationQuaternion.inverse().normalized();
+  rv.translationVector=rv.rotationQuaternion.rotatePoint(translationVector*-1.);
+  return rv;
+}
+
+template <class T>
+inline Vector3<T> operator * (const Transformation3<T>& m, const Vector3<T>& v){
+  return m.translationVector+m.rotationQuaternion.rotatePoint(v);
+}
+
+template <class T>
+inline Transformation3<T> operator * (const Transformation3<T>& m1, const Transformation3<T>& m2){
+  Transformation3<T> rv;
+  rv.translationVector=m1.rotationQuaternion.rotatePoint(m2.translationVector)+m1.translationVector;
+  rv.rotationQuaternion=(m1.rotationQuaternion*m2.rotationQuaternion).normalized();
+  return rv;
+}
+
+} // namespace AISNavigation
+

corelib/src/toro3d/treeoptimizer3.cpp

@@ -0,0 +1,361 @@
+/**********************************************************************
+ *
+ * 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.  
+ **********************************************************************/
+
+/** \file treeoptimizer3.cpp
+ *
+ * \brief Defines the core optimizer class for 3D graphs which is a
+ * subclass of TreePoseGraph3
+ *
+ **/
+
+#include "treeoptimizer3.hh"
+#include <fstream>
+#include <sstream>
+#include <string>
+
+using namespace std;
+
+namespace AISNavigation {
+
+#define DEBUG(i) \
+        if (verboseLevel>i) cerr
+
+
+TreeOptimizer3::TreeOptimizer3(){
+  restartOnDivergence=false;
+  sortedEdges=0;
+  mpl=-1;
+  edgeCompareMode=EVComparator<Edge*>::CompareLevel;
+}
+
+
+TreeOptimizer3::~TreeOptimizer3(){
+}
+
+void TreeOptimizer3::initializeTreeParameters(){
+  ParameterPropagator pp;
+  treeDepthVisit(pp,root);
+}
+
+
+void TreeOptimizer3::iterate(TreePoseGraph3::EdgeSet* eset, bool noPreconditioner){
+  TreePoseGraph3::EdgeSet* temp=sortedEdges;
+  if (eset){
+    sortedEdges=eset;
+  }
+
+  if (noPreconditioner)
+    propagateErrors(false);
+  else {
+    if (iteration==1)
+      computePreconditioner();  
+    propagateErrors(true);
+  }
+  sortedEdges=temp;
+
+  onRestartBegin();
+  if (restartOnDivergence){
+    double mte, ate;
+    double mre, are;
+    error(&mre, &mte, &are, &ate);
+    maxTranslationalErrors.push_back(mte);
+    maxRotationalErrors.push_back(mre);
+    int interval=3;
+    if ((int)maxRotationalErrors.size()>=interval){
+      uint s=maxRotationalErrors.size();
+      double re0 = maxRotationalErrors[s-interval];
+      double re1 = maxRotationalErrors[s-1];
+
+      if ((re1-re0)>are || sqrt(re1)>0.99*M_PI){
+	double rg=rotGain;
+	if (sqrt(re1)>M_PI/4){
+	  cerr << "RESTART!!!!! : Angular wraparound may be occourring" << endl;
+	  cerr << " err=" << re0 << " -> " << re1 << endl; 
+	  cerr << "Restarting optimization and reducing the rotation factor" << endl;
+	  cerr << rg << " -> ";
+	  initializeOnTree();
+	  initializeTreeParameters();
+	  initializeOptimization();
+	  error(&mre, &mte);
+	  maxTranslationalErrors.push_back(mte);
+	  maxRotationalErrors.push_back(mre);
+	  rg*=0.1;
+	  rotGain=rg;
+	  cerr << rotGain << endl;
+	}
+	else {
+	  cerr << "decreasing angular gain" << rotGain*0.1 << endl;
+	  rotGain*=0.1;
+	}
+      }
+    }
+  }
+  onRestartDone();
+}
+
+
+void TreeOptimizer3::recomputeTransformations(Vertex*v, Vertex* top){
+  if (v==top)
+    return;
+  recomputeTransformations(v->parent, top);
+  v->transformation=v->parent->transformation*v->parameters;
+}
+
+
+void TreeOptimizer3::recomputeParameters(Vertex*v, Vertex* top){
+  while (v!=top){
+    v->parameters=v->parent->transformation.inv()*v->transformation;
+    v=v->parent;
+  }
+}
+
+
+TreeOptimizer3::Transformation TreeOptimizer3::getPose(Vertex*v, Vertex* top){
+  Transformation t(0.,0.,0.,0.,0.,0.);
+  if (v==top)
+    return v->transformation;
+  while (v!=top){
+    t=v->parameters*t;
+    v=v->parent;
+  }
+  return top->transformation*t;
+}
+
+TreeOptimizer3::Rotation TreeOptimizer3::getRotation(Vertex*v, Vertex* top){
+  Rotation r(0.,0.,0.);
+  if (v==top)
+    return v->transformation.rotation();
+  while (v!=top){
+    r=v->parameters.rotation()*r;
+    v=v->parent;
+  }
+  return top->transformation.rotation()*r;
+}
+
+
+
+double TreeOptimizer3::error(const Edge* e) const{
+  const Vertex* v1=e->v1;
+  const Vertex* v2=e->v2;
+  
+  Transformation et=e->transformation;
+  Transformation t1=v1->transformation;
+  Transformation t2=v2->transformation;
+  
+  Transformation t12=(t1*et)*t2.inv();
+  Pose p12=t12.toPoseType();
+
+  Pose ps=e->informationMatrix*p12;
+  double err=p12*ps;
+  DEBUG(100) << "e(" << v1->id << "," << v2->id << ")" << err << endl;
+  return err;
+ 
+}
+
+double TreeOptimizer3::traslationalError(const Edge* e) const{
+  const Vertex* v1=e->v1;
+  const Vertex* v2=e->v2;
+  
+  Transformation et=e->transformation;
+  Transformation t1=v1->transformation;
+  Transformation t2=v2->transformation;
+  
+  Translation t12=(t2.inv()*(t1*et)).translation();
+  return t12*t12;;
+ 
+}
+
+double TreeOptimizer3::rotationalError(const Edge* e) const{
+  const Vertex* v1=e->v1;
+  const Vertex* v2=e->v2;
+  
+  Rotation er=e->transformation.rotation();
+  Rotation r1=v1->transformation.rotation();
+  Rotation r2=v2->transformation.rotation();
+  
+  Rotation r12=r2.inverse()*(r1*er);
+  double r=r12.angle();
+  return r*r;
+}
+
+
+double TreeOptimizer3::loopError(const Edge* e) const{
+  double err=0;
+  const Vertex* v=e->v1;
+  while (v!=e->top){
+    err+=error(v->parentEdge);
+    v=v->parent;
+  }
+  v=e->v2;
+  while (v==e->top){
+    err+=error(v->parentEdge);
+    v=v->parent;
+  }
+  if (e->v2->parentEdge!=e && e->v1->parentEdge!=e)
+    err+=error(e);
+  return err;
+}
+
+double TreeOptimizer3::loopRotationalError(const Edge* e) const{
+  double err=0;
+  const Vertex* v=e->v1;
+  while (v!=e->top){
+    err+=rotationalError(v->parentEdge);
+    v=v->parent;
+  }
+  v=e->v2;
+  while (v!=e->top){
+    err+=rotationalError(v->parentEdge);
+    v=v->parent;
+  }
+  if (e->v2->parentEdge!=e && e->v1->parentEdge!=e)
+    err+=rotationalError(e);
+  return err;
+}
+
+
+double TreeOptimizer3::error(double* mre, double* mte, double* are, double* ate, TreePoseGraph3::EdgeSet* eset) const{
+  double globalRotError=0.;
+  double maxRotError=0;
+  double globalTrasError=0.;
+  double maxTrasError=0;
+
+  int c=0;
+  if (! eset){
+    for (TreePoseGraph3::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
+      double re=rotationalError(it->second);
+      globalRotError+=re;
+      maxRotError=maxRotError>re?maxRotError:re;
+      double te=traslationalError(it->second);
+      globalTrasError+=te;
+      maxTrasError=maxTrasError>te?maxTrasError:te;
+      c++;
+    }
+  } else {
+    for (TreePoseGraph3::EdgeSet::const_iterator it=eset->begin(); it!=eset->end(); it++){
+      const TreePoseGraph3::Edge* edge=*it;
+      double re=rotationalError(edge);
+      globalRotError+=re;
+      maxRotError=maxRotError>re?maxRotError:re;
+      double te=traslationalError(edge);
+      globalTrasError+=te;
+      maxTrasError=maxTrasError>te?maxTrasError:te;
+      c++;
+    }
+  }
+
+  if (mte)
+    *mte=maxTrasError;
+  if (mre)
+    *mre=maxRotError;
+  if (ate)
+    *ate=globalTrasError/c;
+  if (are)
+    *are=globalRotError/c;
+  return globalRotError+globalTrasError;
+}
+
+
+
+void TreeOptimizer3::initializeOptimization(EdgeCompareMode mode){
+  edgeCompareMode=mode;
+  // compute the size of the preconditioning matrix
+  int sz=maxIndex()+1;
+  DEBUG(1) << "Size= " << sz << endl;
+  M.resize(sz);
+
+  DEBUG(1) << "allocating M(" << sz << ")" << endl;
+  iteration=1;
+
+  // sorting edges
+  if (sortedEdges!=0){
+    delete sortedEdges;
+    sortedEdges=0;
+  }
+  sortedEdges=sortEdges();
+  mpl=maxPathLength();
+  rotGain=1.;
+  trasGain=1.;
+}
+
+void TreeOptimizer3::initializeOnlineIterations(){
+  int sz=maxIndex()+1;
+  DEBUG(1) << "Size= " << sz << endl;
+  M.resize(sz);
+  DEBUG(1) << "allocating M(" << sz << ")" << endl;
+  iteration=1;
+  maxRotationalErrors.clear();
+  maxTranslationalErrors.clear();
+  rotGain=1.;
+  trasGain=1.;
+}
+
+void TreeOptimizer3::initializeOnlineOptimization(EdgeCompareMode mode){
+  edgeCompareMode=mode;
+  // compute the size of the preconditioning matrix
+  clear();
+  Vertex* v0=addVertex(0,Pose(0,0,0,0,0,0));
+  root=v0;
+  v0->parameters=Transformation(v0->pose);
+  v0->parentEdge=0;
+  v0->parent=0;
+  v0->level=0;
+  v0->transformation=Transformation(TreePoseGraph3::Pose(0,0,0,0,0,0));
+}
+
+void TreeOptimizer3::onStepStart(Edge* e){
+  DEBUG(5) << "entering edge" << e << endl;
+}  
+void TreeOptimizer3::onStepFinished(Edge* e){
+  DEBUG(5) << "exiting edge" << e << endl;
+}
+
+void TreeOptimizer3::onIterationStart(int iteration){
+  DEBUG(5) << "entering iteration " << iteration << endl;
+}
+
+void TreeOptimizer3::onIterationFinished(int iteration){
+  DEBUG(5) << "exiting iteration " << iteration << endl;
+}
+
+void TreeOptimizer3::onRestartBegin(){}
+void TreeOptimizer3::onRestartDone(){}
+bool TreeOptimizer3::isDone(){
+  return false;
+}
+
+
+}; //namespace AISNavigation

corelib/src/toro3d/treeoptimizer3.hh

@@ -0,0 +1,181 @@
+/**********************************************************************
+ *
+ * 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.  
+ **********************************************************************/
+
+/** \file treeoptimizer3.hh
+ *
+ * \brief Defines the core optimizer class for 3D graphs which is a
+ * subclass of TreePoseGraph3
+ *
+ **/
+
+#ifndef _TREEOPTIMIZER3_HH_
+#define _TREEOPTIMIZER3_HH_
+
+#include "posegraph3.hh"
+
+namespace AISNavigation {
+
+/** \brief Class that contains the core optimization algorithm **/
+struct TreeOptimizer3: public TreePoseGraph3{
+  typedef std::vector<Pose> PoseVector;
+
+  /** Constructor **/
+  TreeOptimizer3();
+
+  /** Destructor **/
+  virtual ~TreeOptimizer3();
+
+  /** Initialization function **/
+  void initializeTreeParameters();
+
+  /** Initialization function **/
+  void initializeOptimization(EdgeCompareMode mode=EVComparator<Edge*>::CompareLevel);
+
+  void initializeOnlineOptimization(EdgeCompareMode mode=EVComparator<Edge*>::CompareLevel);
+
+  void initializeOnlineIterations();
+
+  /** Performs one iteration of the algorithm **/
+  void iterate(TreePoseGraph3::EdgeSet* eset=0, bool noPreconditioner=false);
+
+  /** Conmputes the gloabl error of the network **/
+  double error(double* mre=0, double* mte=0, double* are=0, double* ate=0, TreePoseGraph3::EdgeSet* eset=0) const;
+
+  /** Conmputes the gloabl error of the network **/
+  double angularError() const;
+
+  /** Conmputes the gloabl error of the network **/
+  double translationalError() const;
+
+  bool restartOnDivergence;
+
+  inline double getRotGain() const {return rotGain;} 
+
+  /** Iteration counter **/
+  int iteration;
+
+  double rpFraction;
+
+protected:
+
+  /** Recomputes only the pose of the node v wrt. to an arbitraty
+      parent (top) of v in the tree **/
+  Transformation getPose(Vertex*v, Vertex* top);
+
+  /** Recomputes only the pose of the node v wrt. to an arbitraty
+      parent (top) of v in the tree **/
+  Rotation getRotation(Vertex*v, Vertex* top);
+
+  void recomputeTransformations(Vertex*v, Vertex* top);
+
+  void recomputeParameters(Vertex*v, Vertex* top);
+
+  void computePreconditioner();
+
+  void propagateErrors(bool usePreconditioner=false);
+
+  /** Computes the error of the constraint/edge e **/
+  double error(const Edge* e) const;
+
+  /** Computes the error of the constraint/edge e **/
+  double loopError(const Edge* e) const;
+
+  /** Computes the rotational error of the constraint/edge e **/
+  double loopRotationalError(const Edge* e) const;
+  
+  /** Conmputes the error of the constraint/edge e **/
+  double translationalError(const Edge* e) const;
+
+  /** Conmputes the error of the constraint/edge e **/
+  double rotationalError(const Edge* e) const;
+
+  double traslationalError(const Edge* e) const;
+  
+  /** Used to compute the learning rate lambda **/
+  double gamma[2];
+
+  /** The simplified version of the preconditioning matrix **/
+  struct PM_t{
+    double v [2];
+    inline double& operator[](int i){return v[i];}
+  };
+  typedef std::vector< PM_t > PMVector;
+  PMVector M;
+
+  /**cached maximum path length*/
+  int mpl;
+
+  /**history of rhe maximum rotational errors*, used when adaptiveRestart is enabled */
+  std::vector<double> maxRotationalErrors;
+
+  /**history of rhe maximum rotational errors*, used when adaptiveRestart is enabled */
+  std::vector<double> maxTranslationalErrors;
+  double rotGain, trasGain;
+
+  /**callback invoked before starting the optimization of an individual constraint,
+   @param e: the constraint being optimized*/
+  virtual void onStepStart(Edge* e);
+
+  /**callback invoked after finishing the optimization of an individual constraint,
+   @param e: the constraint optimized*/
+  virtual void onStepFinished(Edge* e);
+
+  /**callback invoked before starting a full iteration,
+   @param i: the current iteration number*/
+  virtual void onIterationStart(int i);
+
+  /**callback invoked after finishing a full iteration,
+   @param i: the current iteration number*/
+
+  virtual void onIterationFinished(int iteration);
+
+  /**callback invoked before a restart of the optimizer
+   when the angular wraparound is detected*/
+  virtual void onRestartBegin();
+
+  /**callback invoked after a restart of the optimizer*/
+  virtual void onRestartDone();
+
+  /**callback for determining a termination condition,
+     it can be used by an external thread for stopping the optimizer while performing an iteration.
+     @returns true when the optimizer has to stop.*/
+  virtual bool isDone();
+  
+};
+
+}; //namespace AISNavigation
+
+#endif

corelib/src/toro3d/treeoptimizer3_iteration.cpp

@@ -0,0 +1,343 @@
+/**********************************************************************
+ *
+ * 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.  
+ **********************************************************************/
+
+#include "treeoptimizer3.hh"
+#include <fstream>
+#include <string>
+
+using namespace std;
+
+namespace AISNavigation {
+
+#define DEBUG(i) \
+        if (verboseLevel>i) cerr
+
+//helper functions. Should I explain :-)?
+inline double max3( const double& a, const double& b, const double& c){
+  double m=a>b?a:b;
+  return m>c?m:c;
+}
+inline double min3( const double& a, const double& b, const double& c){
+  double m=a<b?a:b;
+  return m<c?m:c;
+}
+
+
+struct NodeInfo{
+  TreeOptimizer3::Vertex* n;
+  double translationalWeight;
+  double rotationalWeight;
+  int direction;
+  TreeOptimizer3::Transformation transformation;
+  TreeOptimizer3::Transformation parameters;
+  NodeInfo(TreeOptimizer3::Vertex* v=0, double tw=0, double rw=0, int dir=0,
+	   TreeOptimizer3::Transformation t=TreeOptimizer3::Transformation(0,0,0,0,0,0), 
+	   TreeOptimizer3::Parameters p=TreeOptimizer3::Transformation(0,0,0,0,0,0)){
+    n=v;
+    translationalWeight=tw;
+    rotationalWeight=rw;
+    direction=dir;
+    transformation=t;
+    parameters=p;
+  }
+};
+
+typedef std::vector<NodeInfo> NodeInfoVector;
+
+
+/********************************** Preconditioned and unpreconditioned error distribution ************************************/
+
+
+
+
+
+void TreeOptimizer3::computePreconditioner(){
+  for (uint i=0; i<M.size(); i++){
+    M[i][0]=0;
+    M[i][1]=0;
+  }
+  gamma[0] = gamma[1] = numeric_limits<double>::max();
+
+  int edgeCount=0;
+  for (EdgeSet::iterator it=sortedEdges->begin(); it!=sortedEdges->end(); it++){
+    edgeCount++;
+    if (! (edgeCount%1000))
+      DEBUG(1) << "m";
+
+    Edge* e=*it;
+    Transformation t=e->transformation;
+    InformationMatrix W=e->informationMatrix;
+    
+    Vertex* top=e->top;
+    for (int dir=0; dir<2; dir++){
+      Vertex* n = (dir==0)? e->v1 : e->v2;
+      while (n!=top){
+	uint i=n->id;
+	double rW=min3(W[0][0], W[1][1], W[2][2]);
+	double tW=min3(W[3][3], W[4][4], W[5][5]);
+	M[i][0]+=rW;
+	M[i][1]+=tW;
+	gamma[0]=gamma[0]<rW?gamma[0]:rW;
+	gamma[1]=gamma[1]<tW?gamma[1]:tW;
+	n=n->parent;
+      }
+    }
+
+  }
+  
+  if (verboseLevel>1){
+    for (uint i=0; i<M.size(); i++){
+      cerr << "M[" << i << "]=" << M[i][0] << " " << M[i][1] << endl;
+    }
+  }
+}
+
+
+void TreeOptimizer3::propagateErrors(bool usePreconditioner){
+  iteration++;
+  int edgeCount=0;
+  // this is the workspace for computing the paths without 
+  // bothering too much the memory allocation
+  static NodeInfoVector path;
+  path.resize(edges.size()+1);
+  static Rotation zero(0.,0.,0.);
+
+  onIterationStart(iteration);
+  for (EdgeSet::iterator it=sortedEdges->begin(); it!=sortedEdges->end(); it++){
+    edgeCount++;
+    if (! (edgeCount%1000))
+      DEBUG(1) << "c";
+
+    if (isDone())
+      return;
+    
+    Edge* e=*it;
+    Vertex* top=e->top;
+    Vertex* v1=e->v1;
+    Vertex* v2=e->v2;
+    int l=e->length;
+    onStepStart(e);
+    
+    recomputeTransformations(v1,top);
+    recomputeTransformations(v2,top);
+    DEBUG(2) << "Edge: " << v1->id << " " << v2->id << ", top=" << top->id << ", length="<< l <<endl;
+
+    //BEGIN: Path and weight computation 
+    int pc=0;
+    Vertex* aux=v1;
+    double totTW=0, totRW=0;
+    while(aux!=top){
+      int index=aux->id;
+      double tw=1./(double)l, rw=1./(double)l;
+      if (usePreconditioner){
+	tw=1./M[index][0]; 
+	rw=1./M[index][1];
+      }
+      totTW+=tw;
+      totRW+=rw;
+      path[pc++]=NodeInfo(aux,tw,rw,-1,aux->transformation, aux->parameters);
+      aux=aux->parent;
+    }
+    int topIndex=pc;
+    path[pc++]=NodeInfo(top,0.,0.,0, top->transformation, top->parameters);
+    pc=l;
+    aux=v2;
+    while(aux!=top){
+      int index=aux->id;
+      double tw=1./l, rw=1./l;
+      if (usePreconditioner){
+	tw=1./M[index][0]; 
+	rw=1./M[index][1];
+      }
+      totTW+=tw;
+      totRW+=rw;
+      path[pc--]=NodeInfo(aux,tw,rw,1,aux->transformation, aux->parameters);
+      aux=aux->parent;
+    }
+
+    //store the transformations relative to the top node
+    Transformation topTransformation=top->transformation;
+    Transformation topParameters=top->parameters;
+
+    //END: Path and weight computation
+    
+
+
+    //BEGIN: Rotational Error
+    Rotation r1=getRotation(v1, top);
+    Rotation r2=getRotation(v2, top);
+    Rotation re=e->transformation.rotation();
+    Rotation rR=r2.inverse()*(r1*re);
+
+    double rotationFactor=(usePreconditioner)?
+      sqrt(double(l))* min3(e->informationMatrix[0][0],
+			    e->informationMatrix[1][1], 
+			    e->informationMatrix[2][2])/
+      ( gamma[0]* (double)iteration ):
+      sqrt(double(l))*rotGain/(double)iteration;
+
+//     double rotationFactor=(usePreconditioner)?
+//       sqrt(double(l))*rotGain/
+//       ( gamma[0]* (double)iteration * min3(e->informationMatrix[0][0],
+// 					   e->informationMatrix[1][1], 
+// 					   e->informationMatrix[2][2])):
+//       sqrt(double(l))*rotGain/(double)iteration;
+
+    if (rotationFactor>1)
+      rotationFactor=1;
+
+    Rotation totalRotation = path[l].transformation.rotation() * rR * path[l].transformation.rotation().inverse();
+
+    Translation axis   = totalRotation.axis();
+    double angle=totalRotation.angle();
+
+    double cw=0;
+    for (int i= 1; i<=topIndex; i++){
+      cw+=path[i-1].rotationalWeight/totRW;
+      Rotation R=path[i].transformation.rotation();
+      Rotation B(axis, angle*cw*rotationFactor);
+      R= B*R;
+      path[i].transformation.setRotation(R);
+    }
+    for (int i= topIndex+1; i<=l; i++){
+      cw+=path[i].rotationalWeight/totRW;
+      Rotation R=path[i].transformation.rotation();
+      Rotation B(axis, angle*cw*rotationFactor);
+      R= B*R;
+      path[i].transformation.setRotation(R);
+    }
+
+    //recompute the parameters based on the transformation
+    for (int i=0; i<topIndex; i++){
+      Vertex* n=path[i].n;
+      n->parameters.setRotation(path[i+1].transformation.rotation().inverse()*path[i].transformation.rotation());
+    }
+    for (int i= topIndex+1; i<=l; i++){
+      Vertex* n=path[i].n;
+      n->parameters.setRotation(path[i-1].transformation.rotation().inverse()*path[i].transformation.rotation());
+    }
+
+    //END: Rotational Error
+
+
+    //now spread the parameters
+    recomputeTransformations(v1,top);
+    recomputeTransformations(v2,top);
+
+    //BEGIN: Translational Error
+    Translation topTranslation=top->transformation.translation();
+   
+    Transformation tr12=v1->transformation*e->transformation;
+    Translation tR=tr12.translation()-v2->transformation.translation();
+
+
+//     double translationFactor=(usePreconditioner)?
+//       trasGain*l/( gamma[1]* (double)iteration * min3(e->informationMatrix[3][3],
+// 						      e->informationMatrix[4][4], 
+// 						      e->informationMatrix[5][5])):
+//       trasGain*l/(double)iteration;
+
+    double translationFactor=(usePreconditioner)?
+      trasGain*l*min3(e->informationMatrix[3][3],
+		      e->informationMatrix[4][4], 
+		      e->informationMatrix[5][5])/( gamma[1]* (double)iteration):
+      trasGain*l/(double)iteration;
+
+    if (translationFactor>1)
+      translationFactor=1;
+    Translation dt=tR*translationFactor;
+
+    //left wing
+    double lcum=0;
+    for (int i=topIndex-1; i>=0; i--){
+      Vertex* n=path[i].n;
+      lcum-=(usePreconditioner) ? path[i].translationalWeight/totTW :  1./(double)l;
+      double fraction=lcum;
+      Translation offset= dt*fraction;
+      Translation T=n->transformation.translation()+offset;
+      n->transformation.setTranslation(T);
+    }
+    //right wing
+    
+    double rcum=0;
+    for (int i=topIndex+1; i<=l; i++){
+      Vertex* n=path[i].n;
+      rcum+=(usePreconditioner) ? path[i].translationalWeight/totTW :  1./(double)l;
+      double fraction=rcum;
+      Translation offset= dt*fraction;
+      Translation T=n->transformation.translation()+offset;
+      n->transformation.setTranslation(T);
+    }
+    assert(fabs(lcum+rcum)-1<1e-6);
+
+    recomputeParameters(v1, top);
+    recomputeParameters(v2, top);
+
+    //END: Translational Error
+
+    onStepFinished(e);
+
+    if (verboseLevel>2){
+      Rotation newRotResidual=v2->transformation.rotation().inverse()*(v1->transformation.rotation()*re);
+      Translation newRotResidualAxis=newRotResidual.axis();
+      double      newRotResidualAngle=newRotResidual.angle();
+      Translation rotResidualAxis=rR.axis();
+      double      rotResidualAngle=rR.angle();
+      Translation newTransResidual=(v1->transformation*e->transformation).translation()-v2->transformation.translation();
+
+      cerr << "RotationalFraction:  " << rotationFactor << endl;
+      cerr << "Rotational residual: " 
+	   << " axis " << rotResidualAxis.x()  << "\t" << rotResidualAxis.y()  << "\t" << rotResidualAxis.z() << " --> "
+	   << "   ->  " << newRotResidualAxis.x()  << "\t" << newRotResidualAxis.y()  << "\t" << newRotResidualAxis.z()  << endl;
+      cerr << " angle " << rotResidualAngle  << "\t" << newRotResidualAngle  << endl;
+      
+      cerr << "Translational Fraction:  " << translationFactor << endl;
+      cerr << "Translational Residual" << endl;
+      cerr << "    " << tR.x()  << "\t" << tR.y()  << "\t" << tR.z()  << endl;
+      cerr << "    " << newTransResidual.x()  << "\t" << newTransResidual.y()  << "\t" << newTransResidual.z()  << endl;
+    }
+    
+    if (verboseLevel>101){
+      char filename [1000];
+      sprintf(filename, "po-%02d-%03d-%03d-.dat", iteration, v1->id, v2->id);
+      recomputeAllTransformations();
+      saveGnuplot(filename);
+    }
+  }
+  onIterationFinished(iteration);
+}
+
+};//namespace AISNavigation