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. More...

std::string	getPCDFile ()
	Return the *.pcd file given by the corresponding parameter in the ROS launch file. More...

double	getTargetRadius ()
	Return the target radius. More...

void	initParams (ros::NodeHandle node)
	Read the parameters from a ROS launch file. More...

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. More...

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. More...

std::vector< CylindricalShell >	searchAffordances (const PointCloud::Ptr &cloud, tf::StampedTransform *transform=NULL)
	Search grasp affordances (cylindrical shells) in a given point cloud. More...

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. More...

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. More...

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. More...

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. More...

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. More...

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. More...

void	estimateNormals (const PointCloud::Ptr &cloud, const pcl::PointCloud< pcl::Normal >::Ptr &cloud_normals)
	Estimate surface normals for each point in the point cloud. More...

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. More...

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. More...

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. More...

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. More...

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

cloud	the point cloud in which the handles lie (only required for occlusion filtering)
shells	the set of cylindrical shells to be searched for handles

Definition at line 534 of file affordances.cpp.

PointCloud::Ptr Affordances::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.

Parameters

cloud_in the point cloud to be filtered

Definition at line 117 of file affordances.cpp.

Member Data Documentation

double Affordances::alignment_dist_radius

private

Definition at line 233 of file affordances.h.

const double Affordances::ALIGNMENT_DIST_RADIUS = 0.02

staticprivate

Definition at line 254 of file affordances.h.

int Affordances::alignment_min_inliers

private

Definition at line 232 of file affordances.h.

const int Affordances::ALIGNMENT_MIN_INLIERS = 10

staticprivate

Definition at line 253 of file affordances.h.

double Affordances::alignment_orient_radius

private

Definition at line 234 of file affordances.h.

const double Affordances::ALIGNMENT_ORIENT_RADIUS = 0.1

staticprivate

Definition at line 255 of file affordances.h.

double Affordances::alignment_radius_radius

private

Definition at line 235 of file affordances.h.

const double Affordances::ALIGNMENT_RADIUS_RADIUS = 0.003

staticprivate

Definition at line 256 of file affordances.h.

int Affordances::alignment_runs

private

Definition at line 231 of file affordances.h.

const int Affordances::ALIGNMENT_RUNS = 3

staticprivate

Definition at line 252 of file affordances.h.

int Affordances::curvature_estimator

private

Definition at line 230 of file affordances.h.

const int Affordances::CURVATURE_ESTIMATOR = 0

staticprivate

Definition at line 241 of file affordances.h.

std::string Affordances::file

private

Definition at line 238 of file affordances.h.

double Affordances::handle_gap

private

Definition at line 225 of file affordances.h.

const double Affordances::HANDLE_GAP = 0.08

staticprivate

Definition at line 248 of file affordances.h.

const int Affordances::MAX_NUM_IN_FRONT = 20

staticprivate

Definition at line 245 of file affordances.h.

double Affordances::max_range

private

Definition at line 227 of file affordances.h.

const double Affordances::MAX_RANGE = 1.0

staticprivate

Definition at line 249 of file affordances.h.

const double Affordances::NEIGHBOR_RADIUS = 0.025

staticprivate

Definition at line 244 of file affordances.h.

const int Affordances::NUM_NEAREST_NEIGHBORS = 500

staticprivate

Definition at line 243 of file affordances.h.

int Affordances::num_samples

private

Definition at line 226 of file affordances.h.

const int Affordances::NUM_SAMPLES = 5000

staticprivate

Definition at line 242 of file affordances.h.

int Affordances::num_threads

private

Definition at line 237 of file affordances.h.

double Affordances::radius_error

private

Definition at line 224 of file affordances.h.

const double Affordances::RADIUS_ERROR = 0.013

staticprivate

Definition at line 247 of file affordances.h.

double Affordances::target_radius

private

Definition at line 223 of file affordances.h.

const double Affordances::TARGET_RADIUS = 0.08

staticprivate

Definition at line 246 of file affordances.h.

bool Affordances::use_clearance_filter

private

Definition at line 228 of file affordances.h.

const bool Affordances::USE_CLEARANCE_FILTER = true

staticprivate

Definition at line 250 of file affordances.h.

bool Affordances::use_occlusion_filter

private

Definition at line 229 of file affordances.h.

const bool Affordances::USE_OCCLUSION_FILTER = true

staticprivate

Definition at line 251 of file affordances.h.

WorkspaceLimits Affordances::workspace_limits

private

Definition at line 236 of file affordances.h.

const double Affordances::WORKSPACE_MAX = 1.0

staticprivate

Definition at line 258 of file affordances.h.

const double Affordances::WORKSPACE_MIN = -1.0

staticprivate

Definition at line 257 of file affordances.h.

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

Public Member Functions

Private Member Functions

Private Attributes

Static Private Attributes

Detailed Description

Member Function Documentation

Member Data Documentation