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
00034 #ifndef _polygon_boolean_operations_H_
00035 #define _polygon_boolean_operations_H_
00036
00045 #define PFLN {printf("DEBUG PRINT FILE %s LINE %d\n",__FILE__,__LINE__);}
00046
00047
00050 #include <ros/ros.h>
00051
00052 #include <pcl/point_cloud.h>
00053 #include <pcl/point_types.h>
00054 #include <pcl_ros/transforms.h>
00055 #include <tf/tf.h>
00056
00057
00058 #include <CGAL/basic.h>
00059 #include <CGAL/Polygon_2.h>
00060 #include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
00061 #include <CGAL/Partition_traits_2.h>
00062 #include <CGAL/Partition_is_valid_traits_2.h>
00063 #include <CGAL/polygon_function_objects.h>
00064 #include <CGAL/partition_2.h>
00065 #include <CGAL/point_generators_2.h>
00066 #include <CGAL/Polygon_set_2.h>
00067
00068 #include <CGAL/random_polygon_2.h>
00069
00070 #include <CGAL/Cartesian.h>
00071 #include <CGAL/Boolean_set_operations_2.h>
00072
00073 #include "polygon_simplification.h"
00074 #include "transform_wrapper.h"
00075
00076 typedef CGAL::Exact_predicates_exact_constructions_kernel PBO_Kernel;
00077 typedef PBO_Kernel::Point_2 PBO_Point_2;
00078 typedef CGAL::Polygon_2<PBO_Kernel> PBO_Polygon_2;
00079 typedef CGAL::Polygon_with_holes_2<PBO_Kernel> PBO_Polygon_with_holes_2;
00080 typedef std::list<PBO_Polygon_with_holes_2> PBO_Pwh_list_2;
00081 typedef CGAL::Polygon_set_2<PBO_Kernel> PBO_Polygon_set_2;
00082
00083 class class_polygon_boolean_operations: public class_polygon_simplification, public class_transform_wrapper
00084 {
00085 public:
00086 class_polygon_boolean_operations(void);
00087 ~class_polygon_boolean_operations(void);
00088
00089 int insert(pcl::PointCloud<pcl::PointXYZ>::Ptr p_pc);
00090 int insert(pcl::PointCloud<pcl::PointXYZ>* pc);
00091
00092
00093 int intersection(pcl::PointCloud<pcl::PointXYZ>* pc);
00094 int join(pcl::PointCloud<pcl::PointXYZ>* pc);
00095 int complement(void);
00096
00097
00098 int get_largest_pcl(pcl::PointCloud<pcl::PointXYZ>* pc_out);
00099 int get_first_pcl(pcl::PointCloud<pcl::PointXYZ>* pc_out);
00100 int get_all_pcls(std::vector<pcl::PointCloud<pcl::PointXYZ> >* pc_out_vector);
00101
00102 int print(void);
00103
00104 protected:
00105 PBO_Polygon_set_2 S;
00106 PBO_Polygon_2 from_pcl_to_cgalpolygon(pcl::PointCloud<pcl::PointXYZ>* pc);
00107
00108
00109 template<class Kernel, class Container>
00110 void print_polygon (const CGAL::Polygon_2<Kernel, Container>& P)
00111 {
00112 typename CGAL::Polygon_2<Kernel, Container>::Vertex_const_iterator vit;
00113
00114 std::cout << "[ " << P.size() << " vertices:" << std::endl;
00115 int k=1;
00116 for (vit = P.vertices_begin(); vit != P.vertices_end(); ++vit)
00117 {
00118 std::cout << "x(" << k << ")=" << (*vit).x() << "; y(" << k << ")=" << (*vit).y()<< "; ";
00119 k++;
00120 }
00121 std::cout << std::endl;
00122 }
00123
00124 template<class Kernel, class Container>
00125 void print_polygon_with_holes(const CGAL::Polygon_with_holes_2<Kernel, Container> & pwh)
00126 {
00127 if (! pwh.is_unbounded()) {
00128 std::cout << "{ is_simple=" << pwh.outer_boundary().is_simple() << "Outer boundary = ";
00129 print_polygon (pwh.outer_boundary());
00130 }
00131 else
00132 std::cout << "{ Unbounded polygon." << std::endl;
00133
00134 typename CGAL::Polygon_with_holes_2<Kernel,Container>::Hole_const_iterator hit;
00135 unsigned int k = 1;
00136
00137 std::cout << " " << pwh.number_of_holes() << " holes:" << std::endl;
00138 for (hit = pwh.holes_begin(); hit != pwh.holes_end(); ++hit, ++k) {
00139 std::cout << "is_simple=" << (*hit).is_simple() <<" Hole #" << k << " = ";
00140 print_polygon (*hit);
00141 }
00142 std::cout << " }" << std::endl;
00143 }
00144 };
00145
00146
00147 #endif
00148