#include <pose_graph_impl.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.
set< EdgeId >	allEdges () const
	Gets set of edge ids in graph.
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 point 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::LaserScanConstPtr	getScan (NodeId id) const
	Get the scan attached to a node.
bool	hasCloud (NodeId id) const
bool	hasScan (NodeId id) const
set< EdgeId >	incidentEdges (NodeId n) const
	gets adjacent edges to a node
pair< NodeId, NodeId >	incidentNodes (EdgeId e) const
	gets incident nodes to an edge, in the order (tail, head).
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.
PoseGraphImpl &	operator= (const PoseGraphImpl &g)
	Assignment operator.
NodeId	otherNode (const NodeId n, const EdgeId e) const
	PoseGraphImpl ()
	Constructs an empty graph.
	PoseGraphImpl (const PoseGraphImpl &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.
Private Types
typedef pair< DistanceMap, PredecessorMap >	DijkstraResult
typedef map< PoseGraphVertex, double >	DistanceMap
typedef map< PoseGraphVertex, PoseGraphVertex >	PredecessorMap
Private Member Functions
DijkstraResult	dijkstra (const PoseGraphVertex &src) const
PoseGraphEdge	idEdge (EdgeId id) const
PoseGraphVertex	idVertex (NodeId id) const
void	initializeFrom (const PoseGraphImpl &g)
bool	outsideRadius (NodeId n, double r, const DistanceMap &distances) const
EdgeId	shortestEdgeBetween (NodeId n1, NodeId n2) const
	Return shortest edge between two nodes. Asserts if there's no edge between them.
Private Attributes
map< PoseGraphVertex, occupancy_grid_utils::LocalizedCloud::ConstPtr >	clouds_
PoseConstraintMap	constraints_
map< EdgeId, PoseGraphEdge >	edge_map_
PoseGraphAdjacencyList	graph_
EdgeId	next_edge_id_
NodeId	next_node_id_
map< PoseGraphVertex, sensor_msgs::LaserScan::ConstPtr >	scans_
map< NodeId, PoseGraphVertex >	vertex_map_

Detailed Description

Definition at line 110 of file pose_graph_impl.h.

Member Typedef Documentation

typedef pair<DistanceMap, PredecessorMap> pose_graph::PoseGraphImpl::DijkstraResult [private]

Definition at line 296 of file pose_graph_impl.h.

typedef map<PoseGraphVertex, double> pose_graph::PoseGraphImpl::DistanceMap [private]

Definition at line 294 of file pose_graph_impl.h.

typedef map<PoseGraphVertex, PoseGraphVertex> pose_graph::PoseGraphImpl::PredecessorMap [private]

Definition at line 295 of file pose_graph_impl.h.

Constructor & Destructor Documentation

pose_graph::PoseGraphImpl::PoseGraphImpl ( )

Constructs an empty graph.

Definition at line 144 of file pose_graph.cpp.

pose_graph::PoseGraphImpl::PoseGraphImpl ( const PoseGraphImpl & g )

Copy constructor.

Definition at line 149 of file pose_graph.cpp.

Member Function Documentation

EdgeId pose_graph::PoseGraphImpl::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 242 of file pose_graph.cpp.

void pose_graph::PoseGraphImpl::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 252 of file pose_graph.cpp.

NodeId pose_graph::PoseGraphImpl::addNode ( )

Adds a node.

Returns:: NodeId of new node

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

Definition at line 210 of file pose_graph.cpp.

void pose_graph::PoseGraphImpl::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 220 of file pose_graph.cpp.

EdgeIdSet pose_graph::PoseGraphImpl::allEdges ( ) const

Gets set of edge ids in graph.

Definition at line 291 of file pose_graph.cpp.

NodeIdSet pose_graph::PoseGraphImpl::allNodes ( ) const

Gets set of node ids in graph.

Definition at line 282 of file pose_graph.cpp.

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

Attach a point cloud to a node.

Exceptions:

UnknownNodeIdException

Definition at line 521 of file pose_graph.cpp.

void pose_graph::PoseGraphImpl::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 569 of file pose_graph.cpp.

PoseGraphImpl::DijkstraResult pose_graph::PoseGraphImpl::dijkstra ( const PoseGraphVertex & src ) const [private]

Definition at line 397 of file pose_graph.cpp.

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

Checks if an edge id already exists.

Definition at line 84 of file pose_graph.cpp.

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

Gets length of an edge.

Exceptions:

UnknownEdgeIdException

Definition at line 349 of file pose_graph.cpp.

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

Get the point cloud attached to a node.

Exceptions:

UnknownNodeIdException
PoseGraphException	(if no cloud found for this node)

Definition at line 528 of file pose_graph.cpp.

const PoseConstraint & pose_graph::PoseGraphImpl::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 331 of file pose_graph.cpp.

const Pose & pose_graph::PoseGraphImpl::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 342 of file pose_graph.cpp.

LaserScan::ConstPtr pose_graph::PoseGraphImpl::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 576 of file pose_graph.cpp.

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

Return true iff the node has an attached cloud

Exceptions:

UnknownNodeIdException

Definition at line 538 of file pose_graph.cpp.

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

Return true iff the node has an attached scan

Exceptions:

UnknownNodeIdException

Definition at line 585 of file pose_graph.cpp.

PoseGraphEdge pose_graph::PoseGraphImpl::idEdge ( EdgeId id ) const [private]

Definition at line 97 of file pose_graph.cpp.

PoseGraphVertex pose_graph::PoseGraphImpl::idVertex ( NodeId id ) const [private]

Definition at line 89 of file pose_graph.cpp.

EdgeIdSet pose_graph::PoseGraphImpl::incidentEdges ( NodeId n ) const

gets adjacent edges to a node

Exceptions:

UnknownNodeIdException

Definition at line 308 of file pose_graph.cpp.

IncidentNodes pose_graph::PoseGraphImpl::incidentNodes ( EdgeId e ) const

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

Exceptions:

UnknownEdgeIdException

Definition at line 300 of file pose_graph.cpp.

void pose_graph::PoseGraphImpl::initializeFrom ( const PoseGraphImpl & g ) [private]

Clear out data associated with the old graph

Definition at line 162 of file pose_graph.cpp.

NodeIdSet pose_graph::PoseGraphImpl::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 500 of file pose_graph.cpp.

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

Gets neighboring nodes of a node.

Exceptions:

UnknownNodeIdException

Definition at line 321 of file pose_graph.cpp.

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

Checks if a node id already exists.

Definition at line 79 of file pose_graph.cpp.

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

Assignment operator.

Definition at line 154 of file pose_graph.cpp.

NodeId pose_graph::PoseGraphImpl::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 126 of file pose_graph.cpp.

bool pose_graph::PoseGraphImpl::outsideRadius	(	NodeId	n,
		double	r,
		const DistanceMap &	distances
	)		const `[private]`

Definition at line 493 of file pose_graph.cpp.

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

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

Exceptions:

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

Definition at line 361 of file pose_graph.cpp.

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

Sets the initial pose estimate attached to this node.

Exceptions:

UnknownNodeIdException

Definition at line 230 of file pose_graph.cpp.

EdgeId pose_graph::PoseGraphImpl::shortestEdgeBetween	(	NodeId	n1,
		NodeId	n2
	)		const `[private]`

Return shortest edge between two nodes. Asserts if there's no edge between them.

Definition at line 439 of file pose_graph.cpp.

ShortestPathResult pose_graph::PoseGraphImpl::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 457 of file pose_graph.cpp.

PoseGraph pose_graph::PoseGraphImpl::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 544 of file pose_graph.cpp.