Affordances localizes grasp affordances and handles in a point cloud. It also provides helper methods to filter out points from the point cloud that are outside of the robot's workspace. More...

#include <affordances.h>

Public Member Functions
std::vector< int >	createRandomIndices (const PointCloud::Ptr &cloud, int size)
int	getNumSamples ()
	Return the number of samples, i.e., the number of neighborhoods to be searched for.
std::string	getPCDFile ()
	Return the *.pcd file given by the corresponding parameter in the ROS launch file.
double	getTargetRadius ()
	Return the target radius.
void	initParams (ros::NodeHandle node)
	Read the parameters from a ROS launch file.
bool	isPointInWorkspace (double x, double y, double z, tf::StampedTransform *transform=NULL)
	Check whether a given point, using its x, y, and z coordinates, is within the workspace of the robot.
PointCloud::Ptr	maxRangeFilter (const PointCloud::Ptr &cloud_in)
	Filter out all points from a given cloud that are outside of a sphere with a radius max_range and center at the origin of the point cloud, and return the filtered cloud.
std::vector< CylindricalShell >	searchAffordances (const PointCloud::Ptr &cloud, tf::StampedTransform *transform=NULL)
	Search grasp affordances (cylindrical shells) in a given point cloud.
std::vector< CylindricalShell >	searchAffordances (const PointCloud::Ptr &cloud, const std::vector< int > &indices)
	Search grasp affordances (cylindrical shells) using a set of indices in a given point cloud.
std::vector< CylindricalShell >	searchAffordancesTaubin (const PointCloud::Ptr &cloud, const Eigen::MatrixXd &samples, bool is_logging=true)
	Search grasp affordances (cylindrical shells) using a set of samples in a given point cloud. This function uses Taubin Quadric Fitting.
std::vector< std::vector < CylindricalShell > >	searchHandles (const PointCloud::Ptr &cloud, std::vector< CylindricalShell > shells)
	Search handles in a set of cylindrical shells. If occlusion filtering is turned on (using the corresponding parameter in the ROS launch file), the handles found are filtered on possible occlusions.
PointCloud::Ptr	workspaceFilter (const PointCloud::Ptr &cloud_in, tf::StampedTransform *transform=NULL)
	Filter out all points from a given cloud that are outside of a cube defined by the workspace limits of the robot, and return the filtered cloud.
Private Member Functions
void	estimateCurvatureAxisNormals (const pcl::PointCloud< pcl::Normal >::Ptr &cloud_normals, const std::vector< int > &nn_indices, Eigen::Vector3d &axis, Eigen::Vector3d &normal)
	Estimate the cylinder's curvature axis and normal using surface normals.
void	estimateCurvatureAxisPCA (const PointCloud::Ptr &cloud, int nn_center_idx, std::vector< int > nn_indices, Eigen::Vector3d &axis, Eigen::Vector3d &normal)
	Estimate the cylinder's curvature axis and normal using PCA.
void	estimateNormals (const PointCloud::Ptr &cloud, const pcl::PointCloud< pcl::Normal >::Ptr &cloud_normals)
	Estimate surface normals for each point in the point cloud.
void	findBestColinearSet (const std::vector< CylindricalShell > &list, std::vector< int > &inliersMaxSet, std::vector< int > &outliersMaxSet)
	Find the best (largest number of inliers) set of colinear cylindrical shells given a list of shells.
int	numInFront (const PointCloud::Ptr &cloud, int center_index, double radius)
	Find the number of points in the point cloud that lie in front of a point neighborhood with a given centroid index. A sphere with a given radius is searched for these points.
std::vector< CylindricalShell >	searchAffordancesNormalsOrPCA (const PointCloud::Ptr &cloud, tf::StampedTransform *transform=NULL)
	Search grasp affordances (cylindrical shells) in a given point cloud using surface normals.
std::vector< CylindricalShell >	searchAffordancesTaubin (const PointCloud::Ptr &cloud, tf::StampedTransform *transform=NULL)
	Search grasp affordances (cylindrical shells) in a given point cloud using Taubin Quadric Fitting.
Private Attributes
double	alignment_dist_radius
int	alignment_min_inliers
double	alignment_orient_radius
double	alignment_radius_radius
int	alignment_runs
int	curvature_estimator
std::string	file
double	handle_gap
double	max_range
int	num_samples
int	num_threads
double	radius_error
double	target_radius
bool	use_clearance_filter
bool	use_occlusion_filter
WorkspaceLimits	workspace_limits
Static Private Attributes
static const double	ALIGNMENT_DIST_RADIUS = 0.02
static const int	ALIGNMENT_MIN_INLIERS = 10
static const double	ALIGNMENT_ORIENT_RADIUS = 0.1
static const double	ALIGNMENT_RADIUS_RADIUS = 0.003
static const int	ALIGNMENT_RUNS = 3
static const int	CURVATURE_ESTIMATOR = 0
static const double	HANDLE_GAP = 0.08
static const int	MAX_NUM_IN_FRONT = 20
static const double	MAX_RANGE = 1.0
static const double	NEIGHBOR_RADIUS = 0.025
static const int	NUM_NEAREST_NEIGHBORS = 500
static const int	NUM_SAMPLES = 5000
static const double	RADIUS_ERROR = 0.013
static const double	TARGET_RADIUS = 0.08
static const bool	USE_CLEARANCE_FILTER = true
static const bool	USE_OCCLUSION_FILTER = true
static const double	WORKSPACE_MAX = 1.0
static const double	WORKSPACE_MIN = -1.0

Detailed Description

Affordances localizes grasp affordances and handles in a point cloud. It also provides helper methods to filter out points from the point cloud that are outside of the robot's workspace.

Author:: Andreas ten Pas

Definition at line 73 of file affordances.h.

Member Function Documentation

std::vector< int > Affordances::createRandomIndices	(	const PointCloud::Ptr &	cloud,
		int	size
	)

Definition at line 756 of file affordances.cpp.

void Affordances::estimateCurvatureAxisNormals	(	const pcl::PointCloud< pcl::Normal >::Ptr &	cloud_normals,
		const std::vector< int > &	nn_indices,
		Eigen::Vector3d &	axis,
		Eigen::Vector3d &	normal
	)		`[private]`

Estimate the cylinder's curvature axis and normal using surface normals.

Parameters:

cloud	the point cloud in which the cylinder lies
nn_indices	the point cloud indices of the neighborhood
axis	the resultant curvature axis of the cylinder
normal	the resultant normal of the cylinder

Definition at line 178 of file affordances.cpp.

void Affordances::estimateCurvatureAxisPCA	(	const PointCloud::Ptr &	cloud,
		int	nn_center_idx,
		std::vector< int >	nn_indices,
		Eigen::Vector3d &	axis,
		Eigen::Vector3d &	normal
	)		`[private]`

Estimate the cylinder's curvature axis and normal using PCA.

Parameters:

cloud	the point cloud in which the cylinder lies
nn_center_idx	the index of the centroid of the neighborhood associated with the cylinder
nn_indices	the point cloud indices of the neighborhood
axis	the resultant curvature axis of the cylinder
normal	the resultant normal of the cylinder

Definition at line 158 of file affordances.cpp.

void Affordances::estimateNormals	(	const PointCloud::Ptr &	cloud,
		const pcl::PointCloud< pcl::Normal >::Ptr &	cloud_normals
	)		`[private]`

Estimate surface normals for each point in the point cloud.

Parameters:

cloud	the point cloud for which surface normals are estimated
cloud_normals	the resultant point cloud that contains the surface normals

Definition at line 204 of file affordances.cpp.

void Affordances::findBestColinearSet	(	const std::vector< CylindricalShell > &	list,
		std::vector< int > &	inliersMaxSet,
		std::vector< int > &	outliersMaxSet
	)		`[private]`

Find the best (largest number of inliers) set of colinear cylindrical shells given a list of shells.

Parameters:

list	the list of cylindrical shells
inliersMaxSet	the largest set of inliers found (the best colinear set)
outliersMaxSet	the largest set of outliers found (the remaining shells that are not in the best colinear set)

Definition at line 772 of file affordances.cpp.

int Affordances::getNumSamples ( ) [inline]

Return the number of samples, i.e., the number of neighborhoods to be searched for.

Definition at line 146 of file affordances.h.

std::string Affordances::getPCDFile ( ) [inline]

Return the *.pcd file given by the corresponding parameter in the ROS launch file.

Definition at line 142 of file affordances.h.

double Affordances::getTargetRadius ( ) [inline]

Return the target radius.

Definition at line 150 of file affordances.h.

void Affordances::initParams ( ros::NodeHandle node )

Read the parameters from a ROS launch file.

Parameters:

node	the ROS node with which the parameters are associated

Definition at line 30 of file affordances.cpp.

bool Affordances::isPointInWorkspace	(	double	x,
		double	y,
		double	z,
		tf::StampedTransform *	transform = `NULL`
	)

Check whether a given point, using its x, y, and z coordinates, is within the workspace of the robot.

Parameters:

x	the x coordinate of the point
y	the y coordinate of the point
z	the z coordinate of the point

Definition at line 95 of file affordances.cpp.

PointCloud::Ptr Affordances::maxRangeFilter ( const PointCloud::Ptr & cloud_in )

Filter out all points from a given cloud that are outside of a sphere with a radius max_range and center at the origin of the point cloud, and return the filtered cloud.

Parameters:

cloud_in the point cloud to be filtered

Definition at line 81 of file affordances.cpp.

int Affordances::numInFront	(	const PointCloud::Ptr &	cloud,
		int	center_index,
		double	radius
	)		`[private]`

Find the number of points in the point cloud that lie in front of a point neighborhood with a given centroid index. A sphere with a given radius is searched for these points.

Parameters:

cloud	the point cloud
center_index	the index of the neighborhood centroid in the point cloud
radius	the radius of the sphere

Definition at line 131 of file affordances.cpp.

std::vector< CylindricalShell > Affordances::searchAffordances	(	const PointCloud::Ptr &	cloud,
		tf::StampedTransform *	transform = `NULL`
	)

Search grasp affordances (cylindrical shells) in a given point cloud.

Parameters:

cloud the point cloud in which affordances are searched for

Definition at line 214 of file affordances.cpp.

std::vector< CylindricalShell > Affordances::searchAffordances	(	const PointCloud::Ptr &	cloud,
		const std::vector< int > &	indices
	)

Search grasp affordances (cylindrical shells) using a set of indices in a given point cloud.

Parameters:

cloud the point cloud in which affordances are searched for indices the point cloud indices at which affordances are searched for

Definition at line 613 of file affordances.cpp.

std::vector< CylindricalShell > Affordances::searchAffordancesNormalsOrPCA	(	const PointCloud::Ptr &	cloud,
		tf::StampedTransform *	transform = `NULL`
	)		`[private]`

Search grasp affordances (cylindrical shells) in a given point cloud using surface normals.

Parameters:

cloud the point cloud in which affordances are searched for

Definition at line 230 of file affordances.cpp.

std::vector< CylindricalShell > Affordances::searchAffordancesTaubin	(	const PointCloud::Ptr &	cloud,
		const Eigen::MatrixXd &	samples,
		bool	is_logging = `true`
	)

Search grasp affordances (cylindrical shells) using a set of samples in a given point cloud. This function uses Taubin Quadric Fitting.

Parameters:

cloud	the point cloud in which affordances are searched for
samples	a 3xn matrix of points sampled from the point cloud

Definition at line 623 of file affordances.cpp.

std::vector< CylindricalShell > Affordances::searchAffordancesTaubin	(	const PointCloud::Ptr &	cloud,
		tf::StampedTransform *	transform = `NULL`
	)		`[private]`

Search grasp affordances (cylindrical shells) in a given point cloud using Taubin Quadric Fitting.

Parameters:

cloud the point cloud in which affordances are searched for

Definition at line 390 of file affordances.cpp.

std::vector< std::vector< CylindricalShell > > Affordances::searchHandles	(	const PointCloud::Ptr &	cloud,
		std::vector< CylindricalShell >	shells
	)

Search handles in a set of cylindrical shells. If occlusion filtering is turned on (using the corresponding parameter in the ROS launch file), the handles found are filtered on possible occlusions.

Parameters: