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, GraphEdge >	EdgeMap
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< NodeSet >	connectedComponents (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

typedef pair<DistanceMap, PredecessorMap> pose_graph::DijkstraResult

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.

typedef map<GraphVertex, GraphVertex> pose_graph::PredecessorMap

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.