Functions
geometric_helper.h File Reference
#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:
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 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:
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,
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:
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 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:
rthe red channel value
gthe green channel value
bthe 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:
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 ( 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:
pointthe point to send
frame_idthe frame
nodea pointer to the node structure
radiusan 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:
valthe value to transform
src_framethe source frame to transform the value from
tgt_framethe target frame to transform the value into
stampa given time stamp
tfa 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:
tfa pointer to a TransformListener object
target_framethe target frame to transform the point into
stamped_inthe input point
stamped_outthe output point

Definition at line 81 of file geometric_helper.h.



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