dyn_model_exporter.cpp
Go to the documentation of this file.
00001 /******************************************************************************
00002  * \file
00003  *
00004  * $Id: DynModelExporter.cpp 814 2012-05-22 14:00:19Z ihulik $
00005  *
00006  * Copyright (C) Brno University of Technology
00007  *
00008  * This file is part of software developed by dcgm-robotics@FIT group.
00009  *
00010  * Author: Rostislav Hulik (ihulik@fit.vutbr.cz)
00011  * Supervised by: Michal Spanel (spanel@fit.vutbr.cz)
00012  * Date: 11.01.2012 (version 0.8)
00013  * 
00014  * This file is free software: you can redistribute it and/or modify
00015  * it under the terms of the GNU Lesser General Public License as published by
00016  * the Free Software Foundation, either version 3 of the License, or
00017  * (at your option) any later version.
00018  * 
00019  * This file is distributed in the hope that it will be useful,
00020  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00021  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00022  * GNU Lesser General Public License for more details.
00023  * 
00024  * You should have received a copy of the GNU Lesser General Public License
00025  * along with this file.  If not, see <http://www.gnu.org/licenses/>.
00026  */
00027 
00033 #include <srs_env_model_percp/but_plane_detector/dyn_model_exporter.h>
00034 #include <srs_env_model_percp/topics_list.h>
00035 #include <srs_env_model_percp/services_list.h>
00036 
00037 
00038 #include <srs_interaction_primitives/AddPlane.h>
00039 #include <srs_interaction_primitives/RemovePrimitive.h>
00040 #include <srs_interaction_primitives/plane.h>
00041 #include <pcl/filters/project_inliers.h>
00042 
00043 #include <srs_env_model/InsertPlanes.h>
00044 #include <pcl/point_cloud.h>
00045 
00046 #include <pcl/point_types.h>
00047 #include <pcl/io/pcd_io.h>
00048 #include <pcl/kdtree/kdtree_flann.h>
00049 #include <pcl/kdtree/kdtree.h>
00050 #include <pcl/features/normal_3d.h>
00051 #include <pcl/surface/gp3.h>
00052 #include <pcl_ros/transforms.h>
00053 
00054 #include <libxml2/libxml/parser.h>
00055 #include <libxml2/libxml/tree.h>
00056 
00057 using namespace pcl;
00058 using namespace but_plane_detector;
00059 using namespace cv;
00060 
00061 namespace srs_env_model_percp
00062 {
00063 
00065         // Updates sent planes using but environment model server
00066         // @param planes Vector of found planes
00067         // @param scene_cloud point cloud of the scene
00069         void DynModelExporter::update(tPlanes & planes, Normals &normals, std::string color_method, cv::Mat rgb)
00070         {
00071                 if (m_keep_tracking == 0)
00072                         displayed_planes.clear();
00073                 std::vector<PointCloud<pcl::PointXYZ>, Eigen::aligned_allocator<PointCloud<pcl::PointXYZ> > > planesInPCloud(planes.size());
00074                 std::vector<std_msgs::ColorRGBA, Eigen::aligned_allocator<std_msgs::ColorRGBA> > colors(planes.size());
00075 
00076 
00077                 std::cerr << "updating..." << std::endl;
00078 
00079                 for (int i = 0; i < normals.m_points.rows; ++i)
00080                 for (int j = 0; j < normals.m_points.cols; ++j)
00081                 {
00082                         Vec3f point = normals.m_points.at<Vec3f>(i, j);
00083                         if (point[0] != 0.0 || point[1] != 0.0 || point[2] != 0.0)
00084                         {
00085                                 cv::Vec4f localPlane = normals.m_planes.at<cv::Vec4f>(i, j);
00086                                 Plane<float> aaa(localPlane[0], localPlane[1], localPlane[2], localPlane[3]);
00087 
00088                                 // find all planes that fit for this point
00089                                 for (unsigned int a = 0; a < planes.size(); ++a)
00090                                 {
00091                                         if (planes[a].distance(point) < m_max_distance && planes[a].isSimilar(aaa, m_max_plane_normal_dev, m_max_plane_shift_dev))
00092                                         {
00093                                                 PointXYZ pclpoint(point[0], point[1], point[2]);
00094                                                 planesInPCloud[a].push_back(pclpoint);
00095 
00096                                                 if (color_method == "mean_color")
00097                                                 {
00098                                                         cv::Vec<unsigned char, 3> color = rgb.at<cv::Vec<unsigned char, 3> >(i, j);
00099                                                         colors[a].r += (float)color[0]/255.0;
00100                                                         colors[a].g += (float)color[1]/255.0;
00101                                                         colors[a].b += (float)color[2]/255.0;
00102                                                 }
00103                                                 else if (color_method == "mean_color")
00104                                                 {
00105                                                         cv::Vec<unsigned char, 3> color = rgb.at<cv::Vec<unsigned char, 3> >(i, j);
00106                                                         colors[a].r += point[0] / 5.0;
00107                                                         colors[a].g += point[1] / 5.0;
00108                                                         colors[a].b += point[2] / 5.0;
00109                                                 }
00110                                         }
00111                                 }
00112                         }
00113                 }
00114 //              for (int i = 0; i < normals.m_points.rows; ++i)
00115 //              for (int j = 0; j < normals.m_points.cols; ++j)
00116 //              {
00117 //                      Vec3f point = normals.m_points.at<Vec3f>(i, j);
00118 //                      if (point[0] != 0.0 || point[1] != 0.0 || point[2] != 0.0)
00119 //                      {
00120 //                              cv::Vec4f localPlane = normals.m_planes.at<cv::Vec4f>(i, j);
00121 //                              Plane<float> aaa(localPlane[0], localPlane[1], localPlane[2], localPlane[3]);
00122 //
00123 //                              double dist = DBL_MAX;
00124 //
00125 //                              int chosen = -1;
00126 //                              // find the best plane
00127 //                              for (unsigned int a = 0; a < planes.size(); ++a)
00128 //                              {
00129 //                                      if (planes[a].distance(point) < dist && planes[a].distance(point) < m_max_distance &&
00130 //                                              planes[a].isSimilar(aaa, m_max_plane_normal_dev, m_max_plane_shift_dev))
00131 //                                      {
00132 //                                              dist = planes[a].distance(point);
00133 //                                              chosen = a;
00134 //                                      }
00135 //                              }
00136 //
00137 //                              // if there is good plane, insert point into point cloud
00138 //                              if (chosen > -1)
00139 //                              {
00140 //                                      PointXYZ pclpoint(point[0], point[1], point[2]);
00141 //                                      planesInPCloud[chosen].push_back(pclpoint);
00142 //
00143 //                                      if (color_method == "mean_color")
00144 //                                      {
00145 //                                              cv::Vec<unsigned char, 3> color = rgb.at<cv::Vec<unsigned char, 3> >(i, j);
00146 //                                              colors[chosen].r += (float)color[0]/255.0;
00147 //                                              colors[chosen].g += (float)color[1]/255.0;
00148 //                                              colors[chosen].b += (float)color[2]/255.0;
00149 //                                              //std::cerr << color[0] << " " << color[1] << " " << color[2] << std::endl;
00150 //                                      }
00151 //                                      else if (color_method == "mean_color")
00152 //                                      {
00153 //                                              cv::Vec<unsigned char, 3> color = rgb.at<cv::Vec<unsigned char, 3> >(i, j);
00154 //                                              colors[chosen].r += point[0] / 5.0;
00155 //                                              colors[chosen].g += point[1] / 5.0;
00156 //                                              colors[chosen].b += point[2] / 5.0;
00157 //                                                                                      //std::cerr << color[0] << " " << color[1] << " " << color[2] << std::endl;
00158 //                                      }
00164 //                              }
00165 //                      }
00166 //              }
00167 
00168                 if (color_method == "mean_color" || color_method == "centroid")
00169                 {
00170                         for (unsigned int i = 0; i < colors.size(); ++i)
00171                                 if(planesInPCloud.size() > 0)
00172                                 {
00173                                         colors[i].r /= planesInPCloud[i].size();
00174                                         colors[i].g /= planesInPCloud[i].size();
00175                                         colors[i].b /= planesInPCloud[i].size();
00176                                         colors[i].a = 1.0;
00177                                 }
00178                 }
00179                 // Indexed in point cloud
00181 
00182                 for (unsigned int j = 0; j < planesInPCloud.size(); ++j)
00183                 {
00184                         if (planesInPCloud[j].size() > 20)
00185                         {
00186                                 double maxangle = DBL_MAX;
00187                                 double maxdist = DBL_MAX;
00188                                 int index = -1;
00189                                 for (unsigned int i = 0; i < displayed_planes.size(); ++i)
00190                                 {
00191                                         if (!(displayed_planes[i].is_deleted))
00192                                         {
00193                                                 double angle = acos(((planes[j].a * displayed_planes[i].plane.a) + (planes[j].b * displayed_planes[i].plane.b) + (planes[j].c * displayed_planes[i].plane.c)));
00194                                                 double xd = planes[j].d - displayed_planes[i].plane.d;
00195                                                 xd = (xd > 0 ? xd : - xd);
00196 
00197                                                 // Pretty nasty workaround... todo
00198                                                 if (angle != angle) angle = 0.0;
00199 
00200                                                 if (angle <= maxangle  && xd <= maxdist && angle < m_max_plane_normal_dev  && xd < m_max_plane_shift_dev)
00201                                                 {
00202                                                         maxangle = angle;
00203                                                         maxdist = xd;
00204                                                         index = i;
00205                                                 }
00206                                         }
00207                                 }
00208 
00209                                 if (index >= 0)
00210                                 {
00211                                         std::cerr << "adding marker to concave hull..." << std::endl;
00212                                         DynModelExporter::addMarkerToConcaveHull(planesInPCloud[j], displayed_planes[index].plane);
00213                                         displayed_planes[index].update = ros::Time::now();
00214                                 }
00215                                 else
00216                                 {
00217                                         ExportedPlane newplane;
00218                                         newplane.update = ros::Time::now();
00219                                         newplane.plane = PlaneExt(planes[j]);
00220                                         newplane.is_deleted = false;
00221                                         newplane.to_be_deleted = false;
00222                                         DynModelExporter::createMarkerForConcaveHull(planesInPCloud[j], newplane.plane);
00223 
00224                                         newplane.id = displayed_planes.size();
00225                                         newplane.plane.getMeshMarker().id = newplane.id;
00226                                         if (color_method == "mean_color")
00227                                         {
00228                                                 std::cerr << "setting color: " << colors[j].r << " " << colors[j].g << " " << colors[j].b << std::endl;
00229                                                 newplane.plane.setColor(colors[j]);
00230                                         }
00231                                         else if (color_method == "random")
00232                                         {
00233                                                 colors[j].r = (float)rand()/INT_MAX * 0.5 + 0.2;
00234                                                 colors[j].g = (float)rand()/INT_MAX * 0.5 + 0.2;
00235                                                 colors[j].b = (float)rand()/INT_MAX * 0.5 + 0.2;
00236                                                 colors[j].a = 1.0;
00237                                                 newplane.plane.setColor(colors[j]);
00238                                         }
00239                                         //newplane.plane.getShapeMarker().id = newplane.id;
00240                                         displayed_planes.push_back(newplane);
00241                                 }
00242                         }
00243                 }
00244 
00245                 // TTL fix
00246                 if (m_plane_ttl > 0)
00247                 {
00248                         for (unsigned int i = 0; i < displayed_planes.size(); ++i)
00249                         {
00250                                 if (!displayed_planes[i].is_deleted && !displayed_planes[i].to_be_deleted && (ros::Time::now() - displayed_planes[i].update).sec > m_plane_ttl)
00251                                 {
00252                                         std::cerr << (ros::Time::now() - displayed_planes[i].update).sec << " ";
00253                                         displayed_planes[i].is_deleted = true;
00254                                         displayed_planes[i].to_be_deleted = true;
00255                                 }
00256                                 else if (displayed_planes[i].is_deleted && displayed_planes[i].to_be_deleted)
00257                                 {
00258                                         displayed_planes[i].to_be_deleted = false;
00259                                 }
00260                         }
00261                 }
00262         }
00263 
00264 
00265 
00266 
00268         // Initialization
00270         DynModelExporter::DynModelExporter(ros::NodeHandle *node,
00271                                            const std::string& original_frame,
00272                                            const std::string& output_frame,
00273                                            int minOutputCount,
00274                                            double max_distance,
00275                                            double max_plane_normal_dev,
00276                                            double max_plane_shift_dev,
00277                                            int keep_tracking,
00278                                            int ttl,
00279                                            int max_poly_size
00280                                            )
00281             : original_frame_(original_frame)
00282             , output_frame_(output_frame)
00283         {
00284                 n = node;
00285                 m_minOutputCount = minOutputCount;
00286                 m_max_distance = max_distance;
00287                 m_max_plane_normal_dev = max_plane_normal_dev;
00288                 m_max_plane_shift_dev = max_plane_shift_dev;
00289                 m_keep_tracking = keep_tracking;
00290                 m_plane_ttl = ttl;
00291                 m_max_poly_size = max_poly_size;
00292         }
00293 
00294         void DynModelExporter::createMarkerForConcaveHull(pcl::PointCloud<pcl::PointXYZ>& plane_cloud, srs_env_model_percp::PlaneExt& plane)
00295         {
00296                 plane.NewPlanePoints(plane_cloud.makeShared());
00297             plane.getMeshMarker().type = visualization_msgs::Marker::TRIANGLE_LIST;
00298             plane.getMeshMarker().action = visualization_msgs::Marker::ADD;
00299 
00300             plane.getMeshMarker().ns = "Normals";
00301             plane.getMeshMarker().pose.position.x = 0.0;
00302             plane.getMeshMarker().pose.position.y = 0.0;
00303             plane.getMeshMarker().pose.position.z = 0.0;
00304             plane.getMeshMarker().pose.orientation.x = 0.0;
00305             plane.getMeshMarker().pose.orientation.y = 0.0;
00306             plane.getMeshMarker().pose.orientation.z = 0.0;
00307             plane.getMeshMarker().pose.orientation.w = 1.0;
00308             plane.getMeshMarker().scale.x = 1.00;
00309             plane.getMeshMarker().scale.y = 1.00;
00310             plane.getMeshMarker().scale.z = 1.00;
00311 //
00312 
00313         }
00314 
00315         void DynModelExporter::addMarkerToConcaveHull(pcl::PointCloud<pcl::PointXYZ>& plane_cloud, srs_env_model_percp::PlaneExt& plane)
00316         {
00317                 plane.AddPlanePoints(plane_cloud.makeShared(), m_max_poly_size);
00318         }
00319 
00320         void DynModelExporter::xmlFileExport(std::string filename)
00321         {
00322                 xmlDocPtr document = NULL;
00323                 xmlNodePtr root_node = NULL;
00324                 xmlNodePtr plane_node = NULL;
00325                 xmlNodePtr polys_node = NULL;
00326                 xmlNodePtr poly_node = NULL;
00327                 xmlNodePtr outer_node = NULL;
00328                 xmlNodePtr holes_node = NULL;
00329                 xmlNodePtr hole_node = NULL;
00330                 xmlNodePtr points_node = NULL;
00331                 xmlNodePtr node = NULL;
00332 
00334                 // XML structure
00335                 //
00336                 // <planes>
00337                 //       <plane id=... poly_number=...>
00338                 //         <equation a=... b=... c=... d=... />
00339                 //     <polygons>
00340                 //           <polygon>
00341                 //             <outer>
00342                 //                   <points>
00343                 //                     <point x=.. y=..>
00344                 //                   </points>
00345                 //             </outer>
00346                 //             <holes>
00347                 //           <hole>
00348                 //                     <points>
00349                 //                       <point x=.. y=..>
00350                 //                     </points>
00351                 //           </hole>
00352                 //                       .......
00353                 //             </holes>
00354                 //           </polygon>
00355                 //       ......
00356                 //     </polygons>
00357                 //       </plane>
00358                 //   ......
00359                 // </planes>
00361                 // Create a new XML document
00362                 document = xmlNewDoc(BAD_CAST "1.0");
00363                 root_node = xmlNewNode(NULL, BAD_CAST "planes");
00364                 xmlDocSetRootElement(document, root_node);
00365 
00366                 for (unsigned int i = 0; i < displayed_planes.size(); ++i)
00367                 {
00368                         if (!displayed_planes[i].is_deleted)
00369                         {
00370                                 ClipperLib::ExPolygons polygons = displayed_planes[i].plane.getPolygons();
00371 
00372                                 // save plane global info
00373                                 plane_node = xmlNewChild(root_node, NULL, BAD_CAST "plane", NULL);
00374 
00375                                 std::stringstream str;
00376                                 str << i;
00377                                 xmlNewProp(plane_node, BAD_CAST "id", BAD_CAST str.str().c_str());
00378 
00379                                 str.str("");
00380                                 str << polygons.size();
00381                                 xmlNewProp(plane_node, BAD_CAST "poly_number", BAD_CAST str.str().c_str());
00382 
00383                                 // plane equation
00384                                 node = xmlNewChild(plane_node, NULL, BAD_CAST "equation", NULL);
00385 
00386                                 str.str("");
00387                                 str << displayed_planes[i].plane.a;
00388                                 xmlNewProp(node, BAD_CAST "a", BAD_CAST str.str().c_str());
00389 
00390                                 str.str("");
00391                                 str << displayed_planes[i].plane.b;
00392                                 xmlNewProp(node, BAD_CAST "b", BAD_CAST str.str().c_str());
00393 
00394                                 str.str("");
00395                                 str << displayed_planes[i].plane.c;
00396                                 xmlNewProp(node, BAD_CAST "c", BAD_CAST str.str().c_str());
00397 
00398                                 str.str("");
00399                                 str << displayed_planes[i].plane.d;
00400                                 xmlNewProp(node, BAD_CAST "d", BAD_CAST str.str().c_str());
00401 
00402                                 // color
00403                                 node = xmlNewChild(plane_node, NULL, BAD_CAST "color", NULL);
00404 
00405                                 str.str("");
00406                                 str << displayed_planes[i].plane.color.r;
00407                                 xmlNewProp(node, BAD_CAST "r", BAD_CAST str.str().c_str());
00408 
00409                                 str.str("");
00410                                 str << displayed_planes[i].plane.color.g;
00411                                 xmlNewProp(node, BAD_CAST "g", BAD_CAST str.str().c_str());
00412 
00413                                 str.str("");
00414                                 str << displayed_planes[i].plane.color.b;
00415                                 xmlNewProp(node, BAD_CAST "b", BAD_CAST str.str().c_str());
00416 
00417                                 str.str("");
00418                                 str << displayed_planes[i].plane.color.a;
00419                                 xmlNewProp(node, BAD_CAST "a", BAD_CAST str.str().c_str());
00420 
00421                                 // save polygons
00422                                 polys_node = xmlNewChild(plane_node, NULL, BAD_CAST "polygons", NULL);
00423                                 for (unsigned int j = 0; j < polygons.size(); ++j)
00424                                 {
00425                                         poly_node = xmlNewChild(polys_node, NULL, BAD_CAST "polygon", NULL);
00426 
00427                                         str.str("");
00428                                         str << j;
00429                                         xmlNewProp(poly_node, BAD_CAST "id", BAD_CAST str.str().c_str());
00430 
00431                                         // outer body
00432                                         outer_node = xmlNewChild(poly_node, NULL, BAD_CAST "outer", NULL);
00433                                         points_node = xmlNewChild(outer_node, NULL, BAD_CAST "points", NULL);
00434                                         for (unsigned int k = 0; k < polygons[j].outer.size(); ++k)
00435                                         {
00436                                                 node = xmlNewChild(points_node, NULL, BAD_CAST "point", NULL);
00437 
00438                                                 str.str("");
00439                                                 str << polygons[j].outer[k].X;
00440                                                 xmlNewProp(node, BAD_CAST "x", BAD_CAST str.str().c_str());
00441 
00442                                                 str.str("");
00443                                                 str << polygons[j].outer[k].Y;
00444                                                 xmlNewProp(node, BAD_CAST "y", BAD_CAST str.str().c_str());
00445                                         }
00446 
00447         //                              // export holes
00448         //                              holes_node = xmlNewChild(poly_node, NULL, BAD_CAST "holes", NULL);
00449         //                              for (unsigned int k = 0; k < polygons[j].holes.size(); ++k)
00450         //                              {
00451         //                                      hole_node = xmlNewChild(holes_node, NULL, BAD_CAST "hole", NULL);
00452         //                                      points_node = xmlNewChild(hole_node, NULL, BAD_CAST "points", NULL);
00453         //                                      for (unsigned int l = 0; l < polygons[j].holes[k].size(); ++l)
00454         //                                      {
00455         //                                              node = xmlNewChild(points_node, NULL, BAD_CAST "point", NULL);
00456         //
00457         //                                              str.str("");
00458         //                                              str <<  polygons[j].holes[k][l].X;
00459         //                                              xmlNewProp(node, BAD_CAST "x", BAD_CAST str.str().c_str());
00460         //
00461         //                                              str.str("");
00462         //                                              str <<  polygons[j].holes[k][l].Y;
00463         //                                              xmlNewProp(node, BAD_CAST "y", BAD_CAST str.str().c_str());
00464         //                                      }
00465         //                              }
00466 
00467                                 }
00468                         }
00469                 }
00470 
00471                 xmlSaveFormatFileEnc(filename.c_str(), document, "UTF-8", 1);
00472             xmlFreeDoc(document);
00473             xmlCleanupParser();
00474         }
00475 
00476         void DynModelExporter::xmlFileImport(std::string filename)
00477         {
00479                 // XML structure
00480                 //
00481                 // <planes>
00482                 //       <plane id=... poly_number=...>
00483                 //         <equation a=... b=... c=... d=... />
00484                 //     <polygons>
00485                 //           <polygon>
00486                 //             <outer>
00487                 //                   <points>
00488                 //                     <point x=.. y=..>
00489                 //                   </points>
00490                 //             </outer>
00491                 //             <holes>
00492                 //           <hole>
00493                 //                     <points>
00494                 //                       <point x=.. y=..>
00495                 //                     </points>
00496                 //           </hole>
00497                 //                       .......
00498                 //             </holes>
00499                 //           </polygon>
00500                 //       ......
00501                 //     </polygons>
00502                 //       </plane>
00503                 //   ......
00504                 // </planes>
00506                 xmlDocPtr document = xmlParseFile(filename.c_str());
00507 
00508                 if (document->children && xmlStrEqual(document->children->name, (const xmlChar *)"planes"))
00509                 {
00510                         // pass each node
00511                         for (xmlNodePtr plane = document->children->children; plane; plane = plane->next)
00512                         {
00513 
00514                                 if (xmlStrEqual(plane->name, (const xmlChar *)"plane"))
00515                                 {
00516                                         float a = 0.0;
00517                                         float b = 0.0;
00518                                         float c = 0.0;
00519                                         float d = 0.0;
00520                                         std_msgs::ColorRGBA color;
00521                                         color.r = 0.0;
00522                                         color.g = 0.0;
00523                                         color.b = 0.0;
00524                                         color.a = 0.0;
00525                                         int id = 0;
00526                                         for (xmlAttrPtr attribute = plane->properties; attribute; attribute = attribute->next)
00527                                         {
00528                                                 // get id
00529                                                 if (xmlStrEqual(attribute->name, (const xmlChar *)"id"))
00530                                                 {
00531                                                         id = atoi(reinterpret_cast<const char*>(xmlNodeListGetString(document, attribute->children, 1)));
00532                                                 }
00533                                         }
00534 
00535                                         ClipperLib::ExPolygons polygons;
00536                                         for (xmlNodePtr plane_child = plane->children; plane_child; plane_child = plane_child->next)
00537                                         {
00538                                                 // get equation
00539                                                 if (xmlStrEqual(plane_child->name, (const xmlChar *)"equation"))
00540                                                         for (xmlAttrPtr attribute = plane_child->properties; attribute; attribute = attribute->next)
00541                                                         {
00542                                                                 if (xmlStrEqual(attribute->name, (const xmlChar *)"a"))
00543                                                                         a = atof(reinterpret_cast<const char*>(xmlNodeListGetString(document, attribute->children, 1)));
00544 
00545                                                                 if (xmlStrEqual(attribute->name, (const xmlChar *)"b"))
00546                                                                         b = atof(reinterpret_cast<const char*>(xmlNodeListGetString(document, attribute->children, 1)));
00547 
00548                                                                 if (xmlStrEqual(attribute->name, (const xmlChar *)"c"))
00549                                                                         c = atof(reinterpret_cast<const char*>(xmlNodeListGetString(document, attribute->children, 1)));
00550 
00551                                                                 if (xmlStrEqual(attribute->name, (const xmlChar *)"d"))
00552                                                                         d = atof(reinterpret_cast<const char*>(xmlNodeListGetString(document, attribute->children, 1)));
00553                                                         }
00554 
00555                                                 // get color
00556                                                 if (xmlStrEqual(plane_child->name, (const xmlChar *)"color"))
00557                                                         for (xmlAttrPtr attribute = plane_child->properties; attribute; attribute = attribute->next)
00558                                                         {
00559                                                                 if (xmlStrEqual(attribute->name, (const xmlChar *)"r"))
00560                                                                         color.r = atof(reinterpret_cast<const char*>(xmlNodeListGetString(document, attribute->children, 1)));
00561 
00562                                                                 if (xmlStrEqual(attribute->name, (const xmlChar *)"g"))
00563                                                                         color.g = atof(reinterpret_cast<const char*>(xmlNodeListGetString(document, attribute->children, 1)));
00564 
00565                                                                 if (xmlStrEqual(attribute->name, (const xmlChar *)"b"))
00566                                                                         color.b = atof(reinterpret_cast<const char*>(xmlNodeListGetString(document, attribute->children, 1)));
00567 
00568                                                                 if (xmlStrEqual(attribute->name, (const xmlChar *)"a"))
00569                                                                         color.a = atof(reinterpret_cast<const char*>(xmlNodeListGetString(document, attribute->children, 1)));
00570                                                         }
00571 
00572                                                 // <POLYGONS>
00573                                                 if (xmlStrEqual(plane_child->name, (const xmlChar *)"polygons"))
00574                                                 for (xmlNodePtr polygon = plane_child->children; polygon; polygon = polygon->next)
00575                                                 {
00576                                                         // <POLYGON>
00577                                                         if (xmlStrEqual(polygon->name, (const xmlChar *)"polygon") )
00578                                                         {
00579                                                                 polygons.push_back(ClipperLib::ExPolygon());
00580                                                                 for (xmlNodePtr node = polygon->children; node; node = node->next)
00581                                                                 {
00582                                                                         // <OUTER>
00583                                                                         if (xmlStrEqual(node->name, (const xmlChar *)"outer"))
00584                                                                         {
00585                                                                                 // <POINTS>
00586                                                                                 for (xmlNodePtr points = node->children; points; points = points->next)
00587                                                                                 if (xmlStrEqual(points->name, (const xmlChar *)"points"))
00588                                                                                 {
00589                                                                                         // <POINT>
00590                                                                                         for (xmlNodePtr point = points->children; point; point = point->next)
00591                                                                                         if (xmlStrEqual(point->name, (const xmlChar *)"point"))
00592                                                                                         {
00593                                                                                                 int x, y;
00594                                                                                                 for (xmlAttrPtr attribute = point->properties; attribute; attribute = attribute->next)
00595                                                                                                 {
00596                                                                                                         if (xmlStrEqual(attribute->name, (const xmlChar *)"x"))
00597                                                                                                                 x = atoi(reinterpret_cast<const char*>(xmlNodeListGetString(document, attribute->children, 1)));
00598                                                                                                         if (xmlStrEqual(attribute->name, (const xmlChar *)"y"))
00599                                                                                                                 y = atoi(reinterpret_cast<const char*>(xmlNodeListGetString(document, attribute->children, 1)));
00600                                                                                                 }
00601                                                                                                 polygons.back().outer.push_back(ClipperLib::IntPoint(x, y));
00602                                                                                         } // point
00603                                                                                 } // points
00604                                                                         } // outer
00605 //                                                                      //<HOLES>
00606 //                                                                      else if (xmlStrEqual(node->name, (const xmlChar *)"holes"))
00607 //                                                                      {
00608 //                                                                              // <HOLE>
00609 //                                                                              for (xmlNodePtr hole = node->children; hole; hole = node->next)
00610 //                                                                              if (xmlStrEqual(hole->name, (const xmlChar *)"hole"))
00611 //                                                                              {
00612 //                                                                                      polygons.back().holes.push_back(ClipperLib::Polygon());
00613 //                                                                                      // <POINTS>
00614 //                                                                                      for (xmlNodePtr points = hole->children; points; points = points->next)
00615 //                                                                                      if (xmlStrEqual(points->name, (const xmlChar *)"points"))
00616 //                                                                                      {
00617 //                                                                                              // <POINT>
00618 //                                                                                              for (xmlNodePtr point = points->children; point; point = point->next)
00619 //                                                                                              {
00620 //                                                                                                      int x, y;
00621 //                                                                                                      for (xmlAttrPtr attribute = point->properties; attribute; attribute = attribute->next)
00622 //                                                                                                      {
00623 //                                                                                                              if (xmlStrEqual(attribute->name, (const xmlChar *)"x"))
00624 //                                                                                                                      x = atoi(reinterpret_cast<const char*>(xmlNodeListGetString(document, attribute->children, 1)));
00625 //                                                                                                              if (xmlStrEqual(attribute->name, (const xmlChar *)"y"))
00626 //                                                                                                                      y = atoi(reinterpret_cast<const char*>(xmlNodeListGetString(document, attribute->children, 1)));
00627 //                                                                                                      }
00628 //                                                                                                      polygons.back().holes.back().push_back(ClipperLib::IntPoint(x, y));
00629 //                                                                                              } // point
00630 //                                                                                      } // points
00631 //                                                                              } // hole
00632 //                                                                      } // holes
00633                                                                 } // polygon children
00634                                                         } // polygon
00635                                                 } // polygons
00636                                         } // plane
00637 
00638                                         // save new plane
00639                                         displayed_planes.push_back(ExportedPlane());
00640                                         displayed_planes.back().id = id;
00641                                         displayed_planes.back().plane = PlaneExt(Plane<float>(a, b, c, d));
00642                                         displayed_planes.back().plane.setColor(color);
00643                                         displayed_planes.back().is_deleted = false;
00644                                         displayed_planes.back().to_be_deleted = false;
00645                                         displayed_planes.back().update = ros::Time::now();
00646 
00647                                         displayed_planes.back().plane.setPolygons(polygons);
00648                                         displayed_planes.back().plane.TriangulatePlanePolygon();
00649 
00650                                         displayed_planes.back().plane.getMeshMarker().type = visualization_msgs::Marker::TRIANGLE_LIST;
00651                                         displayed_planes.back().plane.getMeshMarker().action = visualization_msgs::Marker::ADD;
00652 
00653                                         displayed_planes.back().plane.getMeshMarker().ns = "Normals";
00654                                         displayed_planes.back().plane.getMeshMarker().pose.position.x = 0.0;
00655                                         displayed_planes.back().plane.getMeshMarker().pose.position.y = 0.0;
00656                                         displayed_planes.back().plane.getMeshMarker().pose.position.z = 0.0;
00657                                         displayed_planes.back().plane.getMeshMarker().pose.orientation.x = 0.0;
00658                                         displayed_planes.back().plane.getMeshMarker().pose.orientation.y = 0.0;
00659                                         displayed_planes.back().plane.getMeshMarker().pose.orientation.z = 0.0;
00660                                         displayed_planes.back().plane.getMeshMarker().pose.orientation.w = 1.0;
00661                                         displayed_planes.back().plane.getMeshMarker().scale.x = 1.00;
00662                                         displayed_planes.back().plane.getMeshMarker().scale.y = 1.00;
00663                                         displayed_planes.back().plane.getMeshMarker().scale.z = 1.00;
00664                                         displayed_planes.back().plane.getMeshMarker().id = id;
00665 
00666                                 } // planes
00667                         }
00668                 }
00669         }
00670 
00671         void DynModelExporter::getMarkerArray(visualization_msgs::MarkerArray &message, std::string output_frame_id)
00672         {
00673                 std::ofstream file;
00674                 file.open("/home/rosta/out.csv", std::ios_base::app);
00675         for (unsigned int i = 0; i < displayed_planes.size(); ++i)
00676         {
00677                 if (!displayed_planes[i].is_deleted && displayed_planes[i].plane.getMeshMarker().points.size() > 0)
00678                 {
00679                         message.markers.push_back(displayed_planes[i].plane.getMeshMarker());
00680                         message.markers.back().header.frame_id = output_frame_id;
00681                         message.markers.back().header.stamp = ros::Time::now();
00682                 }
00683                 else if (displayed_planes[i].to_be_deleted && displayed_planes[i].plane.getMeshMarker().points.size() > 0)
00684                 {
00685                         message.markers.push_back(displayed_planes[i].plane.getMeshMarker());
00686                         message.markers.back().header.frame_id = output_frame_id;
00687                         message.markers.back().header.stamp = ros::Time::now();
00688                         message.markers.back().action = visualization_msgs::Marker::DELETE;
00689                 }
00690         }
00691         file.close();
00692         }
00693 
00694         void DynModelExporter::getShapeArray(cob_3d_mapping_msgs::ShapeArray &message, std::string output_frame_id)
00695         {
00696         message.header.frame_id = output_frame_id;
00697 
00698         for (unsigned int i = 0; i < displayed_planes.size(); ++i)
00699         {
00700                 if (!displayed_planes[i].is_deleted)
00701                 {
00702                         PlaneExt::tShapeMarker shapes = displayed_planes[i].plane.getShapeMarker();
00703                         for (unsigned int j = 0; j < shapes.size(); ++j)
00704                         {
00705                                 message.shapes.push_back(shapes[j]);
00706                                 message.shapes.back().header.frame_id=output_frame_id;
00707                                 message.shapes.back().id = i * 100 + j;
00708                         }
00709                 }
00710         }
00711         }
00712 
00713 
00714 }// but_scenemodel


srs_env_model_percp
Author(s): Rostislav Hulik (ihulik@fit.vutbr.cz), Tomas Hodan, Michal Spanel (spanel@fit.vutbr.cz)
autogenerated on Sun Jan 5 2014 11:51:56