#include <door_handle_detector/geometric_helper.h>
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, 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, 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 | 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, 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. | |
| 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. | |
| 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). | |
| 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.
| 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, | |
| sensor_msgs::PointCloud & | points_down, | |||
| int | k, | |||
| const geometry_msgs::PointStamped & | viewpoint_cloud | |||
| ) |
Estimate point normals for a given point cloud message (in place).
| 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 vector< int > & | point_indices, | |||
| int | k, | |||
| const geometry_msgs::PointStamped & | viewpoint_cloud | |||
| ) |
Estimate point normals for a given point cloud message (in place).
| 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, | |
| 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).
| 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 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 !
| 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, | |
| 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.
| 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.
| 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.
| 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.
| 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.
| 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.
| 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.
| 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.
| 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.
| 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).
| 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).
| 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.