36 #ifndef _extract_polygon_primitives_H_
37 #define _extract_polygon_primitives_H_
47 #include <sensor_msgs/PointCloud2.h>
48 #include <tf/transform_listener.h>
51 #include <pcl/ros/conversions.h>
52 #include <pcl/point_cloud.h>
53 #include <pcl/point_types.h>
54 #include <pcl/segmentation/sac_segmentation.h>
57 #include <visualization_msgs/Marker.h>
58 #include <visualization_msgs/MarkerArray.h>
61 #include <wrapper_collada/wrapper_collada.h>
70 #define PFLN {printf("DEBUG PRINT FILE %s LINE %d\n",__FILE__,__LINE__);}
71 #define _MAX_NUM_POLYGONS_ 270
85 std::string frame_id, ros::Time stamp, std::string name,
86 int action,
int id,
int type,
87 double px,
double py,
double pz,
88 double qx,
double qy,
double qz,
double qw,
89 double sx,
double sy,
double sz,
90 double cr,
double cg,
double cb,
double alpha);
99 void filter_uncoherent_points(pcl::PointCloud<pcl::PointXYZ>::Ptr pcin, pcl::PointCloud<pcl::PointXYZ> *pcout, pcl::PointCloud<pcl::PointXYZ> *pcelim,pcl::PointCloud<pcl::Normal>::Ptr n ,
double radius,
float vx,
float vy,
float vz);
100 void filter_uncoherent_points(pcl::PointCloud<pcl::PointNormal>::Ptr pc,
double radius, pcl::PointCloud<pcl::PointNormal>::Ptr pcout, pcl::PointCloud<pcl::PointNormal>::Ptr pcelim,
float vx,
float vy,
float vz);
101 void draw_markers(
int k, Eigen::Vector4f* q_avg, std::vector<Eigen::Vector4f>* qv, pcl::PointCloud<pcl::PointNormal>::Ptr pc, std::vector<int>* pointIdxRadiusSearch, Eigen::Vector4f* q_avg0, std::vector<bool>* is_fliped);
102 int compute_normals(pcl::PointCloud<pcl::PointXYZ>* pc_in,
float vx,
float vy,
float vz,
float radius,
int K, pcl::PointCloud<pcl::Normal>* pc_out);
103 int save_pc_PointNormal_to_pcd(pcl::PointCloud<pcl::PointXYZ>* pc_in, pcl::PointCloud<pcl::Normal>* normals_in, std::string name);
104 int get_vehicle_position(ros::NodeHandle *
pn, tf::TransformListener* listener, tf::StampedTransform* vehicle_tf, ros::Time* t);
109 #ifdef _extract_polygon_primitives_CPP_
112 unsigned char cr[16]={0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
113 unsigned char g[16]={0, 16, 32, 49, 65, 81, 97, 113, 130, 146, 162, 178, 194, 210, 227, 243};
114 unsigned char b[16]={255, 247, 239, 231, 223, 215, 206, 198, 190, 182, 174, 166, 158, 150, 142, 134};
118 #define _EXTERN_ extern
119 extern unsigned char cr[16];
120 extern unsigned char g[16];
121 extern unsigned char b[16];
A class c_polygon_primitive that contains information about a detected polygon primitive as well as t...