convex_polygon.h
Go to the documentation of this file.
1 // -*- mode: c++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2014, JSK Lab
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/o2r other materials provided
17  * with the distribution.
18  * * Neither the name of the JSK Lab nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
36 #ifndef JSK_RECOGNITION_UTILS_GEO_CONVEX_POLYGON_H_
37 #define JSK_RECOGNITION_UTILS_GEO_CONVEX_POLYGON_H_
38 
40 #include <pcl/segmentation/extract_polygonal_prism_data.h>
41 #include <pcl/filters/project_inliers.h>
42 #include <pcl/surface/convex_hull.h>
44 
45 
46 namespace jsk_recognition_utils
47 {
48  class ConvexPolygon: public Polygon
49  {
50  public:
52  typedef Eigen::Vector3f Vertex;
53  typedef std::vector<Eigen::Vector3f,
54  Eigen::aligned_allocator<Eigen::Vector3f> > Vertices;
55  // vertices should be CW
56  ConvexPolygon(const Vertices& vertices);
57  ConvexPolygon(const Vertices& vertices,
58  const std::vector<float>& coefficients);
59  //virtual Polygon flip();
60  virtual void project(const Eigen::Vector3f& p, Eigen::Vector3f& output);
61  virtual void project(const Eigen::Vector3d& p, Eigen::Vector3d& output);
62  virtual void project(const Eigen::Vector3d& p, Eigen::Vector3f& output);
63  virtual void project(const Eigen::Vector3f& p, Eigen::Vector3d& output);
64  virtual void projectOnPlane(const Eigen::Vector3f& p,
65  Eigen::Vector3f& output);
66  virtual void projectOnPlane(const Eigen::Affine3f& p,
67  Eigen::Affine3f& output);
68  virtual bool isProjectableInside(const Eigen::Vector3f& p);
69  // p should be a point on the plane
70  virtual ConvexPolygon flipConvex();
71  virtual Eigen::Vector3f getCentroid();
72  virtual Ptr magnify(const double scale_factor);
73  virtual Ptr magnifyByDistance(const double distance);
74 
75  static ConvexPolygon fromROSMsg(const geometry_msgs::Polygon& polygon);
76  static ConvexPolygon::Ptr fromROSMsgPtr(const geometry_msgs::Polygon& polygon);
78  const Eigen::Vector3f& p, double distance_threshold);
80  const Eigen::Vector3f& p, double distance_threshold,
81  double& output_distance);
82  bool allEdgesLongerThan(double thr);
83  double distanceFromVertices(const Eigen::Vector3f& p);
84  geometry_msgs::Polygon toROSMsg();
85  protected:
86 
87  private:
88  };
89 
90  template<class PointT>
92  const typename pcl::PointCloud<PointT>::Ptr cloud,
93  const pcl::PointIndices::Ptr inliers,
94  const pcl::ModelCoefficients::Ptr coefficients) {
95  typedef typename pcl::PointCloud<PointT> POINTCLOUD;
96  typename POINTCLOUD::Ptr projected_cloud(new pcl::PointCloud<PointT>);
97  // check inliers has enough points
98  if (inliers->indices.size() == 0) {
99  return ConvexPolygon::Ptr();
100  }
101  // project inliers based on coefficients
102  pcl::ProjectInliers<PointT> proj;
103  proj.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE);
104  proj.setInputCloud(cloud);
105  proj.setModelCoefficients(coefficients);
106  proj.setIndices(inliers);
107  proj.filter(*projected_cloud);
108  // compute convex with giant mutex
109  {
110  boost::mutex::scoped_lock lock(global_chull_mutex);
111  typename POINTCLOUD::Ptr convex_cloud(new pcl::PointCloud<PointT>);
112  pcl::ConvexHull<PointT> chull;
113  chull.setDimension(2);
114  chull.setInputCloud (projected_cloud);
115  chull.reconstruct (*convex_cloud);
116  if (convex_cloud->points.size() > 0) {
117  // convert pointcloud to vertices
118  Vertices vs;
119  for (size_t i = 0; i < convex_cloud->points.size(); i++) {
120  Eigen::Vector3f v(convex_cloud->points[i].getVector3fMap());
121  vs.push_back(v);
122  }
123  return ConvexPolygon::Ptr(new ConvexPolygon(vs));
124  }
125  else {
126  return ConvexPolygon::Ptr();
127  }
128  }
129  }
130 }
131 
132 #endif
bool distanceSmallerThan(const Eigen::Vector3f &p, double distance_threshold)
static ConvexPolygon fromROSMsg(const geometry_msgs::Polygon &polygon)
boost::mutex global_chull_mutex
Definition: pcl_util.cpp:43
virtual ConvexPolygon flipConvex()
virtual Ptr magnify(const double scale_factor)
double distance(const Eigen::Vector3f &point)
Compute distance between point and this polygon.
Definition: polygon.cpp:225
boost::shared_ptr< ConvexPolygon > Ptr
virtual void project(const Eigen::Vector3f &p, Eigen::Vector3f &output)
static ConvexPolygon::Ptr fromROSMsgPtr(const geometry_msgs::Polygon &polygon)
virtual Ptr magnifyByDistance(const double distance)
virtual Eigen::Vector3f getCentroid()
double distanceFromVertices(const Eigen::Vector3f &p)
virtual void projectOnPlane(const Eigen::Vector3f &p, Eigen::Vector3f &output)
ConvexPolygon(const Vertices &vertices)
std::vector< Eigen::Vector3f, Eigen::aligned_allocator< Eigen::Vector3f > > Vertices
Definition: types.h:48
ConvexPolygon::Ptr convexFromCoefficientsAndInliers(const typename pcl::PointCloud< PointT >::Ptr cloud, const pcl::PointIndices::Ptr inliers, const pcl::ModelCoefficients::Ptr coefficients)
virtual bool isProjectableInside(const Eigen::Vector3f &p)
std::vector< Eigen::Vector3f, Eigen::aligned_allocator< Eigen::Vector3f > > Vertices


jsk_recognition_utils
Author(s):
autogenerated on Tue Mar 3 2020 03:59:38