polygon_primitive.h
Go to the documentation of this file.
00001 /**************************************************************************************************
00002  Software License Agreement (BSD License)
00003 
00004  Copyright (c) 2011-2013, LAR toolkit developers - University of Aveiro - http://lars.mec.ua.pt
00005  All rights reserved.
00006 
00007  Redistribution and use in source and binary forms, with or without modification, are permitted
00008  provided that the following conditions are met:
00009 
00010   *Redistributions of source code must retain the above copyright notice, this list of
00011    conditions and the following disclaimer.
00012   *Redistributions in binary form must reproduce the above copyright notice, this list of
00013    conditions and the following disclaimer in the documentation and/or other materials provided
00014    with the distribution.
00015   *Neither the name of the University of Aveiro nor the names of its contributors may be used to
00016    endorse or promote products derived from this software without specific prior written permission.
00017  
00018  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
00019  IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
00020  FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
00021  CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
00022  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
00023  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
00024  IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
00025  OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
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 // #include <pcl-1.6/pcl/segmentation/extract_clusters.h>
00084 #include <pcl/segmentation/extract_clusters.h>
00085 //#include <pcl/segmentation/extract_labeled_clusters.h>
00086 #include <pcl/segmentation/extract_polygonal_prism_data.h>
00087 //#include <pcl_ros/segmentation/extract_polygonal_prism_data.h>
00088 
00090 #include <opencv2/core/core.hpp>
00091 
00093 #include <polygon_primitive_msg/polygon_primitive.h>
00094 
00095 //The interval variables
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 //Define the polygon class
00183 class c_polygon_primitive
00184 {
00185         // ------------------------------------------------------------------------------
00186         //PUBLIC SECTION OF THE CLASS
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                 //--------------------------Now the functions --------------------
00213         public:
00214 
00215                 //-------------------------------------------
00216                 //--- DEFINED IN polygon_primitive.cpp
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                 //--- DEFINED IN polygon_primitive_comunications.cpp
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                 //--- DEFINED IN polygon_primitive_auxiliary.cpp
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                 //--- DEFINED IN polygon_primitive_operations.cpp
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                 //--- DEFINED IN polygon_primitive_2dhulls.cpp
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                 //PRIVATE SECTION OF THE CLASS
00303                 // ------------------------------------------------------------------------------
00304         private:
00305 
00306                 //-------------------------------------------
00307                 //--- DEFINED IN polygon_primitive_2dhulls.cpp
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                 //--- DEFINED IN polygon_primitive_auxiliary.cpp
00317                 //-------------------------------------------
00318 
00319                 void set_names(const char* s);
00320 
00321                 // allocates space all for pointers
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                 //--- DEFINED IN polygon_primitive_planefitting.cpp
00380                 //-------------------------------------------
00381                 double fit_plane_to_pc(pcl::PointCloud<pcl::PointXYZ>::Ptr pcin, pcl::ModelCoefficients::Ptr plane);
00382 
00383                 //-------------------------------------------
00384                 //--- DEFINED IN polygon_primitive_comunications.cpp
00385                 //-------------------------------------------
00386                 void publish_local_tf(void);
00387 
00388                 //-------------------------------------------
00389                 //--- DEFINED IN polygon_primitive_offseting.cpp
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 


polygon_primitives_extraction
Author(s): Miguel Oliveira
autogenerated on Thu Nov 20 2014 11:35:55