#include <door_handle_detector/geometric_helper.h>

Include dependency graph for geometric_helper.cpp:

Functions
bool	checkDoorEdges (const geometry_msgs::Polygon &poly, const geometry_msgs::Point32 &z_axis, double min_height, double eps_angle, double &door_frame1, double &door_frame2)
bool	checkIfClusterPerpendicular (const sensor_msgs::PointCloud &points, const vector< int > &indices, geometry_msgs::PointStamped viewpoint, vector< double > coeff, double eps_angle)
	Test whether the given point indices are roughly sampled at the viewpoint <-> plane perpendicular.
void	estimatePointNormals (const sensor_msgs::PointCloud &points, const vector< int > &point_indices, sensor_msgs::PointCloud &points_down, int k, const geometry_msgs::PointStamped &viewpoint_cloud)
	Estimate point normals for a given point cloud message (in place)
void	estimatePointNormals (sensor_msgs::PointCloud &points, const vector< int > &point_indices, int k, const geometry_msgs::PointStamped &viewpoint_cloud)
	Estimate point normals for a given point cloud message (in place)
void	estimatePointNormals (const sensor_msgs::PointCloud &points, sensor_msgs::PointCloud &points_down, int k, const geometry_msgs::PointStamped &viewpoint_cloud)
	Estimate point normals for a given point cloud message (in place)
void	findClusters (const sensor_msgs::PointCloud &points, const vector< int > &indices, double tolerance, vector< vector< int > > &clusters, int nx_idx, int ny_idx, int nz_idx, double eps_angle, unsigned int min_pts_per_cluster)
	Decompose a region of space into clusters based on the euclidean distance between points, and the normal angular deviation : assumes normalized point normals !
int	fitSACOrientedLine (sensor_msgs::PointCloud &points, const std::vector< int > &indices, double dist_thresh, const geometry_msgs::Point32 &axis, double eps_angle, std::vector< int > &line_inliers)
	Finds the best oriented line in points/indices with respect to a given axis and return the point inliers.
int	fitSACOrientedLine (sensor_msgs::PointCloud &points, double dist_thresh, const geometry_msgs::Point32 &axis, double eps_angle, std::vector< int > &line_inliers)
	Finds the best oriented line in points with respect to a given axis and return the point inliers.
bool	fitSACPlane (sensor_msgs::PointCloud &points, vector< int > indices, vector< int > &inliers, vector< double > &coeff, const geometry_msgs::PointStamped &viewpoint_cloud, double dist_thresh, int min_pts)
	Find a plane model in a point cloud given via a set of point indices with SAmple Consensus methods.
void	get3DBounds (geometry_msgs::Point32 p1, geometry_msgs::Point32 p2, geometry_msgs::Point32 &min_b, geometry_msgs::Point32 &max_b, double min_z_bounds, double max_z_bounds, int multiplier)
	Get the 3D bounds where the door will be searched for.
void	getCloudViewPoint (const string cloud_frame, geometry_msgs::PointStamped &viewpoint_cloud, const tf::TransformListener *tf)
	Get the view point from where the scans were taken in the incoming PointCloud message frame.
void	growCurrentCluster (const sensor_msgs::PointCloud &points, const std::vector< int > &indices, const std::vector< int > &cluster, std::vector< int > &inliers, double dist_thresh)
	Grows the current euclidean cluster with other points outside the plane.
void	obtainCloudIndicesSet (const sensor_msgs::PointCloud &points, vector< int > &indices, door_msgs::Door &door, tf::TransformListener *tf, std::string parameter_frame, double min_z_bounds, double max_z_bounds, double frame_multiplier)
	Get a set of point indices between some specified bounds.
void	selectBestDistributionStatistics (const sensor_msgs::PointCloud &points, const vector< int > &indices, int d_idx, vector< int > &inliers)
	Get the best distribution statistics (mean and standard deviation) and select the inliers based on them in a given channel space (specified by the d_idx index)
void	selectBestDualDistributionStatistics (const sensor_msgs::PointCloud &points, const vector< int > &indices, int d_idx_1, int d_idx_2, vector< int > &inliers)
	Get the best dual distribution statistics (mean and standard deviation) and select the inliers based on them in a given channel space (specified by the d_idx index)

Function Documentation

bool checkDoorEdges	(	const geometry_msgs::Polygon &	poly,
		const geometry_msgs::Point32 &	z_axis,
		double	min_height,
		double	eps_angle,
		double &	door_frame1,
		double &	door_frame2
	)

Definition at line 112 of file geometric_helper.cpp.

bool checkIfClusterPerpendicular	(	const sensor_msgs::PointCloud &	points,
		const vector< int > &	indices,
		geometry_msgs::PointStamped *	viewpoint,
		vector< double > *	coeff,
		double	eps_angle
	)

Test whether the given point indices are roughly sampled at the viewpoint <-> plane perpendicular.

Parameters:

points	a pointer to the point cloud message
indices	a pointer to a set of point cloud indices to test
viewpoint	the viewpoint of the laser when the cloud was acquired
coeff	the plane coefficients
eps_angle	a maximum allowed angular threshold

Definition at line 420 of file geometric_helper.cpp.

void estimatePointNormals	(	const sensor_msgs::PointCloud &	points,
		const vector< int > &	point_indices,
		sensor_msgs::PointCloud &	points_down,
		int	k,
		const geometry_msgs::PointStamped &	viewpoint_cloud
	)

Estimate point normals for a given point cloud message (in place)

Parameters:

points	the original point cloud message
point_indices	only use these point indices
points_down	the downsampled point cloud message
k	the number of nearest neighbors to search for
viewpoint_cloud	a pointer to the viewpoint where the cloud was acquired from (used for normal flip)

Definition at line 627 of file geometric_helper.cpp.

void estimatePointNormals	(	sensor_msgs::PointCloud &	points,
		const vector< int > &	point_indices,
		int	k,
		const geometry_msgs::PointStamped &	viewpoint_cloud
	)

Estimate point normals for a given point cloud message (in place)

Parameters:

points	the original point cloud message
point_indices	only use these point indices
k	the number of nearest neighbors to search for
viewpoint_cloud	a pointer to the viewpoint where the cloud was acquired from (used for normal flip)

Definition at line 681 of file geometric_helper.cpp.

void estimatePointNormals	(	const sensor_msgs::PointCloud &	points,
		sensor_msgs::PointCloud &	points_down,
		int	k,
		const geometry_msgs::PointStamped &	viewpoint_cloud
	)

Estimate point normals for a given point cloud message (in place)

Parameters:

points	the original point cloud message
points_down	the downsampled point cloud message
k	the number of nearest neighbors to search for
viewpoint_cloud	a pointer to the viewpoint where the cloud was acquired from (used for normal flip)

Definition at line 738 of file geometric_helper.cpp.

void findClusters	(	const sensor_msgs::PointCloud &	points,
		const vector< int > &	indices,
		double	tolerance,
		vector< vector< int > > &	clusters,
		int	nx_idx,
		int	ny_idx,
		int	nz_idx,
		double	eps_angle,
		unsigned int	min_pts_per_cluster
	)

Decompose a region of space into clusters based on the euclidean distance between points, and the normal angular deviation : assumes normalized point normals !

Parameters:

points	pointer to the point cloud message
indices	pointer to a list of point indices
tolerance	the spatial tolerance as a measure in the L2 Euclidean space
clusters	the resultant clusters
nx_idx	the index of the channel containing the X component of the normal
ny_idx	the index of the channel containing the Y component of the normal
nz_idx	the index of the channel containing the Z component of the normal
eps_angle	the maximum allowed difference between normals in degrees for cluster/region growing
min_pts_per_cluster	minimum number of points that a cluster may contain (default = 1)

Definition at line 466 of file geometric_helper.cpp.

int fitSACOrientedLine	(	sensor_msgs::PointCloud &	points,
		const std::vector< int > &	indices,
		double	dist_thresh,
		const geometry_msgs::Point32 &	axis,
		double	eps_angle,
		std::vector< int > &	line_inliers
	)

Finds the best oriented line in points/indices with respect to a given axis and return the point inliers.

Parameters:

points	a pointer to the point cloud message
indices	the point cloud indices to use
dist_thresh	maximum allowed distance threshold of an inlier point to the line model
axis	the axis to check against
eps_angle	maximum angular line deviation from the axis (in radians)
line_inliers	the resultant point inliers

Definition at line 794 of file geometric_helper.cpp.

int fitSACOrientedLine	(	sensor_msgs::PointCloud &	points,
		double	dist_thresh,
		const geometry_msgs::Point32 &	axis,
		double	eps_angle,
		std::vector< int > &	line_inliers
	)

Finds the best oriented line in points with respect to a given axis and return the point inliers.

Parameters:

points	a pointer to the point cloud message
dist_thresh	maximum allowed distance threshold of an inlier point to the line model
axis	the axis to check against
eps_angle	maximum angular line deviation from the axis (in radians)
line_inliers	the resultant point inliers

Definition at line 840 of file geometric_helper.cpp.

bool fitSACPlane	(	sensor_msgs::PointCloud &	points,
		vector< int >	indices,
		vector< int > &	inliers,
		vector< double > &	coeff,
		const geometry_msgs::PointStamped &	viewpoint_cloud,
		double	dist_thresh,
		int	min_pts
	)

Find a plane 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 planar inliers
coeff	the resultant plane coefficients
viewpoint_cloud	a point to the pose where the data was acquired from (for normal flipping)
dist_thresh	the maximum allowed distance threshold of an inlier to the model
min_pts	the minimum number of points allowed as inliers for a plane model

Definition at line 565 of file geometric_helper.cpp.

void get3DBounds	(	geometry_msgs::Point32 *	p1,
		geometry_msgs::Point32 *	p2,
		geometry_msgs::Point32 &	min_b,
		geometry_msgs::Point32 &	max_b,
		double	min_z_bounds,
		double	max_z_bounds,
		int	multiplier
	)

Get the 3D bounds where the door will be searched for.

Parameters:

p1	a point on the floor describing the door frame
p2	a point on the floor describing the door frame
min_b	the minimum bounds (x-y-z)
max_b	the maximum bounds (x-y-z)
min_z_bounds	restrict the minimum search bounds on Z to this value
max_z_bounds	restrict the maximum search bounds on Z to this value
multiplier	multiply the \|\|p1-p2\|\| distance by this number to wrap all possible situations in X-Y

Definition at line 213 of file geometric_helper.cpp.

void getCloudViewPoint	(	const string	cloud_frame,
		geometry_msgs::PointStamped &	viewpoint_cloud,
		const tf::TransformListener *	tf
	)

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 254 of file geometric_helper.cpp.

void growCurrentCluster	(	const sensor_msgs::PointCloud &	points,
		const std::vector< int > &	indices,
		const std::vector< int > &	cluster,
		std::vector< int > &	inliers,
		double	dist_thresh
	)

Grows the current euclidean cluster with other points outside the plane.

Parameters:

points	a pointer to the point cloud message
indices	the point cloud indices to use
cluster	the point cloud cluster to grow
inliers	the resultant point inliers
dist_thresh	the distance threshold used

Definition at line 884 of file geometric_helper.cpp.

void obtainCloudIndicesSet	(	const sensor_msgs::PointCloud &	points,
		vector< int > &	indices,
		door_msgs::Door &	door,
		tf::TransformListener *	tf,
		std::string	parameter_frame,
		double	min_z_bounds,
		double	max_z_bounds,
		double	frame_multiplier
	)

Get a set of point indices between some specified bounds.

Parameters:

points	a pointer to the point cloud message
indices	the resultant set of indices
door_req	a door request service containing the X-Y bounds in frame_p1 and frame_p2
tf	a pointer to a TransformListener object
parameter_frame	the TF frame ID in which min_z_bounds and max_z_bounds are given
min_z_bounds	restrict the minimum search bounds on Z to this value
max_z_bounds	restrict the maximum search bounds on Z to this value
frame_multiplier	multiply the \|\|frame_p1-frame_p2\|\| distance by this number to wrap all possible situations in X-Y

Definition at line 49 of file geometric_helper.cpp.

void selectBestDistributionStatistics	(	const sensor_msgs::PointCloud &	points,
		const vector< int > &	indices,
		int	d_idx,
		vector< int > &	inliers
	)

Get the best distribution statistics (mean and standard deviation) and select the inliers based on them in a given channel space (specified by the d_idx index)

Parameters:

points	a pointer to the point cloud message
indices	a pointer to a set of point cloud indices to test
d_idx	the dimension/channel index to test
inliers	the resultant inliers

Definition at line 285 of file geometric_helper.cpp.

void selectBestDualDistributionStatistics	(	const sensor_msgs::PointCloud &	points,
		const vector< int > &	indices,
		int	d_idx_1,
		int	d_idx_2,
		vector< int > &	inliers
	)

Get the best dual distribution statistics (mean and standard deviation) and select the inliers based on them in a given channel space (specified by the d_idx index)

Parameters:

points	a pointer to the point cloud message
indices	a pointer to a set of point cloud indices to test
d_idx_1	the first dimension/channel index to test
d_idx_2	the second dimension/channel index to test
inliers	the resultant inliers

Definition at line 340 of file geometric_helper.cpp.

Functions

Function Documentation