mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-09 03:07:45 +08:00
Merged pcl_integration branch to trunk
git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1014 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
@@ -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
|
||||
Reference in New Issue
Block a user