Graph models static transformations between coordinate frames. More...

#include <graph.h>

Public Member Functions
void	Add (const std::string &name, const RefFrame &ref_frame_name, const Transform &transform)
	Adds or edits a transform (an edge) to the graph.
bool	CanTransform (const std::string &frame1_id, const std::string &frame2_id) const
	Returns whether an undirected path exists between two frames.
bool	ComputeDescription (const LocalFrame &local, const RefFrame &reference, Transform *transform) const
	Computes the transform describing the local frame relative to the reference frame.
bool	ComputeMapping (const From &from, const To &to, Transform *transform) const
	Computes the transform that maps points in the "from" frame to the "to" frame.
bool	DescribePose (const Transform &in, const Source &source_frame, const Target &target_frame, Transform *out) const
	Describe a pose in a new frame.
bool	DescribePosition (const Position &in, const Source &source_frame, const Target &target_frame, Position *out) const
	Describes a position in a new frame.
	Graph ()
	Default constructor.
bool	MapPose (const Transform &in, const From &frame_in, const To &frame_out, Transform *out) const
	Map a pose into a frame.
bool	MapPosition (const Position &in, const From &frame_in, const To &frame_out, Position *out) const
	Maps a position into a frame.
Private Member Functions
bool	GetTransform (const std::string &source, const std::string &target, Eigen::Affine3d *out) const
Private Attributes
internal::Graph	graph_
std::map< std::pair < std::string, std::string > , Transform >	transforms_

Detailed Description

Graph models static transformations between coordinate frames.

The vertices are coordinate frame IDs, given as strings. The edges are directed and represent transformations between coordinate frames.

Once the graph is specified, this class can be used to compute the transformation between any two coordinate frames, as long as a weakly connected path exists between them.

The graph may contain cycles. If it does, our shortest path algorithm will find a path with the minimum number of edges.

The graph may be disconnected, in which case Graph may be used to find transformations between two frames in the same connected component.

Example:

   transform_graph::Graph graph;
   graph.Add("head_mount_kinect", transform_graph::RefFrame("base_link"),
   head_pose);
   graph.Add("wrist", transform_graph::RefFrame("base_link"), wrist_pose);
   for (PointXYZ pt : cloud.points) {
     transform_graph::Transform pt_in_wrist;
     graph.DescribePosition(pt, transform_graph::Source("head_mount_kinect"),
                            transform_graph::Target("wrist"), pt_in_wrist);
     // pt_in_base is in wrist frame, do something with it
   }

In the example above, it may be more efficient to cache the transform between head_mount_kinect and wrist:

   transform_graph::Transform kinect_in_wrist;
   graph.ComputeDescription("head_mount_kinect",
                            transform_graph::RefFrame("wrist"),
                            &kinect_in_wrist);
   graph.Add("head_mount_kinect", transform_graph::RefFrame("wrist"),
             kinect_in_wrist);

Definition at line 56 of file graph.h.

Constructor & Destructor Documentation

transform_graph::Graph::Graph ( )

Default constructor.

Definition at line 20 of file graph.cpp.

Member Function Documentation

void transform_graph::Graph::Add	(	const std::string &	name,
		const RefFrame &	ref_frame_name,
		const Transform &	transform
	)

Adds or edits a transform (an edge) to the graph.

If the transform (or its inverse) is already in the graph, this will replace it.

You should only input transformations consisting of rotations and translations. Graph does not check if the matrix is well-formed or invertible.

For the third parameter, you do not necessarily need to supply an transform_graph::Transform type. Many common transformation types will be implicitly converted, see transform.h to see a list.

Parameters:

[in]	name	The name of the frame being added to the graph.
[in]	ref_frame_name	The name of the reference frame that this transform is expressed in.
[in]	transform	The transform to add.

Example: add a tooltip that is 10cm in front of the wrist, same direction as the wrist

   geometry_msgs::Pose pose;
   pose.position.x = 0.1;
   pose.orientation.w = 1;
   graph.Add("tooltip", transform_graph::RefFrame("wrist"), pose);

Definition at line 22 of file graph.cpp.

bool transform_graph::Graph::CanTransform	(	const std::string &	frame1_id,
		const std::string &	frame2_id
	)		const

Returns whether an undirected path exists between two frames.

Returns:: true if an undirected path exists between the given frames, false otherwise.

Definition at line 28 of file graph.cpp.

bool transform_graph::Graph::ComputeDescription	(	const LocalFrame &	local,
		const RefFrame &	reference,
		Transform *	transform
	)		const

Computes the transform describing the local frame relative to the reference frame.

Multiplying this transform with a point expressed in the local frame results in the same point expressed in the reference frame. The position component of the returned homogeneous matrix is the origin of the local frame expressed in the reference frame. The columns of the rotation matrix are the principal axes of the local frame expressed in the reference frame.

Parameters:

[in]	local	The frame whose description you want.
[in]	reference	The reference frame you want the frame described in.
[out]	transform	The output transform. Call matrix() to get the homogeneous matrix as an Eigen::Matrix4d.

Returns:: true if the transform exists, false otherwise.

Example: describe the wrist frame relative to the base frame

   graph.ComputeDescription("wrist",
                            transform_graph::RefFrame("base"),
                            &wrist_in_base);

The origin of the wrist in the base frame is:

   wrist_in_base.matrix().topRightCorner(3, 1)

The columns of the rotation matrix are the x, y, and z axes of the wrist frame, expressed in the frame of the base:

   wrist_in_base.matrix().topLeftCorner(3, 3)

Note:: If you are just going to transform one or two points, using DescribePosition is more efficient. Use this method to compute the transform just once if you are going to transform many points.

Definition at line 35 of file graph.cpp.

bool transform_graph::Graph::ComputeMapping	(	const From &	from,
		const To &	to,
		Transform *	transform
	)		const

Computes the transform that maps points in the "from" frame to the "to" frame.

To do the mapping, multiply the point with this transform. You can think of this multiplication as applying the same rotation and translation to get from the "from" frame to the "to" frame.

Parameters:

[in]	from	The frame you want to map points from.
[in]	to	The frame you want to map points into.
[out]	transform	The output transform. Call matrix() to get the homogeneous matrix as an Eigen::Matrix4d.

Returns:: true if the transform exists, false otherwise.

Example: get the mapping from left hand to right hand

   graph.Add("left hand", transform_graph::RefFrame("base"), ...);
   graph.Add("right hand", transform_graph::RefFrame("base"), ...);
   graph.ComputeMapping(transform_graph::From("left hand"),
                        transform_graph::To("right hand"), &left_to_right);

Now multiplying left_to_right by the point (0.1, 0, 0) in front of the left hand results in a point 10 cm in front of the right hand. However, this point is still expressed in the frame of the left hand.

Note:: If you are just going to map one or two points, using MapPosition is more efficient. Use this method to compute the transform just once if you are going to transform many points.

Definition at line 59 of file graph.cpp.

bool transform_graph::Graph::DescribePose	(	const Transform &	in,
		const Source &	source_frame,
		const Target &	target_frame,
		Transform *	out
	)		const

Describe a pose in a new frame.

This is similar to simply adding a new frame to the graph and calling ComputeDescription. See DescribePosition for an example that doesn't involve orientation.

Parameters:

[in]	in	The transform (e.g., a Pose) you want to describe in another frame.
[in]	source_frame	The name of the frame that the given pose is currently expressed in.
[in]	target_frame	The name of the frame that you would like the pose to be described in.
[out]	out	The transform that describes the given transform in the target reference frame.

Returns:: true if the transform could be computed, false otherwise.

Example: get the frame 10 cm in front of the wrist in base coordinates

   Pose p;
   p.position.x = 0.1;
   p.orientation.w = 1;
   transform_graph::Transform pose_in_base
   graph.DescribePose(p, transform_graph::Source("wrist"),
                      transform_graph::Target("base"), &pose_in_base);

Note:: Calling this method computes the entire chain of transformations, so if you are just going to transform many poses, compute the transform once using ComputeDescription.

Definition at line 64 of file graph.cpp.

bool transform_graph::Graph::DescribePosition	(	const Position &	in,
		const Source &	source_frame,
		const Target &	target_frame,
		Position *	out
	)		const

Describes a position in a new frame.

Parameters:

[in]	in	The position you want to describe in another frame.
[in]	source_frame	The frame the position is currently in.
[in]	target_frame	The frame you want the position to be in.
[out]	out	The given position, expressed in the target frame.

Returns:: true if the transform could be computed, false otherwise.

Example: get the position 10 cm in front of the wrist in the base frame

   PointStamped ps;
   ps.header.frame_id = "wrist";
   ps.pose.position.x = 0.1;
   graph.DescribePosition(ps.point, transform_graph::Source("wrist"),
                          transform_graph::Target("base"), &ps_base)

Note:: Calling this method computes the entire chain of transformations, so if you are just going to transform many points, compute the transform once using ComputeDescription.

Definition at line 93 of file graph.cpp.

bool transform_graph::Graph::GetTransform	(	const std::string &	source,
		const std::string &	target,
		Eigen::Affine3d *	out
	)		const `[private]`

Definition at line 124 of file graph.cpp.

bool transform_graph::Graph::MapPose	(	const Transform &	in,
		const From &	frame_in,
		const To &	frame_out,
		Transform *	out
	)		const

Map a pose into a frame.

Given a pose, this computes what that pose would be in the "to" frame. The result is expressed, however, in the "from" frame. See MapPosition for an example that doesn't involve orientation.

Parameters:

[in]	in	The pose you want to map to a different frame.
[in]	frame_in	The frame the pose is currently expressed in.
[in]	frame_out	The frame you want to map the pose into.
[out]	out	The output pose, expressed in frame_in.

Returns:: true if the transform could be computed, false otherwise.

Note:: Calling this method computes the entire chain of transformations, so if you are just going to transform many poses, compute the transform once using ComputeMapping.

Definition at line 88 of file graph.cpp.

bool transform_graph::Graph::MapPosition	(	const Position &	in,
		const From &	frame_in,
		const To &	frame_out,
		Position *	out
	)		const

Maps a position into a frame.

Given a position like (1, 0, 0), this computes what (1, 0, 0) is in the "to" frame, and expresses the result in the "from" frame. For example, if left_hand is at (0, 0.3, 0) and right_hand is at (0, -0.3, 0) and they have the same orientation, then:

   geometry_msgs::Point pt;
   pt.x = 1; pt.y = 0; pt.z = 0;
   geometry_msgs::Point pt_out;
   MapPosition(pt, transform_graph::From("left_hand"),
               transform_graph::To("right_hand"), &pt_out);

pt_out will be (1, -0.6, 0);

Parameters:

[in]	in	The position you want to map into another frame.
[in]	frame_in	The frame the position is currently in.
[in]	frame_out	The frame you want to map the position into.
[out]	out	The given position, but now sitting in the "from" frame, but numerically expressed in the "from" frame.

Returns:: true if the transform could be computed, false otherwise.

Note:: Calling this method computes the entire chain of transformations, so if you are just going to map many points, compute the transform once using ComputeMapping.

Definition at line 118 of file graph.cpp.

Member Data Documentation

internal::Graph transform_graph::Graph::graph_ [private]

Definition at line 288 of file graph.h.

std::map<std::pair<std::string, std::string>, Transform> transform_graph::Graph::transforms_ [private]

Definition at line 287 of file graph.h.

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

Public Member Functions

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation