distance_field::DistanceField< T > Class Template Reference

A VoxelGrid that can convert a set of obstacle points into a distance field. More...

#include <distance_field.h>

Inheritance diagram for distance_field::DistanceField< T >:

Public Member Functions
void	addCollisionMapToField (const arm_navigation_msgs::CollisionMap &collision_map)
	Adds the points in a collision map to the distance field.
virtual void	addPointsToField (const std::vector< btVector3 > points)=0
	Add (and expand) a set of points to the distance field.
	DistanceField (double size_x, double size_y, double size_z, double resolution, double origin_x, double origin_y, double origin_z, T default_object)
	Constructor for the VoxelGrid.
double	getDistance (double x, double y, double z) const
	Gets the distance to the closest obstacle at the given location.
double	getDistanceFromCell (int x, int y, int z) const
	Gets the distance to the closest obstacle at the given integer cell location.
double	getDistanceGradient (double x, double y, double z, double &gradient_x, double &gradient_y, double &gradient_z) const
	Gets the distance at a location and the gradient of the field.
virtual void	reset ()=0
	Resets the distance field to the max_distance.
void	visualize (double min_radius, double max_radius, std::string frame_id, const btTransform &cur, ros::Time stamp)
	Publishes an iso-surface to rviz.
void	visualizeGradient (double min_radius, double max_radius, std::string frame_id, ros::Time stamp)
	Publishes the gradient to rviz.
void	visualizePlane (PlaneVisualizationType type, double length, double width, double height, btVector3 origin, std::string frame_id, ros::Time stamp)
	Publishes a set of markers to rviz along the specified plane.
virtual	~DistanceField ()
Protected Member Functions
virtual double	getDistance (const T &object) const =0
Private Attributes
int	inv_twice_resolution_
ros::Publisher	pub_viz_

Detailed Description

template<typename T>
class distance_field::DistanceField< T >

A VoxelGrid that can convert a set of obstacle points into a distance field.

It computes the distance transform of the input points, and stores the distance to the closest obstacle in each voxel. Also available is the location of the closest point, and the gradient of the field at a point. Expansion of obstacles is performed upto a given radius.

This is an abstract base class, current implementations include PropagationDistanceField and PFDistanceField.

Definition at line 71 of file distance_field.h.

Constructor & Destructor Documentation

template<typename T>

distance_field::DistanceField< T >::DistanceField	(	double	size_x,
		double	size_y,
		double	size_z,
		double	resolution,
		double	origin_x,
		double	origin_y,
		double	origin_z,
		T	default_object
	)			`[inline]`

Constructor for the VoxelGrid.

Parameters:

	size_x	Size (x axis) of the container in meters
	size_y	Size (y axis) of the container in meters
	size_z	Size (z axis) of the container in meters
	resolution,:	resolution (size of a single cell) in meters
	origin_x	Origin (x axis) of the container
	origin_y	Origin (y axis) of the container
	origin_z	Origin (z axis) of the container
	default_object	The object to return for an out-of-bounds query

Definition at line 171 of file distance_field.h.

template<typename T >

distance_field::DistanceField< T >::~DistanceField ( ) [inline, virtual]

Definition at line 165 of file distance_field.h.

Member Function Documentation

template<typename T >

void distance_field::DistanceField< T >::addCollisionMapToField ( const arm_navigation_msgs::CollisionMap & collision_map ) [inline]

Adds the points in a collision map to the distance field.

Definition at line 335 of file distance_field.h.

template<typename T>

virtual void distance_field::DistanceField< T >::addPointsToField ( const std::vector< btVector3 > points ) [pure virtual]

Add (and expand) a set of points to the distance field.

This function will incrementally add the given points and update the distance field correspondingly. Use the reset() function if you need to remove all points and start afresh.

Implemented in distance_field::PFDistanceField, distance_field::PropagationDistanceField, and distance_field::SignedPropagationDistanceField.

template<typename T>

virtual double distance_field::DistanceField< T >::getDistance ( const T & object ) const [protected, pure virtual]

Implemented in distance_field::PFDistanceField, distance_field::PropagationDistanceField, and distance_field::SignedPropagationDistanceField.

template<typename T >

double distance_field::DistanceField< T >::getDistance	(	double	x,
		double	y,
		double	z
	)			const `[inline]`

Gets the distance to the closest obstacle at the given location.

Definition at line 181 of file distance_field.h.

template<typename T >

double distance_field::DistanceField< T >::getDistanceFromCell	(	int	x,
		int	y,
		int	z
	)			const `[inline]`

Gets the distance to the closest obstacle at the given integer cell location.

Definition at line 212 of file distance_field.h.

template<typename T >

double distance_field::DistanceField< T >::getDistanceGradient	(	double	x,
		double	y,
		double	z,
		double &	gradient_x,
		double &	gradient_y,
		double &	gradient_z
	)			const `[inline]`

Gets the distance at a location and the gradient of the field.

Definition at line 187 of file distance_field.h.

template<typename T>

virtual void distance_field::DistanceField< T >::reset ( ) [pure virtual]

Resets the distance field to the max_distance.

Implemented in distance_field::PFDistanceField, distance_field::PropagationDistanceField, and distance_field::SignedPropagationDistanceField.

template<typename T >

void distance_field::DistanceField< T >::visualize	(	double	min_radius,
		double	max_radius,
		std::string	frame_id,
		const btTransform &	cur,
		ros::Time	stamp
	)			`[inline]`

Publishes an iso-surface to rviz.

Publishes an iso-surface containing points between min_radius and max_radius as visualization markers for rviz.

Definition at line 218 of file distance_field.h.

template<typename T >

void distance_field::DistanceField< T >::visualizeGradient	(	double	min_radius,
		double	max_radius,
		std::string	frame_id,
		ros::Time	stamp
	)			`[inline]`

Publishes the gradient to rviz.

Publishes the gradient of the distance field as visualization markers for rviz.

Definition at line 270 of file distance_field.h.

template<typename T >

void distance_field::DistanceField< T >::visualizePlane	(	distance_field::PlaneVisualizationType	type,
		double	length,
		double	width,
		double	height,
		btVector3	origin,
		std::string	frame_id,
		ros::Time	stamp
	)			`[inline]`

Publishes a set of markers to rviz along the specified plane.

Parameters:

	type	the plane to publish (XZ, XY, YZ)
	length	the size along the first axis to publish in meters.
	width	the size along the second axis to publish in meters.
	height	the position along the orthogonal axis to the plane, in meters.