IncGroundRemoval Class Reference

List of all members.

Public Member Functions
void	cloud_cb (const sensor_msgs::PointCloudConstPtr &msg)
void	cloud_cb (const sensor_msgs::PointCloudConstPtr &msg)
void	computeCentroid (const sensor_msgs::PointCloud &points, const std::vector< int > &indices, geometry_msgs::Point32 &centroid)
	Compute the centroid of a set of points using their indices and return it as a Point32 message.
void	computeCovarianceMatrix (const sensor_msgs::PointCloud &points, const std::vector< int > &indices, Eigen::Matrix3d &covariance_matrix, geometry_msgs::Point32 &centroid)
	Compute the 3x3 covariance matrix of a given set of points using their indices. The result is returned as a Eigen::Matrix3d.
void	computePointNormal (const sensor_msgs::PointCloud &points, const std::vector< int > &indices, Eigen::Vector4d &plane_parameters, double &curvature)
	Compute the Least-Squares plane fit for a given set of points, using their indices, and return the estimated plane parameters together with the surface curvature.
bool	fitSACLine (sensor_msgs::PointCloud points, vector< int > indices, vector< int > &inliers)
	Find a line model in a point cloud given via a set of point indices with SAmple Consensus methods.
bool	fitSACLine (sensor_msgs::PointCloud points, vector< int > indices, vector< int > &inliers)
	Find a line model in a point cloud given via a set of point indices with SAmple Consensus methods.
void	flipNormalTowardsViewpoint (Eigen::Vector4d &normal, const geometry_msgs::Point32 &point, const geometry_msgs::PointStamped &viewpoint)
	Flip (in place) the estimated normal of a point towards a given viewpoint.
std::string	getAvailableChannels (const sensor_msgs::PointCloud &cloud)
	Get the available dimensions as a space separated string.
int	getChannelIndex (const sensor_msgs::PointCloud &points, std::string channel_name)
	Get the index of a specified dimension/channel in a point cloud.
void	getCloudViewPoint (string cloud_frame, geometry_msgs::PointStamped &viewpoint_cloud, tf::TransformListener *tf)
	Get the view point from where the scans were taken in the incoming PointCloud message frame.
void	getCloudViewPoint (string cloud_frame, geometry_msgs::PointStamped &viewpoint_cloud, tf::TransformListener *tf)
	Get the view point from where the scans were taken in the incoming PointCloud message frame.
	IncGroundRemoval (ros::NodeHandle &anode)
	IncGroundRemoval (ros::NodeHandle &anode)
double	pointToPlaneDistanceSigned (const geometry_msgs::Point32 &p, const Eigen::Vector4d &plane_coefficients)
	Get the distance from a point to a plane (signed) defined by ax+by+cz+d=0.
void	splitPointsBasedOnLaserScanIndex (sensor_msgs::PointCloud points, vector< int > indices, vector< vector< int > > &clusters, int idx)
	Decompose a PointCloud message into LaserScan clusters.
void	splitPointsBasedOnLaserScanIndex (sensor_msgs::PointCloud points, vector< int > indices, vector< vector< int > > &clusters, int idx)
	Decompose a PointCloud message into LaserScan clusters.
double	transformDoubleValueTF (double val, std::string src_frame, std::string tgt_frame, ros::Time stamp, tf::TransformListener *tf)
	Transform a value from a source frame to a target frame at a certain moment in time with TF.
double	transformDoubleValueTF (double val, std::string src_frame, std::string tgt_frame, ros::Time stamp, tf::TransformListener *tf)
	Transform a value from a source frame to a target frame at a certain moment in time with TF.
void	transformPoint (tf::TransformListener *tf, const std::string &target_frame, const tf::Stamped< geometry_msgs::Point32 > &stamped_in, tf::Stamped< geometry_msgs::Point32 > &stamped_out)
	Transform a given point from its current frame to a given target frame.
void	transformPoint (tf::TransformListener *tf, const std::string &target_frame, const tf::Stamped< geometry_msgs::Point32 > &stamped_in, tf::Stamped< geometry_msgs::Point32 > &stamped_out)
	Transform a given point from its current frame to a given target frame.
void	updateParametersFromServer ()
void	updateParametersFromServer ()
virtual	~IncGroundRemoval ()
virtual	~IncGroundRemoval ()
Public Attributes
sensor_msgs::PointCloud	cloud_
sensor_msgs::PointCloud	cloud_noground_
tf::MessageFilter < sensor_msgs::PointCloud > *	cloud_notifier_
ros::Publisher	cloud_publisher_
message_filters::Subscriber < sensor_msgs::PointCloud > *	cloud_subscriber_
double	ground_slope_threshold_
sensor_msgs::PointCloud	laser_cloud_
std::string	laser_tilt_mount_frame_
int	planar_refine_
std::string	robot_footprint_frame_
double	sac_distance_threshold_
double	sac_fitting_distance_threshold_
int	sac_max_iterations_
int	sac_min_points_per_model_
tf::TransformListener	tf_
geometry_msgs::PointStamped	viewpoint_cloud_
double	z_threshold_
Protected Attributes
ros::NodeHandle &	node_

Detailed Description

Definition at line 72 of file sac_inc_ground_removal.cpp.

Constructor & Destructor Documentation

IncGroundRemoval::IncGroundRemoval ( ros::NodeHandle & anode ) [inline]

Definition at line 98 of file sac_inc_ground_removal.cpp.

virtual IncGroundRemoval::~IncGroundRemoval ( ) [inline, virtual]

Definition at line 141 of file sac_inc_ground_removal.cpp.

IncGroundRemoval::IncGroundRemoval ( ros::NodeHandle & anode ) [inline]

Definition at line 88 of file sac_inc_ground_removal_standalone.cpp.

virtual IncGroundRemoval::~IncGroundRemoval ( ) [inline, virtual]

Definition at line 131 of file sac_inc_ground_removal_standalone.cpp.

Member Function Documentation

void IncGroundRemoval::cloud_cb ( const sensor_msgs::PointCloudConstPtr & msg ) [inline]

Definition at line 393 of file sac_inc_ground_removal_standalone.cpp.

void IncGroundRemoval::cloud_cb ( const sensor_msgs::PointCloudConstPtr & msg ) [inline]

Definition at line 244 of file sac_inc_ground_removal.cpp.

void IncGroundRemoval::computeCentroid	(	const sensor_msgs::PointCloud &	points,
		const std::vector< int > &	indices,
		geometry_msgs::Point32 &	centroid
	)			`[inline]`

Compute the centroid of a set of points using their indices and return it as a Point32 message.

Parameters:

	points	the input point cloud
	indices	the point cloud indices that need to be used
	centroid	the output centroid

Definition at line 267 of file sac_inc_ground_removal_standalone.cpp.

void IncGroundRemoval::computeCovarianceMatrix	(	const sensor_msgs::PointCloud &	points,
		const std::vector< int > &	indices,
		Eigen::Matrix3d &	covariance_matrix,
		geometry_msgs::Point32 &	centroid
	)			`[inline]`

Compute the 3x3 covariance matrix of a given set of points using their indices. The result is returned as a Eigen::Matrix3d.

Note:: The (x-y-z) centroid is also returned as a Point32 message.

Parameters:

	points	the input point cloud
	indices	the point cloud indices that need to be used
	covariance_matrix	the 3x3 covariance matrix
	centroid	the computed centroid

Definition at line 293 of file sac_inc_ground_removal_standalone.cpp.

void IncGroundRemoval::computePointNormal	(	const sensor_msgs::PointCloud &	points,
		const std::vector< int > &	indices,
		Eigen::Vector4d &	plane_parameters,
		double &	curvature
	)			`[inline]`

Compute the Least-Squares plane fit for a given set of points, using their indices, and return the estimated plane parameters together with the surface curvature.

Parameters:

	points	the input point cloud
	indices	the point cloud indices that need to be used
	plane_parameters	the plane parameters as: a, b, c, d (ax + by + cz + d = 0)
	curvature	the estimated surface curvature as a measure of $\lambda_0 / (\lambda_0 + \lambda_1 + \lambda_2)$

Definition at line 328 of file sac_inc_ground_removal_standalone.cpp.

bool IncGroundRemoval::fitSACLine	(	sensor_msgs::PointCloud *	points,
		vector< int > *	indices,
		vector< int > &	inliers
	)			`[inline]`

Find a line model in a point cloud given via a set of point indices with SAmple Consensus methods.

Parameters:

	points	the point cloud message
	indices	a pointer to a set of point cloud indices to test
	inliers	the resultant inliers

Definition at line 195 of file sac_inc_ground_removal_standalone.cpp.

bool IncGroundRemoval::fitSACLine	(	sensor_msgs::PointCloud *	points,
		vector< int > *	indices,
		vector< int > &	inliers
	)			`[inline]`

Find a line model in a point cloud given via a set of point indices with SAmple Consensus methods.

Parameters:

	points	the point cloud message
	indices	a pointer to a set of point cloud indices to test
	inliers	the resultant inliers

Definition at line 205 of file sac_inc_ground_removal.cpp.

void IncGroundRemoval::flipNormalTowardsViewpoint	(	Eigen::Vector4d &	normal,
		const geometry_msgs::Point32 &	point,
		const geometry_msgs::PointStamped &	viewpoint
	)			`[inline]`

Flip (in place) the estimated normal of a point towards a given viewpoint.

Parameters:

	normal	the plane normal to be flipped
	point	a given point
	viewpoint	the viewpoint

Definition at line 239 of file sac_inc_ground_removal_standalone.cpp.

std::string IncGroundRemoval::getAvailableChannels ( const sensor_msgs::PointCloud & cloud ) [inline]

Get the available dimensions as a space separated string.

Parameters:

cloud

the point cloud data message

Definition at line 375 of file sac_inc_ground_removal_standalone.cpp.

int IncGroundRemoval::getChannelIndex	(	const sensor_msgs::PointCloud &	points,
		std::string	channel_name
	)			`[inline]`

Get the index of a specified dimension/channel in a point cloud.

Parameters:

	points	the point cloud
	channel_name	the string defining the channel name

Definition at line 362 of file sac_inc_ground_removal_standalone.cpp.

void IncGroundRemoval::getCloudViewPoint	(	string	cloud_frame,
		geometry_msgs::PointStamped &	viewpoint_cloud,
		tf::TransformListener *	tf
	)			`[inline]`

Get the view point from where the scans were taken in the incoming PointCloud message frame.

Parameters:

	cloud_frame	the point cloud message TF frame
	viewpoint_cloud	the resultant view point in the incoming cloud frame
	tf	a pointer to a TransformListener object

Definition at line 576 of file sac_inc_ground_removal_standalone.cpp.

void IncGroundRemoval::getCloudViewPoint	(	string	cloud_frame,
		geometry_msgs::PointStamped &	viewpoint_cloud,
		tf::TransformListener *	tf
	)			`[inline]`

Get the view point from where the scans were taken in the incoming PointCloud message frame.

Parameters:

	cloud_frame	the point cloud message TF frame
	viewpoint_cloud	the resultant view point in the incoming cloud frame
	tf	a pointer to a TransformListener object

Definition at line 417 of file sac_inc_ground_removal.cpp.

double IncGroundRemoval::pointToPlaneDistanceSigned	(	const geometry_msgs::Point32 &	p,
		const Eigen::Vector4d &	plane_coefficients
	)			`[inline]`

Get the distance from a point to a plane (signed) defined by ax+by+cz+d=0.

Parameters:

	p	a point
	plane_coefficients	the normalized coefficients (a, b, c, d) of a plane

Definition at line 564 of file sac_inc_ground_removal_standalone.cpp.

void IncGroundRemoval::splitPointsBasedOnLaserScanIndex	(	sensor_msgs::PointCloud *	points,
		vector< int > *	indices,
		vector< vector< int > > &	clusters,
		int	idx
	)			`[inline]`

Decompose a PointCloud message into LaserScan clusters.

Parameters:

	points	pointer to the point cloud message
	indices	pointer to a list of point indices
	clusters	the resultant clusters
	idx	the index of the channel containing the laser scan index

Definition at line 151 of file sac_inc_ground_removal_standalone.cpp.

void IncGroundRemoval::splitPointsBasedOnLaserScanIndex	(	sensor_msgs::PointCloud *	points,
		vector< int > *	indices,
		vector< vector< int > > &	clusters,
		int	idx
	)			`[inline]`

Decompose a PointCloud message into LaserScan clusters.

Parameters:

	points	pointer to the point cloud message
	indices	pointer to a list of point indices
	clusters	the resultant clusters
	idx	the index of the channel containing the laser scan index

Definition at line 161 of file sac_inc_ground_removal.cpp.

double IncGroundRemoval::transformDoubleValueTF	(	double	val,
		std::string	src_frame,
		std::string	tgt_frame,
		ros::Time	stamp,
		tf::TransformListener *	tf
	)			`[inline]`

Transform a value from a source frame to a target frame at a certain moment in time with TF.

Parameters:

	val	the value to transform
	src_frame	the source frame to transform the value from
	tgt_frame	the target frame to transform the value into
	stamp	a given time stamp
	tf	a pointer to a TransformListener object

Definition at line 644 of file sac_inc_ground_removal_standalone.cpp.

double IncGroundRemoval::transformDoubleValueTF	(	double	val,
		std::string	src_frame,
		std::string	tgt_frame,
		ros::Time	stamp,
		tf::TransformListener *	tf
	)			`[inline]`

Transform a value from a source frame to a target frame at a certain moment in time with TF.

Parameters:

	val	the value to transform
	src_frame	the source frame to transform the value from
	tgt_frame	the target frame to transform the value into
	stamp	a given time stamp
	tf	a pointer to a TransformListener object

Definition at line 485 of file sac_inc_ground_removal.cpp.

void IncGroundRemoval::transformPoint	(	tf::TransformListener *	tf,
		const std::string &	target_frame,
		const tf::Stamped< geometry_msgs::Point32 > &	stamped_in,
		tf::Stamped< geometry_msgs::Point32 > &	stamped_out
	)			`[inline]`

Transform a given point from its current frame to a given target frame.

Parameters:

	tf	a pointer to a TransformListener object
	target_frame	the target frame to transform the point into
	stamped_in	the input point
	stamped_out	the output point

Definition at line 610 of file sac_inc_ground_removal_standalone.cpp.

void IncGroundRemoval::transformPoint	(	tf::TransformListener *	tf,
		const std::string &	target_frame,
		const tf::Stamped< geometry_msgs::Point32 > &	stamped_in,
		tf::Stamped< geometry_msgs::Point32 > &	stamped_out
	)			`[inline]`

Transform a given point from its current frame to a given target frame.

Parameters:

	tf	a pointer to a TransformListener object
	target_frame	the target frame to transform the point into
	stamped_in	the input point
	stamped_out	the output point