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
00033 #ifndef _projection_mesh_H_
00034 #define _projection_mesh_H_
00035
00036
00037
00038
00039
00040
00041 #include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
00042 #include <CGAL/IO/Color.h>
00043 #include <CGAL/Triangulation_2.h>
00044 #include <CGAL/Triangulation_vertex_base_with_info_2.h>
00045
00046 #include <CGAL/Polygon_2_algorithms.h>
00047 #include <CGAL/Delaunay_triangulation_2.h>
00048
00049 #include <CGAL/basic.h>
00050 #include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
00051 #include <CGAL/Exact_predicates_exact_constructions_kernel.h>
00052 #include <CGAL/Partition_traits_2.h>
00053 #include <CGAL/Partition_is_valid_traits_2.h>
00054 #include <CGAL/polygon_function_objects.h>
00055 #include <CGAL/partition_2.h>
00056 #include <CGAL/point_generators_2.h>
00057 #include <CGAL/Polygon_set_2.h>
00058 #include <CGAL/Cartesian.h>
00059 #include <CGAL/polygon_function_objects.h>
00060 #include <CGAL/random_polygon_2.h>
00061
00062 #include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
00063 #include <CGAL/Constrained_Delaunay_triangulation_2.h>
00064
00065 #include <pcl/segmentation/sac_segmentation.h>
00066 #include <pcl/filters/project_inliers.h>
00067 #include <pcl/point_cloud.h>
00068 #include <pcl/point_types.h>
00069 #include <pcl_ros/transforms.h>
00070 #include <pcl/ModelCoefficients.h>
00071 #include <tf/tf.h>
00072 #include <visualization_msgs/Marker.h>
00073
00074 #include <CGAL/Constrained_Delaunay_triangulation_2.h>
00075 #include <CGAL/Constrained_triangulation_plus_2.h>
00076
00077 #include <opencv2/imgproc/imgproc.hpp>
00078 #include <opencv2/highgui/highgui.hpp>
00079 #include <opencv2/core/core.hpp>
00080
00081
00082
00083 typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
00084
00085
00086 typedef struct {double z;float rgb;float weight;} t_vertex_extension;
00087
00088
00089 template <class Gt, class Fb >
00090 class face_extension : public Fb
00091 {
00092 public:
00093 typedef Gt Geom_traits;
00094 typedef typename Fb::Vertex_handle Vertex_handle;
00095 typedef typename Fb::Face_handle Face_handle;
00096
00097 template < typename TDS2 >
00098 struct Rebind_TDS
00099 {
00100 typedef typename Fb::template Rebind_TDS<TDS2>::Other Fb2;
00101 typedef face_extension<Gt,Fb2> Other;
00102 };
00103
00104 int status;
00105 int counter;
00106 bool visited;
00107
00108 float weight;
00109 inline bool is_in_domain() const { return (status%2 == 1); };
00110
00111 inline void set_in_domain(const bool b) { status = (b ? 1 : 0); };
00112
00113
00114 face_extension(): Fb(), status(-1) {};
00115
00116 face_extension(Vertex_handle v0,
00117 Vertex_handle v1,
00118 Vertex_handle v2): Fb(v0,v1,v2), status(-1) {};
00119
00120 face_extension(Vertex_handle v0,
00121 Vertex_handle v1,
00122 Vertex_handle v2,
00123 Face_handle n0,
00124 Face_handle n1,
00125 Face_handle n2): Fb(v0,v1,v2,n0,n1,n2), status(-1) {};
00126
00127 };
00128
00129
00130
00131
00132
00133
00134
00135
00136 typedef CGAL::Triangulation_data_structure_2<CGAL::Triangulation_vertex_base_with_info_2<t_vertex_extension, K>,
00137 face_extension<K, CGAL::Constrained_triangulation_face_base_2<K> > > TDS_projection_mesh;
00138
00139
00140 typedef CGAL::Constrained_triangulation_plus_2<CGAL::Constrained_Delaunay_triangulation_2<K, TDS_projection_mesh, CGAL::No_intersection_tag> > PT;
00141
00142
00143
00144
00145 class class_projection_mesh
00146 {
00147 public:
00148 class_projection_mesh(void)
00149 {
00150 };
00151 ~class_projection_mesh(void){};
00152
00153 int draw_mesh_triangles(cv::Mat* image, cv::Scalar color, int thickness);
00154 int print_all_vertices(void);
00155 int clear_constraints(void);
00156 int clear_vertices(void);
00157 int add_vertex_to_mesh(double x, double y, double z, float rgb, float weight);
00158 int set_constraint_polygon(std::vector<pcl::PointXY>* p);
00159 int export_triangles(pcl::PointCloud<pcl::PointXYZRGB>* vertex_list, std::vector<float>* face_weights);
00160 int export_triangles_in_order(pcl::PointCloud<pcl::PointXYZRGB>* vertex_list, std::vector<float>* face_weights);
00161 int compute_face_weights(void);
00162 int initialize_visited(void)
00163 {
00164 for(PT::All_faces_iterator it = mesh.all_faces_begin(); it != mesh.all_faces_end(); ++it)
00165 {
00166 it->visited=false;
00167 }
00168 return 1;
00169 }
00170
00171
00172 PT mesh;
00173
00174 void discoverComponents(const PT& ct);
00175 void discoverComponent(const PT& ct, PT::Face_handle start, int index, std::list<PT::Edge>& border );
00176 int initialize_all_faces_status(void);
00177 };
00178
00179
00180
00181 #endif
00182