The polygon_primitive class. Defines methods for polygon extraction and expansion from a point cloud. Stores information about the polygon like the number of supporting points, supporting plane, convex hull, etc. More...
#include <polygon_primitive.h>
Public Member Functions | |
c_polygon_primitive (ros::NodeHandle *node, const char *name="unamed", unsigned char r=0, unsigned char g=0, unsigned char b=0) | |
Constructor. Allocates space for the required objecs. | |
void | compute_concave_hull (pcl::PointCloud< pcl::PointXYZ >::Ptr pcin, pcl::PointCloud< pcl::PointXYZ >::Ptr pcout, const pcl::ModelCoefficients::Ptr coeff) |
Computes the concave_hull of a given point cloud. | |
void | compute_convex_hull (const pcl::PointCloud< pcl::PointXYZ >::Ptr pcin, const pcl::ModelCoefficients::Ptr coeff, pcl::PointCloud< pcl::PointXYZ >::Ptr pcout) |
Uses pcl library to compute the convex hull of a set of points in pcin, by projecting them first to the plane defined by coeff. | |
void | compute_convex_hull_cgal (const pcl::PointCloud< pcl::PointXYZ >::Ptr pcin, const pcl::ModelCoefficients::Ptr coeff, pcl::PointCloud< pcl::PointXYZ >::Ptr pcout, tf::Transform *tr) |
Uses cgal to compute the convex hull of the points in pcin. First, points are projected to the plane defined by coeff. Outputs the area and solidity of the hull. | |
void | compute_convex_hull_cgal (const pcl::PointCloud< pcl::PointXYZ >::Ptr pcin, const pcl::ModelCoefficients::Ptr coeff, pcl::PointCloud< pcl::PointXYZ >::Ptr pcout, tf::Transform *tr, double &area, double &solidity, bool flg=false) |
int | compute_supporting_perpendicular_plane_ransac (pcl::PointCloud< pcl::PointXYZ > *pc_in, pcl::PointCloud< pcl::Normal > *n_in, double DistanceThreshold, double NormalDistanceWeight, int MaxIterations, pcl::PointIndices::Ptr ind_out, pcl::ModelCoefficients::Ptr coeff_out) |
int | compute_supporting_plane_ransac (pcl::PointCloud< pcl::PointXYZ > *input_cloud, pcl::PointCloud< pcl::Normal > *input_normals, double DistanceThreshold, double NormalDistanceWeight, int MaxIterations, pcl::PointIndices::Ptr indices, pcl::ModelCoefficients::Ptr coefficients) |
visualization_msgs::Marker | create_visualization_marker_header (std::string frame_id, ros::Time stamp, std::string name, int action, int id, int type, double px, double py, double pz, double qx, double qy, double qz, double qw, double sx, double sy, double sz, double cr, double cg, double cb, double alpha) |
int | create_vizualization_msgs (visualization_msgs::MarkerArray *marker_vec, unsigned int id_start=0) |
int | export_to_polygon_primitive_msg (polygon_primitive_msg::polygon_primitive *msg) |
int | import_from_polygon_primitive_msg (polygon_primitive_msg::polygon_primitive *msg) |
int | indices_extraction (pcl::PointIndices::Ptr ind, pcl::PointCloud< pcl::PointXYZ > *input_cloud, pcl::PointCloud< pcl::PointXYZ > *remove_cloud, pcl::PointCloud< pcl::PointXYZ >::Ptr copy_cloud, pcl::PointCloud< pcl::Normal > *input_normals, pcl::PointCloud< pcl::Normal > *remove_normals, int compute_normals=1) |
Extracts points from input pc and copies them to copy_cloud. Does the same with normals. | |
int | polygon_create (pcl::PointCloud< pcl::PointXYZ > *input_cloud, pcl::PointCloud< pcl::Normal > *input_normals, double DistanceThreshold=0.5, double NormalDistanceWeight=0.5, int MaxIterations=1000, int do_spatial_division=0) |
Computes a polygon primitive candidate from an input point cloud and its normals. | |
int | polygon_expansion (pcl::PointCloud< pcl::PointXYZ > *input_cloud, double longitudinal_offset=0.2, double perpendicular_offset=0.3, pcl::PointCloud< pcl::Normal > *input_normals=NULL) |
Grows polygon by adding points from the input pc that are close to the plane and inside the convex hull. First perfoms a longitudinal expansion of the convex hull, and then selects points which are in the prisma defined by the perpeddicular -/+ offset of the extended convex hull. | |
void | polygon_split (pcl::PointCloud< pcl::PointXYZ >::Ptr pcin, pcl::PointCloud< pcl::PointXYZ >::Ptr pcdumpster, pcl::ModelCoefficients::Ptr coeff) |
int | print_polygon_information (void) |
void | project_pc_to_plane (pcl::PointCloud< pcl::PointXYZ >::Ptr pcin, pcl::ModelCoefficients::Ptr coeff, pcl::PointCloud< pcl::PointXYZ >::Ptr pcout) |
Projects all the points in pcin to a plane (along the normal to that plane) defined by coeff and stores the projected points in pcout. | |
~c_polygon_primitive () | |
Destructor. Frees the space of objects. | |
Public Attributes | |
t_polygon_primitive_data | data |
struct { | |
pcl::PointCloud< pcl::PointXYZ > ::Ptr additional | |
A point cloud with all supporting points. | |
pcl::PointCloud< pcl::PointXYZ > ::Ptr all | |
pcl::PointCloud< pcl::PointXYZ > ::Ptr growed | |
The points that are to be added to the point cloud after an expand operation. | |
pcl::PointCloud< pcl::PointXYZ > ::Ptr projected | |
A point cloud with all supporting points. | |
pcl::PointCloud< pcl::PointXYZ > ::Ptr tmp | |
Dont know. | |
} | pointclouds |
A structure containing all the point clouds needed to represent the polygon primitive. | |
ros::NodeHandle * | rosnode |
Private Member Functions | |
int | _polygon_expansion (pcl::PointCloud< pcl::PointXYZ > *input_cloud, double longitudinal_offset, double perpendicular_offset, pcl::PointCloud< pcl::Normal > *input_normals) |
Internal function to perform single step polygon expansion. | |
void | allocate_space (void) |
bool | check_if_point_lies_on_plane (const pcl::ModelCoefficients::Ptr plane, const pcl::PointXYZ *point) |
Bollean test to assess if a point lies on a plane. | |
int | check_plane_normal_orientation (pcl::ModelCoefficients::Ptr plane, pcl::PointXYZ *ptin, double vx, double vy, double vz) |
Checks the the normal vector of a plane at point ptin is propperly oriented according to the viewpoint vx, vy, vz. | |
void | compute_arrow_points_from_transform (t_reference_frame *frame) |
Computes the 3D coordinates of the arrows of a given reference frame. | |
double | compute_polygon_area (const polygon_3d< pcl::PointXYZ >::Ptr pcin) |
void | copy (const t_reference_frame *in, t_reference_frame *out) |
Copies a t_reference_frame to another. | |
void | copy (pcl::ModelCoefficients::Ptr in, pcl::ModelCoefficients::Ptr out) |
Copies the plane model coefficients. | |
void | create_reference_frame_from_plane_and_two_points (pcl::ModelCoefficients::Ptr plane, pcl::PointXYZ *pt1, pcl::PointXYZ *pt2, t_reference_frame *frame) |
Creates an arbitrary reference frame from a plane equation and two inliers. The Z axis is given by the vector normal to the plane, the X axis is defined by the vector to goes from pt1 to pt2. Y is defined by the external product Z*X. | |
double | fit_plane_to_pc (pcl::PointCloud< pcl::PointXYZ >::Ptr pcin, pcl::ModelCoefficients::Ptr plane) |
Fits a plane to a point cloud. | |
double | fit_plane_to_two_pc_and_ratio (pcl::PointCloud< pcl::PointXYZ >::Ptr pcin1, pcl::PointCloud< pcl::PointXYZ >::Ptr pcin2, double ratio, pcl::ModelCoefficients::Ptr plane) |
fits a plane to two weighted point clouds, given a ratio | |
void | normalize_vector (double *v) |
Normalizes a vector. | |
void | offset_polygon (double val, const pcl::PointCloud< pcl::PointXYZ >::Ptr pcin, pcl::PointCloud< pcl::PointXYZ >::Ptr pcout, tf::Transform *tr) |
Offsets a given polygon. The offseting is produced along the XoY plane defined by the points in pcin. | |
void | project_point_to_plane (const pcl::PointXYZ *ptin, const pcl::ModelCoefficients::Ptr coeff, pcl::PointXYZ *ptout) |
Projects all the points in a point cloud ptin to the plane defined by coeff, and writes the result to ptout. | |
void | publish_local_tf (void) |
Publishes the local coordinate system with respect to the /world. | |
void | refine_plane_coefficients (pcl::PointCloud< pcl::PointXYZ >::Ptr pcin, pcl::ModelCoefficients::Ptr coeff) |
recomputes the plane coefficients of the plane that best fits in the minimum square distance sense all the points in pcin | |
void | set_names (const char *s) |
Sets the polygon name. | |
void | set_reference_systems (void) |
Sets the reference systems. | |
Private Attributes | |
tf::TransformBroadcaster | br |
sensor_msgs::PointCloud2 | cloud_msg |
unsigned int | grow_number |
The polygon_primitive class. Defines methods for polygon extraction and expansion from a point cloud. Stores information about the polygon like the number of supporting points, supporting plane, convex hull, etc.
Definition at line 182 of file polygon_primitive.h.
double c_polygon_primitive::compute_polygon_area | ( | const polygon_3d< pcl::PointXYZ >::Ptr | pcin | ) | [private] |
void c_polygon_primitive::copy | ( | const t_reference_frame * | in, | |
t_reference_frame * | out | |||
) | [inline, private] |
Copies a t_reference_frame to another.
in | the reference frame to copy | |
out | the reference frame where to copy to |
Definition at line 346 of file polygon_primitive.h.
void c_polygon_primitive::copy | ( | pcl::ModelCoefficients::Ptr | in, | |
pcl::ModelCoefficients::Ptr | out | |||
) | [inline, private] |
Copies the plane model coefficients.
in | Modelcoefficients in | |
out | Modelcoefficients out |
Definition at line 332 of file polygon_primitive.h.
pcl::PointCloud<pcl::PointXYZ>::Ptr c_polygon_primitive::additional |
A point cloud with all supporting points.
Definition at line 195 of file polygon_primitive.h.
pcl::PointCloud<pcl::PointXYZ>::Ptr c_polygon_primitive::all |
Definition at line 195 of file polygon_primitive.h.
tf::TransformBroadcaster c_polygon_primitive::br [private] |
Definition at line 206 of file polygon_primitive.h.
sensor_msgs::PointCloud2 c_polygon_primitive::cloud_msg [private] |
Definition at line 207 of file polygon_primitive.h.
Definition at line 203 of file polygon_primitive.h.
unsigned int c_polygon_primitive::grow_number [private] |
Definition at line 208 of file polygon_primitive.h.
pcl::PointCloud<pcl::PointXYZ>::Ptr c_polygon_primitive::growed |
The points that are to be added to the point cloud after an expand operation.
Definition at line 195 of file polygon_primitive.h.
struct { ... } c_polygon_primitive::pointclouds |
A structure containing all the point clouds needed to represent the polygon primitive.
pcl::PointCloud<pcl::PointXYZ>::Ptr c_polygon_primitive::projected |
A point cloud with all supporting points.
Definition at line 195 of file polygon_primitive.h.
ros::NodeHandle* c_polygon_primitive::rosnode |
Definition at line 202 of file polygon_primitive.h.
pcl::PointCloud<pcl::PointXYZ>::Ptr c_polygon_primitive::tmp |
Dont know.
Definition at line 195 of file polygon_primitive.h.