pose_graph Namespace Reference

Classes

struct  AllEdges
 dummy predicate that always returns true for edges More...
struct  AllNodes
class  CachedNodeMap
 A map<unsigned, M::ConstPtr> backed by a warehouse collection. More...
class  ConstraintGraph
class  ConstraintGraphVisualizer
 Class used to visualize constraint graph. More...
struct  DataNotFoundException
 Could not find exactly one entry for a given topic-node pair. More...
class  DiffPublisher
class  DiffSubscriber
struct  DisconnectedComponentException
 Exception when trying to optimize a disconnected graph. More...
struct  DuplicateEdgeIdException
 Exception for trying to add an EdgeId that already exists. More...
struct  DuplicateNodeIdException
 Exception for trying to add a NodeId that already exists. More...
struct  NodeDuplicateDataException
 Node n already had data. More...
struct  NoOptimizedPoseException
 Exception for when there's no optimized pose associated with a graph node. More...
struct  NoPathException
 Exception when no path found in graph between two nodes. More...
struct  OptimizedDistancePredicate
class  PoseGraphException
 A base class for all pose_graph exceptions; provides a handy boost::format parent constructor. More...
struct  Spa2DConversionResult
struct  UnknownDBTopicException
 Topic doesn't exist in the graph db. More...
struct  UnknownEdgeIdException
 Exception for unknown EdgeId. More...
struct  UnknownNodeIdException
 Exception for unknown NodeId. More...
class  Visitor

Typedefs

typedef pair< DistanceMap,
PredecessorMap
DijkstraResult
typedef map< GraphVertex, double > DistanceMap
typedef map< unsigned, GraphEdgeEdgeMap
typedef std::set< unsigned > EdgeSet
typedef boost::adjacency_list
< boost::multisetS,
boost::listS,
boost::undirectedS, msg::Node,
msg::Edge > 
Graph
typedef boost::graph_traits
< Graph >::edge_descriptor 
GraphEdge
typedef boost::graph_traits
< Graph >::vertex_descriptor 
GraphVertex
typedef pair< unsigned, unsigned > IncidentNodes
typedef boost::mutex::scoped_lock Lock
typedef std::map< unsigned,
unsigned > 
NodeIndexMap
typedef map< unsigned,
GraphVertex
NodeMap
typedef std::pair< unsigned,
unsigned > 
NodePair
typedef std::map< unsigned,
tf::Pose > 
NodePoseMap
typedef std::set< unsigned > NodeSet
typedef map< GraphVertex,
GraphVertex
PredecessorMap
typedef std::pair< std::vector
< unsigned >, std::vector
< unsigned > > 
ShortestPath
typedef boost::shared_ptr
< sba::SysSPA2d > 
Spa2DPtr

Functions

void applyDiff (ConstraintGraph *graph, const msg::ConstraintGraphDiff &diff)
 Apply a diff to a ConstraintGraph.
NodeSet componentContaining (const ConstraintGraph &g, unsigned n)
std::vector< NodeSetconnectedComponents (const ConstraintGraph &g)
ConstraintGraph constraintGraphFromMessage (const msg::ConstraintGraphMessage &msg)
 Create a ConstraintGraph from a ROS message.
msg::ConstraintGraphMessage constraintGraphToMessage (const ConstraintGraph &graph)
 Convert a ConstraintGraph to the corresponding ros message.
Spa2DConversionResult constraintGraphToSpa2D (const ConstraintGraph &g, const NodePoseMap &init, const NodeSet &nodes)
DijkstraResult dijkstra (const Graph &g, const GraphVertex &src)
template<class NodePred , class EdgePred >
NodeSet filterNearbyNodes (const ConstraintGraph &g, const unsigned n, const NodePred &node_pred, const EdgePred &edge_pred)
template<class NodePredicate , class EdgePredicate >
NodeSet filterNearbyNodes (const ConstraintGraph &g, const unsigned n, const NodePredicate &node_pred, const EdgePredicate &edge_pred)
template<class Predicate >
NodeSet filterNearbyNodes (const ConstraintGraph &g, const unsigned n, const Predicate &p)
template<class M >
M::ConstPtr getNodeData (wh::Collection< M > *client, const unsigned n)
template<class M >
M::ConstPtr getNodeData (warehouse::Collection< M > *coll, unsigned n)
 Get the message associated with node n of the graph.
tf::Pose getNodePose (const sba::Node2d &n)
 Return the Pose corresponding to a spa node.
double getYaw (const Quaterniond &q)
unsigned index (const unsigned i, const unsigned j)
Node2d makeNode (const double x, const double y, const double theta)
Con2dP2 makeSpa2DConstraint (const PoseWithPrecision &constraint)
std::vector< std::string > nodeIndex ()
 Use to index things by node.
std::string nodeMetadata (unsigned n)
 Generate the metadata to associate a node n of the graph to a message being stored in the warehouse.
NodePoseMap optimizeGraph2D (const ConstraintGraph &g, const NodePoseMap &init, const NodeSet &nodes)
 Optimize a component of a graph using SPA 2d.
NodePoseMap optimizeGraph2D (const ConstraintGraph &g, const NodePoseMap &init)
 Optimize a graph using SPA 2d init Initial estimates.
Eigen::Vector4d quaternionMsgToVector4d (const geometry_msgs::Quaternion &m)
tf::Pose relativePose (const ConstraintGraph &g, const unsigned n1, const unsigned n2)
ShortestPath shortestPath (const ConstraintGraph &g, const unsigned src, const unsigned dest)
Eigen::Vector4d translationToVector4d (const geometry_msgs::Point &p)
void verifyConnected (const ConstraintGraph &g, const NodeSet &nodes)

Typedef Documentation

Definition at line 97 of file graph_search.cpp.

typedef map<GraphVertex, double> pose_graph::DistanceMap

Definition at line 95 of file graph_search.cpp.

typedef map<unsigned, GraphEdge> pose_graph::EdgeMap

Definition at line 52 of file constraint_graph.cpp.

typedef std::set<unsigned> pose_graph::EdgeSet

Definition at line 74 of file constraint_graph.h.

typedef boost::adjacency_list<boost::multisetS, boost::listS, boost::undirectedS, msg::Node, msg::Edge> pose_graph::Graph

Definition at line 63 of file constraint_graph.h.

typedef boost::graph_traits<Graph>::edge_descriptor pose_graph::GraphEdge

Definition at line 64 of file constraint_graph.h.

typedef boost::graph_traits<Graph>::vertex_descriptor pose_graph::GraphVertex

Definition at line 65 of file constraint_graph.h.

typedef pair<unsigned, unsigned> pose_graph::IncidentNodes

Definition at line 50 of file constraint_graph.cpp.

typedef boost::mutex::scoped_lock pose_graph::Lock

Definition at line 61 of file graph_db_impl.h.

typedef std::map<unsigned, unsigned> pose_graph::NodeIndexMap

Definition at line 49 of file spa_2d_conversion.h.

typedef map<unsigned, GraphVertex> pose_graph::NodeMap

Definition at line 51 of file constraint_graph.cpp.

typedef std::pair<unsigned, unsigned> pose_graph::NodePair

Definition at line 43 of file spa_2d_conversion.cpp.

typedef std::map<unsigned, tf::Pose> pose_graph::NodePoseMap

Definition at line 72 of file constraint_graph.h.

typedef std::set<unsigned> pose_graph::NodeSet

Definition at line 73 of file constraint_graph.h.

Definition at line 96 of file graph_search.cpp.

typedef std::pair<std::vector<unsigned>, std::vector<unsigned> > pose_graph::ShortestPath

Definition at line 116 of file graph_search.h.

typedef boost::shared_ptr<sba::SysSPA2d> pose_graph::Spa2DPtr

Definition at line 48 of file spa_2d_conversion.h.


Function Documentation

void pose_graph::applyDiff ( ConstraintGraph *  graph,
const msg::ConstraintGraphDiff &  diff 
)

Apply a diff to a ConstraintGraph.

Definition at line 95 of file message_conversion.cpp.

NodeSet pose_graph::componentContaining ( const ConstraintGraph &  g,
unsigned  n 
)
Return values:
Component containing node n

Definition at line 84 of file graph_search.cpp.

std::vector< NodeSet > pose_graph::connectedComponents ( const ConstraintGraph &  g  ) 
Return values:
A vector of sets of nodes, each of which is one of the connected components of the graph

Definition at line 52 of file graph_search.cpp.

ConstraintGraph pose_graph::constraintGraphFromMessage ( const msg::ConstraintGraphMessage &  msg  ) 

Create a ConstraintGraph from a ROS message.

Definition at line 76 of file message_conversion.cpp.

msg::ConstraintGraphMessage pose_graph::constraintGraphToMessage ( const ConstraintGraph &  graph  ) 

Convert a ConstraintGraph to the corresponding ros message.

Definition at line 46 of file message_conversion.cpp.

Spa2DConversionResult pose_graph::constraintGraphToSpa2D ( const ConstraintGraph &  g,
const NodePoseMap &  init,
const NodeSet &  nodes 
)

Definition at line 117 of file spa_2d_conversion.cpp.

DijkstraResult pose_graph::dijkstra ( const Graph &  g,
const GraphVertex &  src 
)

Definition at line 99 of file graph_search.cpp.

template<class NodePred , class EdgePred >
NodeSet pose_graph::filterNearbyNodes ( const ConstraintGraph &  g,
const unsigned  n,
const NodePred &  node_pred,
const EdgePred &  edge_pred 
) [inline]

Definition at line 67 of file graph_search_impl.h.

template<class NodePredicate , class EdgePredicate >
NodeSet pose_graph::filterNearbyNodes ( const ConstraintGraph &  g,
const unsigned  n,
const NodePredicate &  node_pred,
const EdgePredicate &  edge_pred 
) [inline]
Returns:
The largest connected component of the graph that 1) includes n 2) contains only nodes that satisfy the predicate 3) uses only edges that satisfy the predicate
Template Parameters:
NodePredicate a default-constructible type with operator() (GraphVertex) const defined
EdgePredicate a default-constructible type with operator() (GraphEdge) const defined
template<class Predicate >
NodeSet pose_graph::filterNearbyNodes ( const ConstraintGraph &  g,
const unsigned  n,
const Predicate &  p 
) [inline]
Returns:
The largest connected component of the graph that 1) includes n 2) contains only nodes that satisfy the predicate,
Template Parameters:
Predicate a default-constructible type with operator() (GraphVertex) const defined

Definition at line 51 of file graph_search_impl.h.

template<class M >
M::ConstPtr pose_graph::getNodeData ( wh::Collection< M > *  client,
const unsigned  n 
) [inline]

Definition at line 65 of file graph_db_impl.h.

template<class M >
M::ConstPtr pose_graph::getNodeData ( warehouse::Collection< M > *  coll,
unsigned  n 
) [inline]

Get the message associated with node n of the graph.

Exceptions:
DataNotFoundException 
WarehouseClientException (if communication with warehouse fails)
tf::Pose pose_graph::getNodePose ( const sba::Node2d &  n  ) 

Return the Pose corresponding to a spa node.

Definition at line 160 of file spa_2d_conversion.cpp.

double pose_graph::getYaw ( const Quaterniond &  q  ) 

Definition at line 72 of file spa_2d_conversion.cpp.

unsigned pose_graph::index ( const unsigned  i,
const unsigned  j 
)

Definition at line 78 of file spa_2d_conversion.cpp.

Node2d pose_graph::makeNode ( const double  x,
const double  y,
const double  theta 
)

Definition at line 58 of file spa_2d_conversion.cpp.

Con2dP2 pose_graph::makeSpa2DConstraint ( const PoseWithPrecision &  constraint  ) 

Definition at line 83 of file spa_2d_conversion.cpp.

std::vector<std::string> pose_graph::nodeIndex (  )  [inline]

Use to index things by node.

Definition at line 59 of file graph_db.h.

string pose_graph::nodeMetadata ( unsigned  n  ) 

Generate the metadata to associate a node n of the graph to a message being stored in the warehouse.

Definition at line 44 of file graph_db.cpp.

NodePoseMap pose_graph::optimizeGraph2D ( const ConstraintGraph &  g,
const NodePoseMap &  init,
const NodeSet &  nodes 
)

Optimize a component of a graph using SPA 2d.

Optimize subset of nodes.

Return values:
map from node id to optimized pose
Exceptions:
DisconnectedGraphException 

Definition at line 173 of file spa_2d_conversion.cpp.

NodePoseMap pose_graph::optimizeGraph2D ( const ConstraintGraph &  g,
const NodePoseMap &  init 
)

Optimize a graph using SPA 2d init Initial estimates.

Return values:
map from node id to optimized pose
Exceptions:
DisconnectedGraphException 

Definition at line 167 of file spa_2d_conversion.cpp.

Eigen::Vector4d pose_graph::quaternionMsgToVector4d ( const geometry_msgs::Quaternion &  m  ) 

Definition at line 46 of file spa_2d_conversion.cpp.

tf::Pose pose_graph::relativePose ( const ConstraintGraph &  g,
const unsigned  n1,
const unsigned  n2 
)
Return values:
Relative pose of n2 w.r.t n1 formed by using constraints along shortest path
Exceptions:
NoPathException 

Definition at line 189 of file graph_search.cpp.

ShortestPath pose_graph::shortestPath ( const ConstraintGraph &  g,
const unsigned  src,
const unsigned  dest 
)
Return values:
Shortest path between two nodes, where edge cost is magnitude of relative pose attached to edge constraint.
Todo:
Use covariance information here
Exceptions:
NoPathException 

Definition at line 150 of file graph_search.cpp.

Eigen::Vector4d pose_graph::translationToVector4d ( const geometry_msgs::Point &  p  ) 

Definition at line 51 of file spa_2d_conversion.cpp.

void pose_graph::verifyConnected ( const ConstraintGraph &  g,
const NodeSet &  nodes 
)

Definition at line 103 of file spa_2d_conversion.cpp.

 All Classes Namespaces Files Functions Variables Typedefs Defines


pose_graph
Author(s): Bhaskara Marthi
autogenerated on Fri Jan 11 09:37:08 2013