#include <pose_graph.h>

Public Member Functions
EdgeId	addEdge (NodeId from, NodeId to, const PoseConstraint &constraint)
	Adds an edge.
void	addEdge (NodeId from, NodeId to, const PoseConstraint &constraint, EdgeId id)
	Like addEdge, but with a given id.
NodeId	addNode ()
	Adds a node.
void	addNode (NodeId id)
	Adds a node with a given id.
std::set< EdgeId >	allEdges () const
	Gets set of edge ids in graph.
std::set< NodeId >	allNodes () const
	Gets set of node ids in graph.
void	attachCloud (NodeId id, occupancy_grid_utils::LocalizedCloud::ConstPtr cloud)
void	attachScan (NodeId id, sensor_msgs::LaserScan::ConstPtr cloud)
bool	edgeIdExists (EdgeId id) const
	Checks if an edge id already exists.
double	edgeLength (EdgeId id) const
	Gets length of an edge.
occupancy_grid_utils::LocalizedCloud::ConstPtr	getCloud (NodeId id) const
	Get the cloud attached to a node.
const PoseConstraint &	getConstraint (EdgeId id) const
	Gets the constraint attached to an edge.
const geometry_msgs::Pose &	getInitialPoseEstimate (NodeId n) const
	Gets the initial pose estimate attached to this node.
sensor_msgs::LaserScan::ConstPtr	getScan (NodeId id) const
	Get the scan attached to a node.
bool	hasCloud (NodeId id) const
bool	hasScan (NodeId id) const
std::set< EdgeId >	incidentEdges (NodeId n) const
	gets adjacent edges to a node
std::pair< NodeId, NodeId >	incidentNodes (EdgeId e) const
	gets incident nodes to an edge, in the order (tail, head).
std::set< NodeId >	nearbyNodes (NodeId src, double radius) const
	Nodes within a given radius of current one.
std::set< NodeId >	neighbors (NodeId n) const
	Gets neighboring nodes of a node.
bool	nodeIdExists (NodeId id) const
	Checks if a node id already exists.
PoseGraph &	operator= (const PoseGraph &g)
	Assignment operator.
NodeId	otherNode (const NodeId n, const EdgeId e) const
	PoseGraph ()
	Constructs an empty graph.
	PoseGraph (const PoseGraph &g)
	Copy constructor.
geometry_msgs::Pose	relativePose (NodeId n, NodeId ref) const
	Get relative pose of n w.r.t ref.
void	setInitialPoseEstimate (NodeId n, const geometry_msgs::Pose &pose)
	Sets the initial pose estimate attached to this node.
ShortestPathResult	shortestPath (NodeId src, NodeId dest) const
	Computes shortest path using Dijkstra's algorithm.
PoseGraph	subgraph (const std::set< NodeId > &nodes) const
	Get subgraph over a specific set of nodes.
	~PoseGraph ()
	Destructor.
Static Public Member Functions
static std::string	nodeFrameName (const NodeId id)
	Return the string naming the frame attached to a given node id.
Private Attributes
boost::scoped_ptr< PoseGraphImpl >	impl_

Detailed Description

Definition at line 89 of file pose_graph.h.

Constructor & Destructor Documentation

pose_graph::PoseGraph::PoseGraph ( )

Constructs an empty graph.

Definition at line 599 of file pose_graph.cpp.

pose_graph::PoseGraph::PoseGraph ( const PoseGraph & g )

Copy constructor.

Definition at line 601 of file pose_graph.cpp.

pose_graph::PoseGraph::~PoseGraph ( )

Destructor.

Definition at line 597 of file pose_graph.cpp.

Member Function Documentation

EdgeId pose_graph::PoseGraph::addEdge	(	NodeId	from,
		NodeId	to,
		const PoseConstraint &	constraint
	)

Adds an edge.

Parameters:

from	tail of new edge
to	head of new edge
constraint	PoseConstraint expressing head pose in frame of tail pose

Returns:: EdgeId of new edge

Exceptions:

UnknownNodeIdException if either id is not known

Definition at line 614 of file pose_graph.cpp.

void pose_graph::PoseGraph::addEdge	(	NodeId	from,
		NodeId	to,
		const PoseConstraint &	constraint,
		EdgeId	id
	)

Like addEdge, but with a given id.

Parameters:

from	tail of new edge
to	head of new edge
constraint	PoseConstraint expressing head pose in frame of tail pose
id	Desired id for new edge

Exceptions:

UnknownNodeIdException	if either id is not known
DuplicateEdgeIdException	if already exists

Definition at line 619 of file pose_graph.cpp.

NodeId pose_graph::PoseGraph::addNode ( )

Adds a node.

Returns:: NodeId of new node

Postcondition:: The new node's initial pose estimate is the identity pose.

Definition at line 610 of file pose_graph.cpp.

void pose_graph::PoseGraph::addNode ( NodeId id )

Adds a node with a given id.

Exceptions:

DuplicateNodeIdException if already exists

Postcondition:: The new node's initial pose estimate is the identity pose.

Definition at line 612 of file pose_graph.cpp.

set< EdgeId > pose_graph::PoseGraph::allEdges ( ) const

Gets set of edge ids in graph.

Definition at line 637 of file pose_graph.cpp.

set< NodeId > pose_graph::PoseGraph::allNodes ( ) const

Gets set of node ids in graph.

Definition at line 635 of file pose_graph.cpp.

void pose_graph::PoseGraph::attachCloud	(	NodeId	id,
		occupancy_grid_utils::LocalizedCloud::ConstPtr	cloud
	)

Attach a point cloud to a node. The pointed-to cloud should not change in future.

Exceptions:

UnknownNodeIdException

Parameters:

The	reference frame of the cloud is the node frame

Definition at line 669 of file pose_graph.cpp.

void pose_graph::PoseGraph::attachScan	(	NodeId	id,
		sensor_msgs::LaserScan::ConstPtr	cloud
	)

Attach a laser scan to a node. The pointed-to scan should not change in future.

Exceptions:

UnknownNodeIdException

Parameters:

The	reference frame of the scan is the node frame

Note that the robot might have been moving while the scan was taken, so the data may not be valid in any given fixed frame (due to the LaserScan type's assumption of a constant angular increment). So it's preferable to transform the scan to a point cloud for most cases. The scan option is provided for compatibility with Karto.

Definition at line 684 of file pose_graph.cpp.

bool pose_graph::PoseGraph::edgeIdExists ( EdgeId id ) const

Checks if an edge id already exists.

Definition at line 654 of file pose_graph.cpp.

double pose_graph::PoseGraph::edgeLength ( EdgeId id ) const

Gets length of an edge.

Exceptions:

UnknownEdgeIdException

Definition at line 644 of file pose_graph.cpp.

LocalizedCloud::ConstPtr pose_graph::PoseGraph::getCloud ( NodeId id ) const

Get the cloud attached to a node.

Return values:

A	pointer whose pointed-to cloud is guaranteed stable

Exceptions:

UnknownNodeIdException
PoseGraphException	(if no cloud found for this node)

Definition at line 674 of file pose_graph.cpp.

const PoseConstraint & pose_graph::PoseGraph::getConstraint ( EdgeId id ) const

Gets the constraint attached to an edge.

Return values:

A	reference that is valid so long as edge is in graph.

Exceptions:

UnknownEdgeIdException

Definition at line 639 of file pose_graph.cpp.

const Pose & pose_graph::PoseGraph::getInitialPoseEstimate ( NodeId n ) const

Gets the initial pose estimate attached to this node.

Exceptions:

UnknownNodeIdException

Return values:

A	reference valid as long as the node exists

Definition at line 630 of file pose_graph.cpp.

LaserScan::ConstPtr pose_graph::PoseGraph::getScan ( NodeId id ) const

Get the scan attached to a node.

Return values:

A	pointer whose pointed-to scan is guaranteed stable

Exceptions:

UnknownNodeIdException
PoseGraphException	(if no scan found for this node)

See comments for attachScan

Definition at line 689 of file pose_graph.cpp.

bool pose_graph::PoseGraph::hasCloud ( NodeId id ) const

Return true iff the node has an attached cloud

Exceptions:

UnknownNodeIdException

Definition at line 679 of file pose_graph.cpp.

bool pose_graph::PoseGraph::hasScan ( NodeId id ) const

Return true iff the node has an attached scan

Exceptions:

UnknownNodeIdException

Definition at line 694 of file pose_graph.cpp.

set< EdgeId > pose_graph::PoseGraph::incidentEdges ( NodeId n ) const

gets adjacent edges to a node

Exceptions:

UnknownNodeIdException

Definition at line 646 of file pose_graph.cpp.

pair< NodeId, NodeId > pose_graph::PoseGraph::incidentNodes ( EdgeId e ) const

gets incident nodes to an edge, in the order (tail, head).

Exceptions:

UnknownEdgeIdException

Definition at line 650 of file pose_graph.cpp.

set< NodeId > pose_graph::PoseGraph::nearbyNodes	(	NodeId	src,
		double	radius
	)		const

Nodes within a given radius of current one.

Parameters:

src	Source node
radius

Exceptions:

UnknownNodeIdException

Return values:

Set	of nodes, including src, whose shortest path distance from src is <= radius

Definition at line 664 of file pose_graph.cpp.

NodeIdSet pose_graph::PoseGraph::neighbors ( NodeId n ) const

Gets neighboring nodes of a node.

Exceptions:

UnknownNodeIdException

Definition at line 648 of file pose_graph.cpp.

string pose_graph::PoseGraph::nodeFrameName ( const NodeId id ) [static]

Return the string naming the frame attached to a given node id.

Definition at line 699 of file pose_graph.cpp.

bool pose_graph::PoseGraph::nodeIdExists ( NodeId id ) const

Checks if a node id already exists.

Definition at line 652 of file pose_graph.cpp.

PoseGraph & pose_graph::PoseGraph::operator= ( const PoseGraph & g )

Assignment operator.

Definition at line 603 of file pose_graph.cpp.

NodeId pose_graph::PoseGraph::otherNode	(	const NodeId	n,
		const EdgeId	e
	)		const

Precondition:: n is one of the nodes incident to edge e \ retval Node The other incident node

Definition at line 709 of file pose_graph.cpp.

Pose pose_graph::PoseGraph::relativePose	(	NodeId	n,
		NodeId	ref
	)		const

Get relative pose of n w.r.t ref.

Exceptions:

DisconnectedNodesException	if there's no path between ref and n
UnknownNodeIdException	If there are multiple paths from ref to n, just use the shortest one

Definition at line 704 of file pose_graph.cpp.

void pose_graph::PoseGraph::setInitialPoseEstimate	(	NodeId	n,
		const geometry_msgs::Pose &	pose
	)

Sets the initial pose estimate attached to this node.

Exceptions:

UnknownNodeIdException

Definition at line 625 of file pose_graph.cpp.

ShortestPathResult pose_graph::PoseGraph::shortestPath	(	NodeId	src,
		NodeId	dest
	)		const

Computes shortest path using Dijkstra's algorithm.

Parameters:

src	Source node id
dest	Destination node id

Exceptions:

UnknownNodeIdException

Definition at line 659 of file pose_graph.cpp.

PoseGraph pose_graph::PoseGraph::subgraph ( const std::set< NodeId > & nodes ) const

Get subgraph over a specific set of nodes.

The returned graph consists of the given nodes, with the same ids and all edges between them. It does not share any structure with the original graph.

Definition at line 656 of file pose_graph.cpp.

Member Data Documentation

boost::scoped_ptr<PoseGraphImpl> pose_graph::PoseGraph::impl_ [private]

Definition at line 272 of file pose_graph.h.

The documentation for this class was generated from the following files:

Public Member Functions

Static Public Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation