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_POLYGON_H_
37 #define JSK_RECOGNITION_UTILS_GEO_POLYGON_H_
38 
39 #include <Eigen/Geometry>
40 #include <boost/shared_ptr.hpp>
41 #include <pcl/point_cloud.h>
42 #include <pcl/point_types.h>
43 #include <boost/random.hpp>
44 #include <geometry_msgs/Polygon.h>
45 #include <jsk_recognition_msgs/PolygonArray.h>
46 
52 
53 namespace jsk_recognition_utils
54 {
55  class Polygon: public Plane
56  {
57  public:
58  typedef boost::shared_ptr<Polygon> Ptr;
59  typedef boost::tuple<Ptr, Ptr> PtrPair;
60  Polygon(const Vertices& vertices);
61  Polygon(const Vertices& vertices,
62  const std::vector<float>& coefficients);
63  virtual ~Polygon();
64  virtual std::vector<Polygon::Ptr> decomposeToTriangles();
66  cached_triangles_.clear();
67  }
68 
69  virtual Eigen::Vector3f getNormalFromVertices();
70  virtual bool isTriangle();
71  Eigen::Vector3f randomSampleLocalPoint(boost::mt19937& random_generator);
72  virtual void getLocalMinMax(double& min_x, double& min_y,
73  double& max_x, double& max_y);
74  template <class PointT>
75  typename pcl::PointCloud<PointT>::Ptr samplePoints(double grid_size)
76  {
77  typename pcl::PointCloud<PointT>::Ptr
78  ret (new pcl::PointCloud<PointT>);
79  double min_x, min_y, max_x, max_y;
80  getLocalMinMax(min_x, min_y, max_x, max_y);
81  // ROS_INFO("min_x: %f", min_x);
82  // ROS_INFO("min_y: %f", min_y);
83  // ROS_INFO("max_x: %f", max_x);
84  // ROS_INFO("max_y: %f", max_y);
85  // Decompose into triangle first for optimization
86  std::vector<Polygon::Ptr> triangles = decomposeToTriangles();
87 
88  for (double x = min_x; x < max_x; x += grid_size) {
89  for (double y = min_y; y < max_y; y += grid_size) {
90  Eigen::Vector3f candidate(x, y, 0);
91  Eigen::Vector3f candidate_global = coordinates() * candidate;
92  // check candidate is inside of the polygon or not
93  bool insidep = false;
94  for (size_t i = 0; i < triangles.size(); i++) {
95  if (triangles[i]->isInside(candidate_global)) {
96  insidep = true;
97  break;
98  }
99  }
100  if (insidep) {
101  PointT p;
102  p.x = candidate_global[0];
103  p.y = candidate_global[1];
104  p.z = candidate_global[2];
105  p.normal_x = normal_[0];
106  p.normal_y = normal_[1];
107  p.normal_z = normal_[2];
108  ret->points.push_back(p);
109  }
110  }
111  }
112  return ret;
113  }
114 
119  std::vector<Segment::Ptr> edges() const;
120 
125  double distance(const Eigen::Vector3f& point);
126 
132  double distance(const Eigen::Vector3f& point,
133  Eigen::Vector3f& nearest_point);
134 
154  virtual Eigen::Vector3f nearestPoint(const Eigen::Vector3f& p,
155  double& distance);
156  virtual size_t getNumVertices();
157  virtual size_t getFarestPointIndex(const Eigen::Vector3f& O);
158  virtual Eigen::Vector3f directionAtPoint(size_t i);
159  virtual Eigen::Vector3f getVertex(size_t i);
160  virtual PointIndexPair getNeighborIndex(size_t index);
161  virtual Vertices getVertices() { return vertices_; };
162  virtual double area();
164  size_t index,
165  const Eigen::Vector3f& direction);
166  virtual PtrPair separatePolygon(size_t index);
172  virtual bool isInside(const Eigen::Vector3f& p);
173  size_t previousIndex(size_t i);
174  size_t nextIndex(size_t i);
175 
176  static Polygon fromROSMsg(const geometry_msgs::Polygon& polygon);
177  static Polygon::Ptr fromROSMsgPtr(const geometry_msgs::Polygon& polygon);
178  static Polygon createPolygonWithSkip(const Vertices& vertices);
179 
186  static std::vector<Polygon::Ptr> fromROSMsg(const jsk_recognition_msgs::PolygonArray& msg,
187  const Eigen::Affine3f& offset = Eigen::Affine3f::Identity());
188 
194  virtual void transformBy(const Eigen::Affine3d& transform);
195 
201  virtual void transformBy(const Eigen::Affine3f& transform);
202 
209  virtual bool maskImage(const jsk_recognition_utils::CameraDepthSensor& model,
210  cv::Mat& image) const;
211 
217  cv::Mat& image,
218  const cv::Scalar& color,
219  const int line_width = 1) const;
220  virtual bool isConvex();
221  virtual Eigen::Vector3f centroid();
222  template<class PointT> void boundariesToPointCloud(
223  pcl::PointCloud<PointT>& output) {
224  output.points.resize(vertices_.size());
225  for (size_t i = 0; i < vertices_.size(); i++) {
226  Eigen::Vector3f v = vertices_[i];
227  PointT p;
228  p.x = v[0]; p.y = v[1]; p.z = v[2];
229  output.points[i] = p;
230  }
231  output.height = 1;
232  output.width = output.points.size();
233  }
234 
235  protected:
237  std::vector<Polygon::Ptr> cached_triangles_;
238  private:
239 
240  };
241 }
242 
243 #endif
size_t nextIndex(size_t i)
Definition: polygon.cpp:554
virtual Eigen::Vector3f directionAtPoint(size_t i)
Definition: polygon.cpp:142
virtual bool maskImage(const jsk_recognition_utils::CameraDepthSensor &model, cv::Mat &image) const
generate mask image of the polygon. if all the points are outside of field-of-view, returns false.
Definition: polygon.cpp:412
virtual void transformBy(const Eigen::Affine3d &transform)
transform Polygon with given transform. cached triangles is cleared.
Definition: polygon.cpp:389
static Polygon createPolygonWithSkip(const Vertices &vertices)
Definition: polygon.cpp:46
double distance(const Eigen::Vector3f &point)
Compute distance between point and this polygon.
Definition: polygon.cpp:225
virtual Vertices getVertices()
Definition: polygon.h:161
virtual Plane transform(const Eigen::Affine3d &transform)
Definition: plane.cpp:225
boost::shared_ptr< Polygon > Ptr
Definition: polygon.h:58
static Polygon fromROSMsg(const geometry_msgs::Polygon &polygon)
Definition: polygon.cpp:564
virtual Eigen::Affine3f coordinates()
Definition: plane.cpp:282
virtual void getLocalMinMax(double &min_x, double &min_y, double &max_x, double &max_y)
Definition: polygon.cpp:166
Eigen::Vector3f randomSampleLocalPoint(boost::mt19937 &random_generator)
Definition: polygon.cpp:185
virtual PtrPair separatePolygon(size_t index)
Definition: polygon.cpp:275
void boundariesToPointCloud(pcl::PointCloud< PointT > &output)
Definition: polygon.h:222
virtual bool isPossibleToRemoveTriangleAtIndex(size_t index, const Eigen::Vector3f &direction)
Definition: polygon.cpp:305
virtual void drawLineToImage(const jsk_recognition_utils::CameraDepthSensor &model, cv::Mat &image, const cv::Scalar &color, const int line_width=1) const
draw line of polygons on image.
Definition: polygon.cpp:442
static Polygon::Ptr fromROSMsgPtr(const geometry_msgs::Polygon &polygon)
Definition: polygon.cpp:576
virtual Eigen::Vector3f nearestPoint(const Eigen::Vector3f &p, double &distance)
Compute nearest point from p on this polygon.
Definition: polygon.cpp:239
virtual PointIndexPair getNeighborIndex(size_t index)
Definition: polygon.cpp:121
virtual bool isInside(const Eigen::Vector3f &p)
return true if p is inside of polygon. p should be in global coordinates.
Definition: polygon.cpp:600
boost::tuple< size_t, size_t > PointIndexPair
Definition: types.h:50
virtual Eigen::Vector3f getNormalFromVertices()
Definition: polygon.cpp:533
virtual void clearTriangleDecompositionCache()
Definition: polygon.h:65
std::vector< Eigen::Vector3f, Eigen::aligned_allocator< Eigen::Vector3f > > Vertices
Definition: types.h:48
size_t previousIndex(size_t i)
Definition: polygon.cpp:544
pcl::PointCloud< PointT >::Ptr samplePoints(double grid_size)
Definition: polygon.h:75
std::vector< Segment::Ptr > edges() const
get all the edges as point of Segment.
Definition: polygon.cpp:212
virtual size_t getFarestPointIndex(const Eigen::Vector3f &O)
Definition: polygon.cpp:106
virtual Eigen::Vector3f getVertex(size_t i)
Definition: polygon.cpp:271
boost::tuple< Ptr, Ptr > PtrPair
Definition: polygon.h:59
Eigen::Vector3f normal_
Definition: plane.h:83
std::vector< Polygon::Ptr > cached_triangles_
Definition: polygon.h:237
virtual std::vector< Polygon::Ptr > decomposeToTriangles()
Definition: polygon.cpp:482
virtual size_t getNumVertices()
Definition: polygon.cpp:267
Polygon(const Vertices &vertices)
Definition: polygon.cpp:87
virtual Eigen::Vector3f centroid()
Definition: polygon.cpp:62


jsk_recognition_utils
Author(s):
autogenerated on Thu May 21 2020 04:00:42