mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-04 00:37:46 +08:00
matlabbe
8b8511e154
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1014 f169173b-cf89-36c8-b27e-44dbe73f0c83
275 lines
8.8 KiB
C++
275 lines
8.8 KiB
C++
/**********************************************************************
|
|
*
|
|
* 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
|