plane.h
Go to the documentation of this file.00001 /****************************************************************************** 00002 * \file 00003 * 00004 * $Id: plane.h 693 2012-10-20 09:22:39Z 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: 15.06.2012 (version 1.0) 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 00028 #ifndef PLANE_EXT_H 00029 #define PLANE_EXT_H 00030 00031 #include <srs_env_model_percp/but_segmentation/normals.h> 00032 #include <srs_env_model_percp/but_plane_detector/clipper.hpp> 00033 #include <srs_env_model_percp/but_plane_detector/polypartition.h> 00034 #include <visualization_msgs/Marker.h> 00035 #include <cob_3d_mapping_msgs/Shape.h> 00036 #include <pcl/point_cloud.h> 00037 #include <pcl/ModelCoefficients.h> 00038 #include <pcl/filters/project_inliers.h> 00039 #include <pcl/surface/concave_hull.h> 00040 #include <pcl_ros/transforms.h> 00041 00042 // conversion between any type to ClipperLib long long type and back 00043 #define CONVERT_TO_LONG(a) a * 1000000 00044 #define CONVERT_FROM_LONG(a) (float)a / 1000000 00045 00046 namespace srs_env_model_percp 00047 { 00048 class PlaneExt : public but_plane_detector::Plane<float> 00049 { 00050 public: 00051 typedef std::vector<cob_3d_mapping_msgs::Shape, Eigen::aligned_allocator<cob_3d_mapping_msgs::Shape> > tShapeMarker; 00052 00053 // typedef std::vector<pcl::Vertices, Eigen::aligned_allocator<pcl::Vertices> > tVertices; 00054 typedef std::vector<pcl::Vertices> tVertices; 00055 00056 public: 00060 PlaneExt(but_plane_detector::Plane<float> plane); 00061 00065 visualization_msgs::Marker NewPlanePoints(pcl::PointCloud<pcl::PointXYZ>::Ptr plane_cloud); 00066 00070 visualization_msgs::Marker AddPlanePoints(pcl::PointCloud<pcl::PointXYZ>::Ptr plane_cloud, int max_poly_size); 00071 00075 ClipperLib::ExPolygons &getPolygons(); 00076 00077 void setPolygons(ClipperLib::ExPolygons &polys); 00078 00082 std::list<TPPLPoly> &getMesh(); 00083 00087 visualization_msgs::Marker &getMeshMarker(); 00088 00092 tShapeMarker &getShapeMarker(); 00093 00094 void setColor(std_msgs::ColorRGBA &new_color); 00095 00096 // Triangulates plane polygon 00097 void TriangulatePlanePolygon(); 00098 00099 std_msgs::ColorRGBA color; 00100 protected: 00101 // Computes concave hull of set of points 00102 tVertices ComputeConcaveHull(pcl::PointCloud<pcl::PointXYZ>::Ptr &plane_cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr &plane_hull); 00103 00104 // Computes hull U current polygon 00105 bool ConcaveHullJoinCurrent(pcl::PointCloud<pcl::PointXYZ>::Ptr &plane_hull, tVertices &polygon_indices, int max_poly_size); 00106 00107 // Rewrites current plane with this hull 00108 void ConcaveHullRewrite(pcl::PointCloud<pcl::PointXYZ>::Ptr &plane_hull, tVertices &polygon_indices); 00109 00110 // Polygonizes current hull 00111 ClipperLib::ExPolygons PolygonizeConcaveHull(pcl::PointCloud<pcl::PointXYZ>::Ptr &plane_hull, tVertices &polygon_indices); 00112 00113 Eigen::Affine3f planeTransXY; 00114 double planeShift; 00115 pcl::ModelCoefficients::Ptr planeCoefficients; 00116 00117 ClipperLib::ExPolygons planePolygonsClipper; 00118 visualization_msgs::Marker planeTriangles; 00119 tShapeMarker planeTrianglesSRS; 00120 Eigen::Quaternion<float> rotationQuaternion; 00121 00122 static const int MAX_POLYS = 1000; 00123 00124 public: 00125 EIGEN_MAKE_ALIGNED_OPERATOR_NEW 00126 }; 00127 } 00128 00129 #endif