#include <math.h>
#include <vector>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud.h>
#include <geometry_msgs/Polygon.h>
#include <geometry_msgs/Point32.h>
#include <geometry_msgs/PointStamped.h>
#include <visualization_msgs/Marker.h>
#include <door_msgs/Door.h>
#include <tf/transform_listener.h>
#include <door_handle_detector/geometry/angles.h>
#include <door_handle_detector/geometry/areas.h>
#include <door_handle_detector/geometry/distances.h>
#include <door_handle_detector/geometry/intersections.h>
#include <door_handle_detector/geometry/nearest.h>
#include <door_handle_detector/geometry/transforms.h>
#include <door_handle_detector/geometry/point.h>
#include <door_handle_detector/geometry/projections.h>
#include <door_handle_detector/geometry/statistics.h>
#include <door_handle_detector/kdtree/kdtree_ann.h>
#include <door_handle_detector/sample_consensus/sac.h>
#include <door_handle_detector/sample_consensus/msac.h>
#include <door_handle_detector/sample_consensus/ransac.h>
#include <door_handle_detector/sample_consensus/lmeds.h>
#include <door_handle_detector/sample_consensus/sac_model_plane.h>
#include <door_handle_detector/sample_consensus/sac_model_oriented_line.h>

Include dependency graph for geometric_helper.h:

This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

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 (sensor_msgs::PointCloud points, std::vector< int > indices, geometry_msgs::PointStamped viewpoint, std::vector< double > coeff, double eps_angle)
bool	compareDoorsWeight (const door_msgs::Door &a, const door_msgs::Door &b)
bool	compareRegions (const std::vector< int > &a, const std::vector< int > &b)
void	estimatePointNormals (const sensor_msgs::PointCloud &points, const std::vector< int > &point_indices, sensor_msgs::PointCloud &points_down, int k, const geometry_msgs::PointStamped &viewpoint_cloud)
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	estimatePointNormals (sensor_msgs::PointCloud &points, const std::vector< int > &point_indices, int k, const geometry_msgs::PointStamped &viewpoint_cloud)
void	findClusters (const sensor_msgs::PointCloud &points, const std::vector< int > &indices, double tolerance, std::vector< std::vector< int > > &clusters, int nx_idx, int ny_idx, int nz_idx, double eps_angle, unsigned int min_pts_per_cluster=1)
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, std::vector< int > indices, std::vector< int > &inliers, std::vector< double > &coeff, const geometry_msgs::PointStamped &viewpoint_cloud, double dist_thresh, int min_pts)
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 std::string cloud_frame, geometry_msgs::PointStamped &viewpoint_cloud, const tf::TransformListener *tf)
double	getRGB (float r, float g, float b)
	Obtain a 24-bit RGB coded value from 3 independent <r, g, b> channel values.
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 (sensor_msgs::PointCloud points, std::vector< int > &indices, door_msgs::Door &door, tf::TransformListener tf, std::string fixed_param_frame, double min_z_bounds, double max_z_bounds, double frame_multiplier)
void	selectBestDistributionStatistics (const sensor_msgs::PointCloud &points, const std::vector< int > &indices, int d_idx, std::vector< int > &inliers)
void	selectBestDualDistributionStatistics (const sensor_msgs::PointCloud &points, const std::vector< int > &indices, int d_idx_1, int d_idx_2, std::vector< int > &inliers)
void	sendMarker (float px, float py, float pz, std::string frame_id, ros::Publisher &pub, int &id, int r, int g, int b, double radius=0.03)
	Send a sphere vizualization marker.
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 (const 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.

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	(	sensor_msgs::PointCloud *	points,
		std::vector< int > *	indices,
		geometry_msgs::PointStamped *	viewpoint,
		std::vector< double > *	coeff,
		double	eps_angle
	)

bool compareDoorsWeight	(	const door_msgs::Door &	a,
		const door_msgs::Door &	b
	)		`[inline]`

Definition at line 131 of file geometric_helper.h.

bool compareRegions	(	const std::vector< int > &	a,
		const std::vector< int > &	b
	)		`[inline]`

Definition at line 123 of file geometric_helper.h.

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

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 estimatePointNormals	(	sensor_msgs::PointCloud &	points,
		const std::vector< int > &	point_indices,
		int	k,
		const geometry_msgs::PointStamped &	viewpoint_cloud
	)

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

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,
		std::vector< int >	indices,
		std::vector< int > &	inliers,
		std::vector< double > &	coeff,
		const geometry_msgs::PointStamped &	viewpoint_cloud,
		double	dist_thresh,
		int	min_pts
	)

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 std::string	cloud_frame,
		geometry_msgs::PointStamped &	viewpoint_cloud,
		const tf::TransformListener *	tf
	)

double getRGB	(	float	r,
		float	g,
		float	b
	)		`[inline]`

Obtain a 24-bit RGB coded value from 3 independent <r, g, b> channel values.

Parameters:

r	the red channel value
g	the green channel value
b	the blue channel value

Definition at line 142 of file geometric_helper.h.

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	(	sensor_msgs::PointCloud *	points,
		std::vector< int > &	indices,
		door_msgs::Door &	door,
		tf::TransformListener *	tf,
		std::string	fixed_param_frame,
		double	min_z_bounds,
		double	max_z_bounds,
		double	frame_multiplier
	)

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

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

void sendMarker	(	float	px,
		float	py,
		float	pz,
		std::string	frame_id,
		ros::Publisher &	pub,
		int &	id,
		int	r,
		int	g,
		int	b,
		double	radius = `0.03`
	)		`[inline]`

Send a sphere vizualization marker.

Parameters:

point	the point to send
frame_id	the frame
node	a pointer to the node structure
radius	an optional radius for the sphere marker (2cm by default)

Definition at line 157 of file geometric_helper.h.

double 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 109 of file geometric_helper.h.

void transformPoint	(	const 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.

Author:: Radu Bogdan Rusu

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 81 of file geometric_helper.h.

Functions

Function Documentation