polygon_primitives_extraction: /home/laradmin/lar/perception/3D/polygon_primitives_extraction/src/polygon_primitive_with_texture

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_with_texture_rviz_CPP_
00037 #define _polygon_primitive_with_texture_rviz_CPP_
00038 
00039 
00040 #include "polygon_primitive_with_texture.h"
00041 
00042 int c_polygon_primitive_with_texture::erase_old_markers(visualization_msgs::MarkerArray* marker_vec, unsigned int from, unsigned int to, std::string namesp)
00043 {
00044         for (unsigned int j=from; j<to; j++) //erase old markers
00045         {
00046                 visualization_msgs::Marker msg; //declare the msg 
00047                 msg.header.frame_id = data.frames.global_name;
00048                 msg.header.stamp = ros::Time::now();
00049                 msg.ns = namesp.c_str();
00050                 msg.id = j;
00051                 msg.action = visualization_msgs::Marker::DELETE;
00052                 marker_vec->markers.push_back(msg);
00053         }       
00054         return 1;
00055 }
00056 
00057 int c_polygon_primitive_with_texture::create_textures_vizualization_msg(visualization_msgs::MarkerArray* marker_vec, bool reset_id)
00058 {
00059 
00060         //compute the id_start for this polygon based on its name
00061         std::string str;
00062         str = data.misc.name;
00063         str.erase(str.begin());
00064         id_start = atoi(str.c_str())*100; 
00065 
00066         unsigned int id_range;
00067 
00068 
00069         geometry_msgs::Point p;
00070         std_msgs::ColorRGBA color;
00071 
00072         if(2)
00073         {       
00074                 //Draw the camera positions as a ref system
00075                 id_range = id_camera_position;
00076                 id_camera_position=id_start;
00077                 for (size_t j=0; j<cp.size(); j++)
00078                 {
00079                         visualization_msgs::Marker msg; //declare the msg                                              
00080                         msg.header.frame_id = data.frames.global_name;
00081                         msg.header.stamp = ros::Time::now();
00082                         msg.ns = ns_camera_position;
00083                         msg.action = visualization_msgs::Marker::ADD;
00084                         msg.pose.orientation.w = 1.0;
00085                         msg.type = visualization_msgs::Marker::LINE_STRIP;
00086                         msg.id = id_camera_position++;
00087 
00088                         msg.scale.x = 0.05; 
00089                         msg.scale.y = 0.05; 
00090                         msg.scale.z = 0.05; 
00091                         msg.color = colormap->color(j);
00092                         msg.color.a = 1;
00093 
00094                         geometry_msgs::Point p_origin;
00095                         p_origin.x = (cp[j].camera_6dof_position.inverse().getOrigin()).x(); 
00096                         p_origin.y = (cp[j].camera_6dof_position.inverse().getOrigin()).y(); 
00097                         p_origin.z = (cp[j].camera_6dof_position.inverse().getOrigin()).z(); 
00098 
00099                         pcl::PointCloud<pcl::PointXYZ> pts_local;
00100                         pcl::PointCloud<pcl::PointXYZ> pts;
00101                         pcl::PointXYZ pt;
00102 
00103                         pt.x=0.5; pt.y=0; pt.z=0; pts_local.push_back(pt);
00104                         pt.x=0; pt.y=0.5; pt.z=0; pts_local.push_back(pt);
00105                         pt.x=0; pt.y=0; pt.z=0.5; pts_local.push_back(pt);
00106 
00107                         pcl_ros::transformPointCloud(pts_local, pts, cp[j].camera_6dof_position.inverse()); 
00108 
00109                         msg.points.push_back(p_origin);
00110                         p.x = pts.points[0].x; 
00111                         p.y = pts.points[0].y; 
00112                         p.z = pts.points[0].z; 
00113                         msg.points.push_back(p);
00114 
00115                         msg.points.push_back(p_origin);
00116                         p.x = pts.points[1].x; 
00117                         p.y = pts.points[1].y; 
00118                         p.z = pts.points[1].z; 
00119                         msg.points.push_back(p);
00120 
00121                         msg.points.push_back(p_origin);
00122                         p.x = pts.points[2].x; 
00123                         p.y = pts.points[2].y; 
00124                         p.z = pts.points[2].z; 
00125                         msg.points.push_back(p);
00126 
00127 
00128                         marker_vec->markers.push_back(msg);
00129                 }
00130                 erase_old_markers(marker_vec, id_camera_position, id_range,ns_camera_position);
00131         }
00132 
00133         if(2)
00134         {       
00135                 //Draw the projections names
00136                 id_range = id_projection_name;
00137                 id_projection_name=id_start;
00138                 for (size_t j=0; j<cp.size(); j++)
00139                 {
00140                         visualization_msgs::Marker msg; //declare the msg                                              
00141                         msg.header.frame_id = data.frames.global_name;
00142                         msg.header.stamp = ros::Time::now();
00143                         msg.ns = ns_projection_name;
00144                         msg.action = visualization_msgs::Marker::ADD;
00145                         msg.pose.orientation.w = 1.0;
00146                         msg.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
00147                         msg.id = id_projection_name++;
00148 
00149                         msg.scale.x = 1; 
00150                         msg.scale.y = 1; 
00151                         msg.scale.z = 0.6; 
00152                         msg.color = colormap->color(j);
00153                         msg.color.a = 1;
00154 
00155                         msg.pose.position.x = (cp[j].camera_6dof_position.inverse().getOrigin()).x(); 
00156                         msg.pose.position.y = (cp[j].camera_6dof_position.inverse().getOrigin()).y(); 
00157                         msg.pose.position.z = (cp[j].camera_6dof_position.inverse().getOrigin()).z(); 
00158                         msg.text = cp[j].projection_name; 
00159 
00160 
00161                         marker_vec->markers.push_back(msg);
00162                 }
00163                 erase_old_markers(marker_vec, id_projection_name, id_range,ns_projection_name);
00164         }
00165 
00166         if(2)
00167         {       
00168                 //Draw the camera projections
00169                 id_range = id_camera_canvas;
00170                 id_camera_canvas=id_start;
00171                 for (size_t j=0; j<cp.size(); j++)
00172                 {
00173                         visualization_msgs::Marker msg; //declare the msg                                              
00174                         msg.header.frame_id = data.frames.global_name;
00175                         msg.header.stamp = ros::Time::now();
00176                         msg.ns = ns_camera_canvas;
00177                         msg.action = visualization_msgs::Marker::ADD;
00178                         msg.pose.orientation.w = 1.0;
00179                         msg.type = visualization_msgs::Marker::LINE_STRIP;
00180                         msg.id = id_camera_canvas++;
00181 
00182                         msg.scale.x = 0.1; 
00183                         msg.color = colormap->color(j);
00184                         msg.color.a = 1;
00185 
00186                         for (size_t u=0; u< cp[j].vertex_canvas.points.size(); u++)
00187                         {
00188                                 p.x = cp[j].vertex_canvas.points[u].x; 
00189                                 p.y = cp[j].vertex_canvas.points[u].y; 
00190                                 p.z = cp[j].vertex_canvas.points[u].z; 
00191 
00192                                 if (!isnan(p.x)) msg.points.push_back(p);
00193                         }
00194 
00195                         p.x = cp[j].vertex_canvas.points[0].x; 
00196                         p.y = cp[j].vertex_canvas.points[0].y; 
00197                         p.z = cp[j].vertex_canvas.points[0].z; 
00198 
00199                         if (!isnan(p.x)) msg.points.push_back(p);
00200                         marker_vec->markers.push_back(msg);
00201                 }
00202                 erase_old_markers(marker_vec, id_camera_canvas, id_range,ns_camera_canvas);
00203         }
00204 
00205         if(2)
00206         {       
00207                 //Draw the camera projections polygon intersection
00208                 id_range = id_camera_intersection;
00209                 id_camera_intersection=id_start;
00210                 for (size_t j=0; j<cp.size(); j++)
00211                 {
00212                         visualization_msgs::Marker msg; //declare the msg                                              
00213                         msg.header.frame_id = data.frames.global_name;
00214                         msg.header.stamp = ros::Time::now();
00215                         msg.ns = ns_camera_intersection;
00216                         msg.action = visualization_msgs::Marker::ADD;
00217                         msg.pose.orientation.w = 1.0;
00218                         msg.type = visualization_msgs::Marker::LINE_STRIP;
00219                         msg.id = id_camera_intersection++;
00220 
00221                         msg.scale.x = 0.1; 
00222                         msg.color = colormap->color(j);
00223                         msg.color.a =1;
00224 
00225                         for (size_t u=0; u< cp[j].vertex_intersection.points.size(); u++)
00226                         {
00227                                 p.x = cp[j].vertex_intersection.points[u].x; 
00228                                 p.y = cp[j].vertex_intersection.points[u].y; 
00229                                 p.z = cp[j].vertex_intersection.points[u].z; 
00230 
00231                                 if (!isnan(p.x)) msg.points.push_back(p);
00232 
00233                         }
00234                         p.x = cp[j].vertex_intersection.points[0].x; 
00235                         p.y = cp[j].vertex_intersection.points[0].y; 
00236                         p.z = cp[j].vertex_intersection.points[0].z; 
00237 
00238                         if (!isnan(p.x)) msg.points.push_back(p);
00239                         marker_vec->markers.push_back(msg);
00240 
00241                 }
00242                 erase_old_markers(marker_vec, id_camera_intersection, id_range,ns_camera_intersection);
00243         }
00244 
00245         if(2)
00246         {       
00247                 //Draw the camera projections polygon intersection vertices
00248                 id_range = id_camera_intersection_vertices;
00249                 id_camera_intersection_vertices=id_start;
00250                 for (size_t j=0; j<cp.size(); j++)
00251                 {
00252                         visualization_msgs::Marker msg; //declare the msg                                              
00253                         msg.header.frame_id = data.frames.global_name;
00254                         msg.header.stamp = ros::Time::now();
00255                         msg.ns = ns_camera_intersection_vertices;
00256                         msg.action = visualization_msgs::Marker::ADD;
00257                         msg.pose.orientation.w = 1.0;
00258                         msg.type = visualization_msgs::Marker::POINTS;
00259                         msg.id = id_camera_intersection_vertices++;
00260 
00261                         msg.scale.x = 0.15; 
00262                         msg.scale.y = 0.15; 
00263                         msg.color = colormap->color(j);
00264                         msg.color.a =1;
00265 
00266                         for (size_t u=0; u< cp[j].vertex_intersection.points.size(); u++)
00267                         {
00268                                 p.x = cp[j].vertex_intersection.points[u].x; 
00269                                 p.y = cp[j].vertex_intersection.points[u].y; 
00270                                 p.z = cp[j].vertex_intersection.points[u].z; 
00271 
00272                                 if (!isnan(p.x)) msg.points.push_back(p);
00273                         }
00274 
00275                         if (!isnan(p.x)) msg.points.push_back(p);
00276                         marker_vec->markers.push_back(msg);
00277 
00278                 }
00279                 erase_old_markers(marker_vec, id_camera_intersection_vertices, id_range,ns_camera_intersection_vertices);
00280         }
00281 
00282         if(1)
00283         {       
00284                 //Draw the Triangles with color
00285                 id_range = id_textured_triangles;
00286                 id_textured_triangles=id_start;
00287                 if (dp.pc.points.size()>=3)
00288                 {
00289 
00290                         visualization_msgs::Marker msg; //declare the msg                                              
00291                         msg.header.frame_id = data.frames.global_name;
00292                         msg.header.stamp = ros::Time::now();
00293                         msg.ns = ns_textured_triangles;
00294                         msg.action = visualization_msgs::Marker::ADD;
00295                         msg.type = visualization_msgs::Marker::TRIANGLE_LIST;
00296                         msg.id = id_textured_triangles++;
00297 
00298                         msg.pose.position.x = 0;
00299                         msg.pose.position.y = 0;
00300                         msg.pose.position.z = 0;
00301                         msg.pose.orientation.x = 0.0;
00302                         msg.pose.orientation.y = 0.0;
00303                         msg.pose.orientation.z = 0.0;
00304                         msg.pose.orientation.w = 1.0;
00305 
00306                         msg.scale.x = 1; 
00307                         msg.scale.y = 1; 
00308                         msg.scale.z = 1; 
00309                         msg.color.r = data.misc.color.r/255.;
00310                         msg.color.g = data.misc.color.g/255.;
00311                         msg.color.b = data.misc.color.b/255.; 
00312                         msg.color.a = 1;
00313                         for (int u=0; u<(int)dp.pc.points.size(); u++)
00314                         {
00315                                 p.x = dp.pc.points[u].x; 
00316                                 p.y = dp.pc.points[u].y; 
00317                                 p.z = dp.pc.points[u].z; 
00318                                 msg.points.push_back(p);
00319 
00320                                 uint32_t rgb = *reinterpret_cast<int*>(&dp.pc.points[u].rgb);
00321                                 uint8_t r = (rgb >> 16) & 0x0000ff;
00322                                 uint8_t g = (rgb >> 8)  & 0x0000ff;
00323                                 uint8_t b = (rgb)     & 0x0000ff;
00324 
00325                                 color.r = (float)r/255.0;
00326                                 color.g = (float)g/255.0;
00327                                 color.b = (float)b/255.0;
00328                                 color.a =1;
00329                                 msg.colors.push_back(color);
00330 
00331                         }
00332 
00333                         marker_vec->markers.push_back(msg);
00334                 }
00335                 erase_old_markers(marker_vec, id_textured_triangles, id_range,ns_textured_triangles);
00336         }
00337 
00338         if(2)
00339         {       
00340                 //Draw the Triangles edges
00341                 id_range = id_triangle_edges;
00342                 id_triangle_edges=id_start;
00343                 //if (ts.pc.points.size()>=3)
00344 
00345                 //printf("dp.pc.points.size()=%d\n",(int)dp.pc.points.size());
00346                 //printf("dp.pcproveniences.size()=%d\n",(int)dp.pc_proveniences.size());
00347                 if (dp.pc.points.size()>=3)
00348                 {
00349                         visualization_msgs::Marker msg; //declare the msg 
00350                         msg.header.frame_id = data.frames.global_name;
00351                         msg.header.stamp = ros::Time::now();
00352                         msg.ns = ns_triangle_edges;
00353                         msg.action = visualization_msgs::Marker::ADD;
00354                         msg.type = visualization_msgs::Marker::LINE_LIST;
00355                         msg.id = id_triangle_edges++;
00356 
00357                         msg.pose.position.x = 0;
00358                         msg.pose.position.y = 0;
00359                         msg.pose.position.z = 0;
00360                         msg.pose.orientation.x = 0.0;
00361                         msg.pose.orientation.y = 0.0;
00362                         msg.pose.orientation.z = 0.0;
00363                         msg.pose.orientation.w = 1.0;
00364 
00365                         msg.scale.x = 0.002; 
00366                         msg.scale.y = 1; 
00367                         msg.scale.z = 1; 
00368                         msg.color.r = 0/255.;
00369                         msg.color.g = 255/255.;
00370                         msg.color.b = 0/255.; 
00371                         msg.color.a = 0.6;
00372 
00373                         //for (int u=0; u<(int)ts.pc.points.size(); u+=3)
00374                         for (int u=0; u<(int)dp.pc.points.size(); u+=3)
00375                         {
00376                                 color = colormap->color(dp.pc_proveniences[u]);
00377                                 p.x = dp.pc.points[u].x; 
00378                                 p.y = dp.pc.points[u].y; 
00379                                 p.z = dp.pc.points[u].z; 
00380                                 msg.points.push_back(p);
00381                                 msg.colors.push_back(color);
00382 
00383                                 p.x = dp.pc.points[u+1].x; 
00384                                 p.y = dp.pc.points[u+1].y; 
00385                                 p.z = dp.pc.points[u+1].z; 
00386                                 msg.points.push_back(p);
00387                                 msg.colors.push_back(color);
00388 
00389 
00390 
00391                                 p.x = dp.pc.points[u+1].x; 
00392                                 p.y = dp.pc.points[u+1].y; 
00393                                 p.z = dp.pc.points[u+1].z; 
00394                                 msg.points.push_back(p);
00395                                 msg.colors.push_back(color);
00396 
00397 
00398                                 p.x = dp.pc.points[u+2].x; 
00399                                 p.y = dp.pc.points[u+2].y; 
00400                                 p.z = dp.pc.points[u+2].z; 
00401                                 msg.points.push_back(p);
00402                                 msg.colors.push_back(color);
00403 
00404 
00405                                 p.x = dp.pc.points[u].x; 
00406                                 p.y = dp.pc.points[u].y; 
00407                                 p.z = dp.pc.points[u].z; 
00408                                 msg.points.push_back(p);
00409                                 msg.colors.push_back(color);
00410 
00411                                 p.x = dp.pc.points[u+2].x; 
00412                                 p.y = dp.pc.points[u+2].y; 
00413                                 p.z = dp.pc.points[u+2].z; 
00414                                 msg.points.push_back(p);
00415                                 msg.colors.push_back(color);
00416                         }
00417 
00418                         marker_vec->markers.push_back(msg);
00419 
00420                 }
00421                 erase_old_markers(marker_vec, id_triangle_edges, id_range,ns_triangle_edges);
00422         }
00423 
00424         if(2)
00425         {       
00426                 //Draw the Triangles proveniences with color
00427                 id_range = id_proveniences;
00428                 id_proveniences=id_start;
00429                 //if (ts.pc.points.size()>=3)
00430                 if (dp.pc.points.size()>=3)
00431                 {
00432                         visualization_msgs::Marker msg; //declare the msg                                              
00433                         msg.header.frame_id = data.frames.global_name;
00434                         msg.header.stamp = ros::Time::now();
00435                         msg.ns = ns_proveniences;
00436                         msg.action = visualization_msgs::Marker::ADD;
00437                         msg.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
00438 
00439                         msg.pose.orientation.x = 0.0;
00440                         msg.pose.orientation.y = 0.0;
00441                         msg.pose.orientation.z = 0.0;
00442                         msg.pose.orientation.w = 1.0;
00443                         msg.scale.z = 0.01; 
00444 
00445                         //printf("dp.pc_faces_visited.points.size()=%d\n",(int)dp.pc_faces_visited.points.size());
00446                         //printf("dp.pc.points.size()=%d\n",(int)dp.pc.points.size());
00447 
00448                         for (int u=0; u<(int)dp.pc.points.size(); u+=3)
00449                         {
00450                                 msg.id = id_proveniences++;
00451                                 msg.pose.position.x = (dp.pc.points[u].x + dp.pc.points[u+1].x + dp.pc.points[u+2].x)/3; ;
00452                                 msg.pose.position.y = (dp.pc.points[u].y + dp.pc.points[u+1].y + dp.pc.points[u+2].y)/3; ;
00453                                 msg.pose.position.z = (dp.pc.points[u].z + dp.pc.points[u+1].z + dp.pc.points[u+2].z)/3; ;
00454                                 msg.color = colormap->color(dp.pc_proveniences[u]);
00455                                 msg.color.a = 1;
00456 
00457                                 if (dp.pc_proveniences[u]<0)
00458                                 {
00459                                         msg.text="NA";  
00460                                 }
00461                                 else
00462                                 {
00463                                         char str[1024];
00464                                         sprintf(str, "%d", dp.pc_proveniences[u]);
00465                                         msg.text=str;   
00466                                 }
00467 
00468                                 marker_vec->markers.push_back(msg);
00469                         }
00470                 }
00471                 erase_old_markers(marker_vec, id_proveniences, id_range,ns_proveniences);
00472         }
00473 
00474         if(2)
00475         {       
00476                 //Draw the Triangles vertices 
00477                 //id_range = id_triangle_vertices;
00478                 //id_triangle_vertices=id_start;
00479                 //if (ts.pc.points.size()>=3)
00480                 //if (dp.pc.points.size()>=3)
00481                 //{
00482                 //visualization_msgs::Marker msg; //declare the msg 
00483                 //msg.header.frame_id = data.frames.global_name;
00484                 //msg.header.stamp = ros::Time::now();
00485                 //msg.ns = ns_triangle_vertices;
00486                 //msg.action = visualization_msgs::Marker::ADD;
00487                 //msg.type = visualization_msgs::Marker::POINTS;
00488                 //msg.id = id_triangle_vertices++;
00489 
00490                 //msg.pose.position.x = 0;
00491                 //msg.pose.position.y = 0;
00492                 //msg.pose.position.z = 0;
00493                 //msg.pose.orientation.x = 0.0;
00494                 //msg.pose.orientation.y = 0.0;
00495                 //msg.pose.orientation.z = 0.0;
00496                 //msg.pose.orientation.w = 1.0;
00497 
00498                 //msg.scale.x = 0.05; 
00499                 //msg.scale.y = 0.05; 
00500                 //msg.scale.z = 1; 
00501                 //msg.color.r = 1.;
00502                 //msg.color.g = 0;
00503                 //msg.color.b = 1;
00504                 //msg.color.a = 1;
00505 
00512 
00517 
00518                 //for (int u=0; u<(int)dp.pc.points.size(); u++)
00519                 //{
00520                 //p.x = dp.pc.points[u].x; 
00521                 //p.y = dp.pc.points[u].y; 
00522                 //p.z = dp.pc.points[u].z; 
00523                 //msg.points.push_back(p);
00524 
00525                 //std_msgs::ColorRGBA color1 = colormap->color(dp.pc_proveniences[u]);
00526                 //color1.a = 0.5;
00527                 //msg.colors.push_back(color1);
00528                 //}
00529 
00530                 //marker_vec->markers.push_back(msg);
00531 
00532                 //}
00533                 //erase_old_markers(marker_vec, id_triangle_vertices, id_range,ns_triangle_vertices);
00534         }
00535 
00536         if(2)
00537         {       
00538                 //Draw the projection union
00539                 id_range = id_projection_union;
00540                 id_projection_union=id_start;
00541                 for (size_t j=0; j<dp.projection_union.size(); j++)
00542                 {
00543                         visualization_msgs::Marker msg; //declare the msg 
00544                         msg.header.frame_id = data.frames.global_name;
00545                         msg.header.stamp = ros::Time::now();
00546                         msg.ns = ns_projection_union;
00547                         msg.action = visualization_msgs::Marker::ADD;
00548                         msg.pose.orientation.w = 1.0;
00549                         msg.type = visualization_msgs::Marker::LINE_STRIP;
00550                         msg.id = id_projection_union++;
00551 
00552                         msg.scale.x = 0.2; 
00553                         msg.color.r = 1.;
00554                         msg.color.g = 0;
00555                         msg.color.b = 1;
00556                         msg.color.a = 0.7;
00557 
00558                         for (size_t u=0; u< dp.projection_union[j].points.size(); u++)
00559                         {
00560                                 p.x = dp.projection_union[j].points[u].x; 
00561                                 p.y = dp.projection_union[j].points[u].y; 
00562                                 p.z = dp.projection_union[j].points[u].z; 
00563 
00564                                 if (!isnan(p.x)) msg.points.push_back(p);
00565 
00566                         }
00567 
00568                         p.x = dp.projection_union[j].points[0].x; 
00569                         p.y = dp.projection_union[j].points[0].y; 
00570                         p.z = dp.projection_union[j].points[0].z; 
00571 
00572                         if (!isnan(p.x)) msg.points.push_back(p);
00573                         marker_vec->markers.push_back(msg);
00574 
00575                 }
00576                 erase_old_markers(marker_vec, id_projection_union, id_range,ns_projection_union);
00577         }
00578 
00579         if(2)
00580         {       
00581                 //Draw the projection union vertices
00582                 id_range = id_projection_union_vertices;
00583                 id_projection_union_vertices=id_start;
00584                 for (size_t j=0; j<dp.projection_union.size(); j++)
00585                 {
00586                         visualization_msgs::Marker msg; //declare the msg 
00587                         msg.header.frame_id = data.frames.global_name;
00588                         msg.header.stamp = ros::Time::now();
00589                         msg.ns = ns_projection_union_vertices;
00590                         msg.action = visualization_msgs::Marker::ADD;
00591                         msg.pose.orientation.w = 1.0;
00592                         msg.type = visualization_msgs::Marker::POINTS;
00593                         msg.id = id_projection_union_vertices++;
00594 
00595                         msg.scale.x = 0.3; 
00596                         msg.scale.y = 0.3; 
00597                         msg.scale.z = 1; 
00598                         msg.color.r = 1.;
00599                         msg.color.g = 0;
00600                         msg.color.b = 1;
00601                         msg.color.a = 0.7;
00602 
00603                         for (size_t u=0; u< dp.projection_union[j].points.size(); u++)
00604                         {
00605                                 p.x = dp.projection_union[j].points[u].x; 
00606                                 p.y = dp.projection_union[j].points[u].y; 
00607                                 p.z = dp.projection_union[j].points[u].z; 
00608 
00609                                 if (!isnan(p.x)) msg.points.push_back(p);
00610 
00611                         }
00612 
00613                         if (!isnan(p.x)) msg.points.push_back(p);
00614                         marker_vec->markers.push_back(msg);
00615 
00616                 }
00617                 erase_old_markers(marker_vec, id_projection_union_vertices, id_range,ns_projection_union_vertices);
00618 
00619                 //Draw the constraints
00620                 id_range = id_constraints;
00621                 id_constraints=id_start;
00622                 //ROS_WARN("size of pc_constraints=%d", (int)dp.pc_constraints.points.size());
00623                 if(dp.pc_constraints.points.size()>1) 
00624                 {
00625                         visualization_msgs::Marker msg; //declare the msg 
00626                         msg.header.frame_id = data.frames.global_name;
00627                         msg.header.stamp = ros::Time::now();
00628                         msg.ns = ns_constraints;
00629                         msg.action = visualization_msgs::Marker::ADD;
00630                         msg.pose.orientation.w = 1.0;
00631                         msg.type = visualization_msgs::Marker::LINE_LIST;
00632                         msg.id = id_constraints++;
00633 
00634                         msg.scale.x = 0.01; 
00635                         msg.color.r = 1.;
00636                         msg.color.g = 0;
00637                         msg.color.b = 0;
00638                         msg.color.a = 0.8;
00639 
00640                         for (size_t u=0; u< dp.pc_constraints.points.size(); u+=2)
00641                         {
00642                                 p.x = (dp.pc_constraints.points[u+1].x - dp.pc_constraints.points[u].x); 
00643                                 p.y = (dp.pc_constraints.points[u+1].y - dp.pc_constraints.points[u].y); 
00644                                 p.z = (dp.pc_constraints.points[u+1].z - dp.pc_constraints.points[u].z); 
00645 
00646 
00647                                 geometry_msgs::Point p1,p2;
00648 
00649                                 p1.x = dp.pc_constraints.points[u].x + 0.4*p.x; p1.y = dp.pc_constraints.points[u].y + 0.4*p.y; p1.z = dp.pc_constraints.points[u].z + 0.4*p.z;
00650                                 p2.x = dp.pc_constraints.points[u].x + 0.6*p.x; p2.y = dp.pc_constraints.points[u].y + 0.6*p.y; p2.z = dp.pc_constraints.points[u].z + 0.6*p.z;
00651 
00652                                 if (!isnan(p.x)) msg.points.push_back(p1);
00653                                 if (!isnan(p.x)) msg.points.push_back(p2);
00654                         }
00655                         marker_vec->markers.push_back(msg);
00656 
00657                 }
00658                 erase_old_markers(marker_vec, id_constraints, id_range,ns_constraints);
00659         }
00660 
00661         if(2)
00662         {       
00663                 //Draw the local meshes
00664                 //id_range = id_local_mesh;
00665                 //id_local_mesh=id_start;
00666                 //for (size_t j=0; j<cp.size(); j++)
00667                 //{
00668                 //visualization_msgs::Marker msg; //declare the msg 
00669                 //msg.header.frame_id = data.frames.global_name;
00670                 //msg.header.stamp = ros::Time::now();
00671                 //msg.ns = ns_local_mesh;
00672                 //msg.action = visualization_msgs::Marker::ADD;
00673                 //msg.type = visualization_msgs::Marker::LINE_LIST;
00674                 //msg.id = id_local_mesh++;
00675 
00676                 //msg.pose.orientation.w = 1.0;
00677 
00678                 //msg.scale.x = 0.01; 
00679                 //msg.scale.y = 1; 
00680                 //msg.scale.z = 1; 
00681                 //msg.color = colormap->color(j);
00682                 //msg.color.a = 0.5;
00683 
00684                 //for (int u=0; u<(int)cp[j].vertex_list.points.size(); u+=3)
00685                 //{
00686                 //if (isnan(cp[j].vertex_list.points[u].x) || isnan(cp[j].vertex_list.points[u+1].x) || isnan(cp[j].vertex_list.points[u+2].x) ||
00687                 //isnan(cp[j].vertex_list.points[u].y) || isnan(cp[j].vertex_list.points[u+1].y) || isnan(cp[j].vertex_list.points[u+2].y) ||
00688                 //isnan(cp[j].vertex_list.points[u].z) || isnan(cp[j].vertex_list.points[u+1].z) || isnan(cp[j].vertex_list.points[u+2].z))
00689                 //continue;
00690 
00691                 //p.x = cp[j].vertex_list.points[u].x; 
00692                 //p.y = cp[j].vertex_list.points[u].y; 
00693                 //p.z = cp[j].vertex_list.points[u].z; 
00694                 //msg.points.push_back(p);
00695 
00696                 //p.x = cp[j].vertex_list.points[u+1].x; 
00697                 //p.y = cp[j].vertex_list.points[u+1].y; 
00698                 //p.z = cp[j].vertex_list.points[u+1].z; 
00699                 //msg.points.push_back(p);
00700 
00701                 //p.x = cp[j].vertex_list.points[u].x; 
00702                 //p.y = cp[j].vertex_list.points[u].y; 
00703                 //p.z = cp[j].vertex_list.points[u].z; 
00704                 //msg.points.push_back(p);
00705 
00706                 //p.x = cp[j].vertex_list.points[u+2].x; 
00707                 //p.y = cp[j].vertex_list.points[u+2].y; 
00708                 //p.z = cp[j].vertex_list.points[u+2].z; 
00709                 //msg.points.push_back(p);
00710 
00711                 //p.x = cp[j].vertex_list.points[u+1].x; 
00712                 //p.y = cp[j].vertex_list.points[u+1].y; 
00713                 //p.z = cp[j].vertex_list.points[u+1].z; 
00714                 //msg.points.push_back(p);
00715 
00716                 //p.x = cp[j].vertex_list.points[u+2].x; 
00717                 //p.y = cp[j].vertex_list.points[u+2].y; 
00718                 //p.z = cp[j].vertex_list.points[u+2].z; 
00719                 //msg.points.push_back(p);
00720 
00721                 //}
00722 
00723                 //ROS_INFO("Projection %d Number of points = %d", (int)j, (int)cp[j].vertex_list.points.size());
00724                 //marker_vec->markers.push_back(msg);
00725 
00726                 //}
00727                 //erase_old_markers(marker_vec, id_local_mesh, id_range,ns_local_mesh);
00728         }
00729 
00730         if(2)
00731         {       
00732                 //Draw the next triangle
00733                 id_range = id_next_triangle;
00734                 id_next_triangle=id_start;
00735                 //if (ts.pc.points.size()>=3)
00736                 if (next_triangle.points.size()==6)
00737                 {
00738                         visualization_msgs::Marker msg; //declare the msg 
00739                         msg.header.frame_id = data.frames.global_name;
00740                         msg.header.stamp = ros::Time::now();
00741                         msg.ns = ns_next_triangle;
00742                         msg.action = visualization_msgs::Marker::ADD;
00743                         msg.type = visualization_msgs::Marker::LINE_LIST;
00744                         msg.id = id_next_triangle++;
00745 
00746                         msg.pose.position.x = 0;
00747                         msg.pose.position.y = 0;
00748                         msg.pose.position.z = 0;
00749                         msg.pose.orientation.x = 0.0;
00750                         msg.pose.orientation.y = 0.0;
00751                         msg.pose.orientation.z = 0.0;
00752                         msg.pose.orientation.w = 1.0;
00753 
00754                         msg.scale.x = 0.01; 
00755                         msg.scale.y = 1; 
00756                         msg.scale.z = 1; 
00757                         msg.color.r = 0;
00758                         msg.color.g = 1;
00759                         msg.color.b = 0; 
00760                         msg.color.a = 0.8;
00761 
00762                         for (int u=0; u<(int)next_triangle.points.size(); u++)
00763                         {
00764                                 p.x = next_triangle.points[u].x; 
00765                                 p.y = next_triangle.points[u].y; 
00766                                 p.z = next_triangle.points[u].z; 
00767                                 msg.points.push_back(p);
00768                         }
00769                         marker_vec->markers.push_back(msg);
00770                 }
00771                 erase_old_markers(marker_vec, id_next_triangle, id_range,ns_next_triangle);
00772         }
00773 
00774         if(2)
00775         {       
00776                 //Draw the edge 0
00777                 id_range = id_edge0;
00778                 id_edge0=id_start;
00779                 if (next_triangle.points.size()==6)
00780                 {
00781                         visualization_msgs::Marker msg; //declare the msg 
00782                         msg.header.frame_id = data.frames.global_name;
00783                         msg.header.stamp = ros::Time::now();
00784                         msg.ns = ns_edge0;
00785                         msg.action = visualization_msgs::Marker::ADD;
00786                         msg.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
00787                         msg.id = id_edge0++;
00788 
00789                         msg.pose.position.x = next_triangle.points[0].x + 0.05;
00790                         msg.pose.position.y = next_triangle.points[0].y;
00791                         msg.pose.position.z = next_triangle.points[0].z;
00792                         msg.pose.orientation.x = 0.0;
00793                         msg.pose.orientation.y = 0.0;
00794                         msg.pose.orientation.z = 0.0;
00795                         msg.pose.orientation.w = 1.0;
00796                         msg.scale.z = 0.05; 
00797                         msg.color.r = 0; msg.color.g = 1; msg.color.b = 0; msg.color.a = 0.8;
00798                         msg.text = "0";
00799 
00800                         marker_vec->markers.push_back(msg);
00801                 }
00802                 erase_old_markers(marker_vec, id_edge0, id_range,ns_edge0);
00803         }
00804 
00805         if(2)
00806         {       
00807                 //Draw the edge 1
00808                 id_range = id_edge1;
00809                 id_edge1=id_start;
00810                 if (next_triangle.points.size()==6)
00811                 {
00812                         visualization_msgs::Marker msg; //declare the msg 
00813                         msg.header.frame_id = data.frames.global_name;
00814                         msg.header.stamp = ros::Time::now();
00815                         msg.ns = ns_edge1;
00816                         msg.action = visualization_msgs::Marker::ADD;
00817                         msg.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
00818                         msg.id = id_edge1++;
00819 
00820                         msg.pose.position.x = next_triangle.points[1].x + 0.05;
00821                         msg.pose.position.y = next_triangle.points[1].y;
00822                         msg.pose.position.z = next_triangle.points[1].z;
00823                         msg.pose.orientation.x = 0.0;
00824                         msg.pose.orientation.y = 0.0;
00825                         msg.pose.orientation.z = 0.0;
00826                         msg.pose.orientation.w = 1.0;
00827                         msg.scale.z = 0.05; 
00828                         msg.color.r = 0; msg.color.g = 1; msg.color.b = 0; msg.color.a = 0.8;
00829                         msg.text = "1";
00830 
00831                         marker_vec->markers.push_back(msg);
00832                 }
00833                 erase_old_markers(marker_vec, id_edge1, id_range,ns_edge1);
00834         }
00835 
00836         if(2)
00837         {       
00838                 //Draw the edge 2
00839                 id_range = id_edge2;
00840                 id_edge2=id_start;
00841                 if (next_triangle.points.size()==6)
00842                 {
00843                         visualization_msgs::Marker msg; //declare the msg 
00844                         msg.header.frame_id = data.frames.global_name;
00845                         msg.header.stamp = ros::Time::now();
00846                         msg.ns = ns_edge2;
00847                         msg.action = visualization_msgs::Marker::ADD;
00848                         msg.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
00849                         msg.id = id_edge2++;
00850 
00851                         msg.pose.position.x = next_triangle.points[5].x + 0.05;
00852                         msg.pose.position.y = next_triangle.points[5].y;
00853                         msg.pose.position.z = next_triangle.points[5].z;
00854                         msg.pose.orientation.x = 0.0;
00855                         msg.pose.orientation.y = 0.0;
00856                         msg.pose.orientation.z = 0.0;
00857                         msg.pose.orientation.w = 1.0;
00858                         msg.scale.z = 0.05; 
00859                         msg.color.r = 0; msg.color.g = 1; msg.color.b = 0; msg.color.a = 0.8;
00860                         msg.text = "2";
00861 
00862                         marker_vec->markers.push_back(msg);
00863                 }
00864                 erase_old_markers(marker_vec, id_edge2, id_range,ns_edge2);
00865         }
00866 
00867         if(2)
00868         {       
00869                 //vertices_indices 
00870                 id_range = id_vertices_indices;
00871                 id_vertices_indices=id_start;
00872                 for (size_t i=0; i<dp.pc_vertices.size(); i++)
00873                 {
00874                         visualization_msgs::Marker msg; //declare the msg 
00875                         msg.header.frame_id = data.frames.global_name;
00876                         msg.header.stamp = ros::Time::now();
00877                         msg.ns = ns_vertices_indices;
00878                         msg.action = visualization_msgs::Marker::ADD;
00879                         msg.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
00880                         msg.id = id_vertices_indices++;
00881                         msg.scale.z = 0.01; 
00882                         msg.color.r = 1; msg.color.g = 0; msg.color.b = 0; msg.color.a = 1;
00883                         msg.pose.orientation.x = 0.0;
00884                         msg.pose.orientation.y = 0.0;
00885                         msg.pose.orientation.z = 0.0;
00886                         msg.pose.orientation.w = 1.0;
00887 
00888                         msg.pose.position.x = dp.pc_vertices.points[i].x;
00889                         msg.pose.position.y = dp.pc_vertices.points[i].y;
00890                         msg.pose.position.z = dp.pc_vertices.points[i].z;
00891                         char str[1024];
00892                         sprintf(str, "%d",(int)dp.pc_vertices_indices[i]);
00893                         msg.text = str;
00894                         marker_vec->markers.push_back(msg);
00895                 }
00896                 erase_old_markers(marker_vec, id_vertices_indices, id_range,ns_vertices_indices);
00897         }
00898 
00899         if(2)
00900         {       
00901                 //visited_faces 
00902                 id_range = id_visited_faces;
00903                 id_visited_faces=id_start;
00904                 for (size_t i=0; i<dp.pc_faces_visited.points.size(); i++)
00905                 {
00906                         visualization_msgs::Marker msg; //declare the msg 
00907                         msg.header.frame_id = data.frames.global_name;
00908                         msg.header.stamp = ros::Time::now();
00909                         msg.ns = ns_visited_faces;
00910                         msg.action = visualization_msgs::Marker::ADD;
00911                         msg.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
00912                         msg.id = id_visited_faces++;
00913                         msg.scale.z = 0.01; 
00914                         msg.color.r = 0/255.;
00915                         msg.color.g = 128/255.;
00916                         msg.color.b = 128/255.; 
00917                         msg.color.a = 0.8;
00918                         msg.pose.orientation.x = 0.0;
00919                         msg.pose.orientation.y = 0.0;
00920                         msg.pose.orientation.z = 0.0;
00921                         msg.pose.orientation.w = 1.0;
00922 
00923                         msg.pose.position.x = dp.pc_faces_visited.points[i].x;
00924                         msg.pose.position.y = dp.pc_faces_visited.points[i].y;
00925                         msg.pose.position.z = dp.pc_faces_visited.points[i].z;
00926                         msg.text = "__V";
00927                         marker_vec->markers.push_back(msg);
00928                 }
00929                 erase_old_markers(marker_vec, id_visited_faces, id_range,ns_visited_faces);
00930         }
00931         return 1;
00932 }
00933 
00934 
00935 
00936 
00937 
00938 
00939 #endif
00940
/home/laradmin/lar/perception/3D/polygon_primitives_extraction/src/polygon_primitive_with_texture_rviz.cpp
