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_with_texture_H_
00037 #define _polygon_primitive_with_texture_H_
00038
00039 #ifndef PFLN
00040 #define PFLN {printf("DEBUG PRINT FILE %s LINE %d\n",__FILE__,__LINE__);}
00041 #endif
00042
00045
00046 #include "polygon_primitive.h"
00047 #include "camera_projection.h"
00048 #include "constrained_delaunay_triangulation.h"
00049 #include "textured_triangle.h"
00050 #include <colormap/colormap.h>
00051 #include <pthread.h>
00052
00053
00054 class c_polygon_primitive_with_texture: public c_polygon_primitive
00055 {
00056
00057
00058
00059 public:
00060
00061 c_polygon_primitive_with_texture(ros::NodeHandle* n): c_polygon_primitive(n)
00062 {
00063 id_visited_faces=0; ns_visited_faces = "visited_faces";
00064 id_vertices_indices=0; ns_vertices_indices = "vertices_indices";
00065 id_edge0=0; ns_edge0 = "edge0";
00066 id_edge1=0; ns_edge1 = "edge1";
00067 id_edge2=0; ns_edge2 = "edge2";
00068 id_next_triangle=0; ns_next_triangle = "next_triangle";
00069 id_proveniences=0; ns_proveniences = "proveniences";
00070 id_constraints=0; ns_constraints = "constraints";
00071 id_local_mesh=0; ns_local_mesh = "local_mesh";
00072 id_camera_canvas=0; ns_camera_canvas = "camera_canvas";
00073 id_camera_position=0; ns_camera_position = "camera_position";
00074 id_projection_name=0; ns_projection_name = "projection_name";
00075 id_camera_intersection=0; ns_camera_intersection="camera_intersection";
00076 id_camera_intersection_vertices=0; ns_camera_intersection_vertices="camera_intersection_vertices";
00077 id_textured_triangles=0; ns_textured_triangles="textured_triangles";
00078 id_triangle_edges=0; ns_triangle_edges="triangle_edges";
00079 id_triangle_vertices=0; ns_triangle_vertices="triangle_vertices";
00080 id_projection_union=0; ns_projection_union="projection_union";
00081 id_projection_union_vertices=0; ns_projection_union_vertices="projection_union_vertices";
00082 std::string str="hot";
00083 colormap = (class_colormap*) new class_colormap(str,8, 1, false);
00084 mutex = PTHREAD_MUTEX_INITIALIZER;
00085 };
00086
00087
00088 int compute_mean_and_std(std::vector<float>* v, float* mean, float* std);
00089
00090 size_t ask_for_number(void);
00091 int publish_to_rviz(ros::Publisher* p_marker_pub);
00092 int build_global_texture_set(ros::Publisher* p_marker_pub);
00093 int build_global_mesh(ros::Publisher* p_marker_pub);
00094 int compute_projection_union(void);
00095 int readapt_to_new_plane(polygon_primitive_msg::polygon_primitive* new_plg);
00096 int erase_old_markers(visualization_msgs::MarkerArray* marker_vec, unsigned int from, unsigned int to, std::string namesp);
00097
00098 int add_camera_projection_to_triangle_mesh(int projection_index, ros::Publisher* p_marker_pub);
00099
00100 int add_camera_projection_known_camera(cv::Mat* m, ros::Time t, tf::StampedTransform tf, std::string cam_name, std::string projection_name, cv::Scalar color);
00101 int add_camera_projection(cv::Mat* m, ros::Time t, double fx, double fy, double cx, double cy, tf::StampedTransform tf, double k1, double k2, double p1, double p2, double k3, double sd_cx, double sd_cy, double param, double scale, std::string projection_name, cv::Scalar color);
00102
00103 int create_textures_vizualization_msg(visualization_msgs::MarkerArray* marker_vec, bool reset_id=false);
00104 int get_mesh_statistics(void);
00105
00106 class_constrained_delaunay_triangulation dp;
00107
00108
00109 std::vector<class_camera_projection> cp;
00110
00111 pcl::PointCloud<pcl::PointXYZ> next_triangle;
00112 class_texture_set ts;
00113 class_colormap* colormap;
00114 pthread_mutex_t mutex;
00115 pthread_t thread;
00116
00117
00118
00119
00120
00121
00122 private:
00123
00124
00125 unsigned int id_visited_faces;
00126 std::string ns_visited_faces;
00127 unsigned int id_vertices_indices;
00128 std::string ns_vertices_indices;
00129 unsigned int id_edge0;
00130 std::string ns_edge0;
00131 unsigned int id_edge1;
00132 std::string ns_edge1;
00133 unsigned int id_edge2;
00134 std::string ns_edge2;
00135 unsigned int id_next_triangle;
00136 std::string ns_next_triangle;
00137 unsigned int id_proveniences;
00138 std::string ns_proveniences;
00139 unsigned int id_constraints;
00140 std::string ns_constraints;
00141 unsigned int id_local_mesh;
00142 std::string ns_local_mesh;
00143 unsigned int id_camera_canvas;
00144 std::string ns_camera_canvas;
00145 unsigned int id_camera_position;
00146 std::string ns_camera_position;
00147 unsigned int id_projection_name;
00148 std::string ns_projection_name;
00149 unsigned int id_camera_intersection;
00150 std::string ns_camera_intersection;
00151 unsigned int id_camera_intersection_vertices;
00152 std::string ns_camera_intersection_vertices;
00153 unsigned int id_textured_triangles;
00154 std::string ns_textured_triangles;
00155 unsigned int id_triangle_edges;
00156 std::string ns_triangle_edges;
00157 unsigned int id_triangle_vertices;
00158 std::string ns_triangle_vertices;
00159 unsigned int id_projection_union;
00160 std::string ns_projection_union;
00161 unsigned int id_projection_union_vertices;
00162 std::string ns_projection_union_vertices;
00163
00164 unsigned int id_start;
00165
00166 };
00167
00168
00169
00170 #endif
00171