00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00036 #ifndef _polygon_primitive_H_
00037 #define _polygon_primitive_H_
00038
00042 #define PFLN {printf("DEBUG PRINT FILE %s LINE %d\n",__FILE__,__LINE__);}
00043
00044
00047
00049 #include <ros/ros.h>
00050 #include <sensor_msgs/PointCloud2.h>
00051 #include <sensor_msgs/PointCloud.h>
00052 #include <visualization_msgs/Marker.h>
00053 #include <visualization_msgs/MarkerArray.h>
00054 #include <tf/transform_broadcaster.h>
00055
00057 #include "pcl_ros/impl/transforms.hpp"
00058 #include <pcl/ros/conversions.h>
00059 #include <pcl/point_cloud.h>
00060 #include <pcl/point_types.h>
00061 #include <pcl_ros/point_cloud.h>
00062 #include <pcl/io/pcd_io.h>
00063 #include <pcl/point_types.h>
00064 #include <pcl/sample_consensus/method_types.h>
00065 #include <pcl/sample_consensus/model_types.h>
00066 #include <pcl/segmentation/sac_segmentation.h>
00067 #include <pcl/features/normal_3d.h>
00068 #include <pcl/ModelCoefficients.h>
00069 #include <pcl/filters/extract_indices.h>
00070 #include <pcl/segmentation/sac_segmentation.h>
00071 #include <pcl/filters/project_inliers.h>
00072 #include <pcl/filters/extract_indices.h>
00073 #include <pcl/surface/convex_hull.h>
00074 #include <pcl/surface/concave_hull.h>
00075 #include <pcl/ModelCoefficients.h>
00076 #include <pcl/features/normal_3d.h>
00077 #include <pcl/filters/extract_indices.h>
00078 #include <pcl/filters/voxel_grid.h>
00079 #include <pcl/kdtree/kdtree.h>
00080 #include <pcl/sample_consensus/method_types.h>
00081 #include <pcl/sample_consensus/model_types.h>
00082 #include <pcl/segmentation/sac_segmentation.h>
00083
00084 #include <pcl/segmentation/extract_clusters.h>
00085
00086 #include <pcl/segmentation/extract_polygonal_prism_data.h>
00087
00088
00090 #include <opencv2/core/core.hpp>
00091
00093 #include <polygon_primitive_msg/polygon_primitive.h>
00094
00095
00099 template <class PointT>
00100 class polygon_3d:public pcl::PointCloud<PointT>{};
00101
00102
00103 typedef struct {double x,y,z;}t_vector3d;
00104 typedef struct
00105 {
00106 t_vector3d origin;
00107 t_vector3d arrow_x;
00108 t_vector3d arrow_y;
00109 t_vector3d arrow_z;
00110 tf::Transform transform;
00111 }t_reference_frame;
00112
00113 typedef struct
00114 {
00115 struct
00116 {
00120 struct
00121 {
00122 unsigned char r,g,b;
00123 }color;
00124
00125 char name[1024];
00126 bool ground_search;
00127
00128 }misc;
00129
00133 struct
00134 {
00138 struct
00139 {
00140 polygon_3d<pcl::PointXYZ>::Ptr polygon, extended_polygon;
00141 double area;
00142 double solidity;
00143 }convex;
00144
00148 struct
00149 {
00150 polygon_3d<pcl::PointXYZ>::Ptr polygon, extended_polygon;
00151 double area;
00152 double solidity;
00153 }concave;
00154 }hulls;
00155
00159 struct
00160 {
00161 pcl::ModelCoefficients::Ptr current;
00162 pcl::ModelCoefficients::Ptr previous;
00163 }planes;
00164
00168 struct
00169 {
00170 std::string local_name;
00171 std::string global_name;
00172 t_reference_frame current;
00173 t_reference_frame previous;
00174 }frames;
00175
00176 }t_polygon_primitive_data;
00177
00178
00179
00183 class c_polygon_primitive
00184 {
00185
00186
00187
00188 public:
00189
00190
00194 struct {
00195 pcl::PointCloud<pcl::PointXYZ>::Ptr
00196 all,
00197 projected,
00198 additional,
00199 growed,
00200 tmp;
00201 }pointclouds;
00202
00203 ros::NodeHandle *rosnode;
00204 t_polygon_primitive_data data;
00205
00206 private:
00207 tf::TransformBroadcaster br;
00208 sensor_msgs::PointCloud2 cloud_msg;
00209 unsigned int grow_number;
00210
00211
00212
00213 public:
00214
00215
00216
00217
00218 c_polygon_primitive(ros::NodeHandle *node, const char* name="unamed", unsigned char r=0, unsigned char g=0, unsigned char b=0);
00219 ~c_polygon_primitive();
00220
00221
00222
00223
00224 int export_to_polygon_primitive_msg(polygon_primitive_msg::polygon_primitive* msg);
00225 int import_from_polygon_primitive_msg(polygon_primitive_msg::polygon_primitive* msg);
00226 int create_vizualization_msgs(visualization_msgs::MarkerArray* marker_vec, unsigned int id_start=0);
00227 visualization_msgs::Marker create_visualization_marker_header(
00228 std::string frame_id, ros::Time stamp, std::string name,
00229 int action, int id, int type,
00230 double px, double py, double pz,
00231 double qx, double qy, double qz, double qw,
00232 double sx, double sy, double sz,
00233 double cr, double cg, double cb, double alpha
00234 );
00235
00236
00237
00238
00239
00240 int print_polygon_information(void);
00241
00242 int compute_supporting_perpendicular_plane_ransac( pcl::PointCloud<pcl::PointXYZ> *pc_in,
00243 pcl::PointCloud<pcl::Normal> *n_in,
00244 double DistanceThreshold,
00245 double NormalDistanceWeight,
00246 int MaxIterations,
00247 pcl::PointIndices::Ptr ind_out,
00248 pcl::ModelCoefficients::Ptr coeff_out
00249 );
00250
00251 int compute_supporting_plane_ransac( pcl::PointCloud<pcl::PointXYZ> *input_cloud,
00252 pcl::PointCloud<pcl::Normal> *input_normals,
00253 double DistanceThreshold,
00254 double NormalDistanceWeight,
00255 int MaxIterations,
00256 pcl::PointIndices::Ptr indices,
00257 pcl::ModelCoefficients::Ptr coefficients
00258 );
00259
00260 int indices_extraction(pcl::PointIndices::Ptr ind,
00261 pcl::PointCloud<pcl::PointXYZ> *input_cloud,
00262 pcl::PointCloud<pcl::PointXYZ> *remove_cloud,
00263 pcl::PointCloud<pcl::PointXYZ>::Ptr copy_cloud,
00264 pcl::PointCloud<pcl::Normal> *input_normals,
00265 pcl::PointCloud<pcl::Normal> *remove_normals,
00266 int compute_normals=1);
00267
00268 void project_pc_to_plane(pcl::PointCloud<pcl::PointXYZ>::Ptr pcin, pcl::ModelCoefficients::Ptr coeff, pcl::PointCloud<pcl::PointXYZ>::Ptr pcout);
00269
00270
00271
00272
00273 int polygon_create(pcl::PointCloud<pcl::PointXYZ> *input_cloud,
00274 pcl::PointCloud<pcl::Normal> *input_normals,
00275 double DistanceThreshold = 0.5,
00276 double NormalDistanceWeight = 0.5,
00277 int MaxIterations = 1000,
00278 int do_spatial_division=0
00279 );
00280
00281 void polygon_split(pcl::PointCloud<pcl::PointXYZ>::Ptr pcin, pcl::PointCloud<pcl::PointXYZ>::Ptr pcdumpster, pcl::ModelCoefficients::Ptr coeff);
00282
00283
00284 int polygon_expansion(pcl::PointCloud<pcl::PointXYZ> *input_cloud,
00285 double longitudinal_offset=0.2,
00286 double perpendicular_offset=0.3,
00287 pcl::PointCloud<pcl::Normal>* input_normals=NULL);
00288
00289
00290
00291
00292
00293 void compute_convex_hull(const pcl::PointCloud<pcl::PointXYZ>::Ptr pcin, const pcl::ModelCoefficients::Ptr coeff, pcl::PointCloud<pcl::PointXYZ>::Ptr pcout);
00294
00295 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);
00296
00297 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);
00298
00299 void compute_concave_hull(pcl::PointCloud<pcl::PointXYZ>::Ptr pcin, pcl::PointCloud<pcl::PointXYZ>::Ptr pcout, const pcl::ModelCoefficients::Ptr coeff);
00300
00301
00302
00303
00304 private:
00305
00306
00307
00308
00309 int _polygon_expansion(pcl::PointCloud<pcl::PointXYZ> *input_cloud,
00310 double longitudinal_offset,
00311 double perpendicular_offset,
00312 pcl::PointCloud<pcl::Normal>* input_normals);
00313
00314
00315
00316
00317
00318
00319 void set_names(const char* s);
00320
00321
00322 void allocate_space(void);
00323 double compute_polygon_area(const polygon_3d<pcl::PointXYZ>::Ptr pcin);
00324
00325 void normalize_vector(double *v);
00326
00333 void copy(pcl::ModelCoefficients::Ptr in, pcl::ModelCoefficients::Ptr out)
00334 {
00335 out->values[0] = in->values[0];
00336 out->values[1] = in->values[1];
00337 out->values[2] = in->values[2];
00338 out->values[3] = in->values[3];
00339 }
00340
00347 void copy(const t_reference_frame *in, t_reference_frame *out)
00348 {
00349 out->origin.x = in->origin.x; out->origin.y = in->origin.y; out->origin.z = in->origin.z;
00350 out->arrow_x.x = in->arrow_x.x; out->arrow_x.y = in->arrow_x.y; out->arrow_x.z = in->arrow_x.z;
00351 out->arrow_y.x = in->arrow_y.x; out->arrow_y.y = in->arrow_y.y; out->arrow_y.z = in->arrow_y.z;
00352 out->arrow_z.x = in->arrow_z.x; out->arrow_z.y = in->arrow_z.y; out->arrow_z.z = in->arrow_z.z;
00353 out->transform = in->transform;
00354 }
00355
00356 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);
00357
00358 void project_point_to_plane(const pcl::PointXYZ *ptin, const pcl::ModelCoefficients::Ptr coeff, pcl::PointXYZ *ptout);
00359
00360 bool check_if_point_lies_on_plane(const pcl::ModelCoefficients::Ptr plane, const pcl::PointXYZ *point);
00361
00362 int check_plane_normal_orientation(pcl::ModelCoefficients::Ptr plane, pcl::PointXYZ *ptin, double vx, double vy, double vz);
00363
00364 void compute_arrow_points_from_transform(t_reference_frame *frame);
00365
00366 void create_reference_frame_from_plane_and_two_points(
00367 pcl::ModelCoefficients::Ptr plane,
00368 pcl::PointXYZ *pt1,
00369 pcl::PointXYZ *pt2,
00370 t_reference_frame *frame
00371 );
00372
00373 void set_reference_systems(void);
00374
00375 void refine_plane_coefficients(pcl::PointCloud<pcl::PointXYZ>::Ptr pcin, pcl::ModelCoefficients::Ptr coeff);
00376
00377
00378
00379
00380
00381 double fit_plane_to_pc(pcl::PointCloud<pcl::PointXYZ>::Ptr pcin, pcl::ModelCoefficients::Ptr plane);
00382
00383
00384
00385
00386 void publish_local_tf(void);
00387
00388
00389
00390
00391 void offset_polygon(double val, const pcl::PointCloud<pcl::PointXYZ>::Ptr pcin, pcl::PointCloud<pcl::PointXYZ>::Ptr pcout, tf::Transform *tr);
00392 };
00393 #endif
00394