Functions
geometric_helper.cpp File Reference
#include <door_handle_detector/geometric_helper.h>
Include dependency graph for geometric_helper.cpp:

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 (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:
pointsa pointer to the point cloud message
indicesa pointer to a set of point cloud indices to test
viewpointthe viewpoint of the laser when the cloud was acquired
coeffthe plane coefficients
eps_anglea 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:
pointsthe original point cloud message
point_indicesonly use these point indices
points_downthe downsampled point cloud message
kthe number of nearest neighbors to search for
viewpoint_clouda 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:
pointsthe original point cloud message
point_indicesonly use these point indices
kthe number of nearest neighbors to search for
viewpoint_clouda 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:
pointsthe original point cloud message
points_downthe downsampled point cloud message
kthe number of nearest neighbors to search for
viewpoint_clouda 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:
pointspointer to the point cloud message
indicespointer to a list of point indices
tolerancethe spatial tolerance as a measure in the L2 Euclidean space
clustersthe resultant clusters
nx_idxthe index of the channel containing the X component of the normal
ny_idxthe index of the channel containing the Y component of the normal
nz_idxthe index of the channel containing the Z component of the normal
eps_anglethe maximum allowed difference between normals in degrees for cluster/region growing
min_pts_per_clusterminimum 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:
pointsa pointer to the point cloud message
indicesthe point cloud indices to use
dist_threshmaximum allowed distance threshold of an inlier point to the line model
axisthe axis to check against
eps_anglemaximum angular line deviation from the axis (in radians)
line_inliersthe 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:
pointsa pointer to the point cloud message
dist_threshmaximum allowed distance threshold of an inlier point to the line model
axisthe axis to check against
eps_anglemaximum angular line deviation from the axis (in radians)
line_inliersthe 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:
pointsthe point cloud message
indicesa pointer to a set of point cloud indices to test
inliersthe resultant planar inliers
coeffthe resultant plane coefficients
viewpoint_clouda point to the pose where the data was acquired from (for normal flipping)
dist_threshthe maximum allowed distance threshold of an inlier to the model
min_ptsthe 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:
p1a point on the floor describing the door frame
p2a point on the floor describing the door frame
min_bthe minimum bounds (x-y-z)
max_bthe maximum bounds (x-y-z)
min_z_boundsrestrict the minimum search bounds on Z to this value
max_z_boundsrestrict the maximum search bounds on Z to this value
multipliermultiply 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_framethe point cloud message TF frame
viewpoint_cloudthe resultant view point in the incoming cloud frame
tfa 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:
pointsa pointer to the point cloud message
indicesthe point cloud indices to use
clusterthe point cloud cluster to grow
inliersthe resultant point inliers
dist_threshthe 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:
pointsa pointer to the point cloud message
indicesthe resultant set of indices
door_reqa door request service containing the X-Y bounds in frame_p1 and frame_p2
tfa pointer to a TransformListener object
parameter_framethe TF frame ID in which min_z_bounds and max_z_bounds are given
min_z_boundsrestrict the minimum search bounds on Z to this value
max_z_boundsrestrict the maximum search bounds on Z to this value
frame_multipliermultiply 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:
pointsa pointer to the point cloud message
indicesa pointer to a set of point cloud indices to test
d_idxthe dimension/channel index to test
inliersthe 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:
pointsa pointer to the point cloud message
indicesa pointer to a set of point cloud indices to test
d_idx_1the first dimension/channel index to test
d_idx_2the second dimension/channel index to test
inliersthe resultant inliers

Definition at line 340 of file geometric_helper.cpp.



door_handle_detector
Author(s): Radu Bogdan Rusu, Marius
autogenerated on Wed Dec 11 2013 14:17:01