Point Cloud Library (PCL)  1.9.0-dev
polygon_operations.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2010, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage, Inc. nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *
34  */
35 
36 #ifndef PCL_GEOMETRY_POLYGON_OPERATIONS_HPP_
37 #define PCL_GEOMETRY_POLYGON_OPERATIONS_HPP_
38 
39 #include <pcl/geometry/polygon_operations.h>
40 
41 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
42 template<typename PointT> void
43 pcl::approximatePolygon (const PlanarPolygon<PointT>& polygon, PlanarPolygon<PointT>& approx_polygon, float threshold, bool refine, bool closed)
44 {
45  const Eigen::Vector4f& coefficients = polygon.getCoefficients ();
46  const typename pcl::PointCloud<PointT>::VectorType &contour = polygon.getContour ();
47 
48  Eigen::Vector3f rotation_axis (coefficients[1], -coefficients[0], 0.0f);
49  rotation_axis.normalize ();
50 
51  float rotation_angle = acosf (coefficients [2]);
52  Eigen::Affine3f transformation = Eigen::Translation3f (0, 0, coefficients [3]) * Eigen::AngleAxisf (rotation_angle, rotation_axis);
53 
54  typename pcl::PointCloud<PointT>::VectorType polygon2D (contour.size ());
55  for (unsigned pIdx = 0; pIdx < polygon2D.size (); ++pIdx)
56  polygon2D [pIdx].getVector3fMap () = transformation * contour [pIdx].getVector3fMap ();
57 
58  typename pcl::PointCloud<PointT>::VectorType approx_polygon2D;
59  approximatePolygon2D<PointT> (polygon2D, approx_polygon2D, threshold, refine, closed);
60 
61  typename pcl::PointCloud<PointT>::VectorType &approx_contour = approx_polygon.getContour ();
62  approx_contour.resize (approx_polygon2D.size ());
63 
64  Eigen::Affine3f inv_transformation = transformation.inverse ();
65  for (unsigned pIdx = 0; pIdx < approx_polygon2D.size (); ++pIdx)
66  approx_contour [pIdx].getVector3fMap () = inv_transformation * approx_polygon2D [pIdx].getVector3fMap ();
67 }
68 
69 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
70 template <typename PointT> void
72  typename pcl::PointCloud<PointT>::VectorType &approx_polygon,
73  float threshold, bool refine, bool closed)
74 {
75  approx_polygon.clear ();
76  if (polygon.size () < 3)
77  return;
78 
79  std::vector<std::pair<unsigned, unsigned> > intervals;
80  std::pair<unsigned,unsigned> interval (0, 0);
81 
82  if (closed)
83  {
84  float max_distance = .0f;
85  for (unsigned idx = 1; idx < polygon.size (); ++idx)
86  {
87  float distance = (polygon [0].x - polygon [idx].x) * (polygon [0].x - polygon [idx].x) +
88  (polygon [0].y - polygon [idx].y) * (polygon [0].y - polygon [idx].y);
89 
90  if (distance > max_distance)
91  {
92  max_distance = distance;
93  interval.second = idx;
94  }
95  }
96 
97  for (unsigned idx = 1; idx < polygon.size (); ++idx)
98  {
99  float distance = (polygon [interval.second].x - polygon [idx].x) * (polygon [interval.second].x - polygon [idx].x) +
100  (polygon [interval.second].y - polygon [idx].y) * (polygon [interval.second].y - polygon [idx].y);
101 
102  if (distance > max_distance)
103  {
104  max_distance = distance;
105  interval.first = idx;
106  }
107  }
108 
109  if (max_distance < threshold * threshold)
110  return;
111 
112  intervals.push_back (interval);
113  std::swap (interval.first, interval.second);
114  intervals.push_back (interval);
115  }
116  else
117  {
118  interval.first = 0;
119  interval.second = static_cast<unsigned int> (polygon.size ()) - 1;
120  intervals.push_back (interval);
121  }
122 
123  std::vector<unsigned> result;
124  // recursively refine
125  while (!intervals.empty ())
126  {
127  std::pair<unsigned, unsigned>& currentInterval = intervals.back ();
128  float line_x = polygon [currentInterval.first].y - polygon [currentInterval.second].y;
129  float line_y = polygon [currentInterval.second].x - polygon [currentInterval.first].x;
130  float line_d = polygon [currentInterval.first].x * polygon [currentInterval.second].y - polygon [currentInterval.first].y * polygon [currentInterval.second].x;
131 
132  float linelen = 1.0f / std::sqrt (line_x * line_x + line_y * line_y);
133 
134  line_x *= linelen;
135  line_y *= linelen;
136  line_d *= linelen;
137 
138  float max_distance = 0.0;
139  unsigned first_index = currentInterval.first + 1;
140  unsigned max_index = 0;
141 
142  // => 0-crossing
143  if (currentInterval.first > currentInterval.second)
144  {
145  for (unsigned idx = first_index; idx < polygon.size(); idx++)
146  {
147  float distance = fabsf (line_x * polygon[idx].x + line_y * polygon[idx].y + line_d);
148  if (distance > max_distance)
149  {
150  max_distance = distance;
151  max_index = idx;
152  }
153  }
154  first_index = 0;
155  }
156 
157  for (unsigned int idx = first_index; idx < currentInterval.second; idx++)
158  {
159  float distance = fabsf (line_x * polygon[idx].x + line_y * polygon[idx].y + line_d);
160  if (distance > max_distance)
161  {
162  max_distance = distance;
163  max_index = idx;
164  }
165  }
166 
167  if (max_distance > threshold)
168  {
169  std::pair<unsigned, unsigned> interval (max_index, currentInterval.second);
170  currentInterval.second = max_index;
171  intervals.push_back (interval);
172  }
173  else
174  {
175  result.push_back (currentInterval.second);
176  intervals.pop_back ();
177  }
178  }
179 
180  approx_polygon.reserve (result.size ());
181  if (refine)
182  {
183  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > lines (result.size ());
184  std::reverse (result.begin (), result.end ());
185  for (unsigned rIdx = 0; rIdx < result.size (); ++rIdx)
186  {
187  unsigned nIdx = rIdx + 1;
188  if (nIdx == result.size ())
189  nIdx = 0;
190 
191  Eigen::Vector2f centroid = Eigen::Vector2f::Zero ();
192  Eigen::Matrix2f covariance = Eigen::Matrix2f::Zero ();
193  unsigned pIdx = result[rIdx];
194  unsigned num_points = 0;
195  if (pIdx > result[nIdx])
196  {
197  num_points = static_cast<unsigned> (polygon.size ()) - pIdx;
198  for (; pIdx < polygon.size (); ++pIdx)
199  {
200  covariance.coeffRef (0) += polygon [pIdx].x * polygon [pIdx].x;
201  covariance.coeffRef (1) += polygon [pIdx].x * polygon [pIdx].y;
202  covariance.coeffRef (3) += polygon [pIdx].y * polygon [pIdx].y;
203  centroid [0] += polygon [pIdx].x;
204  centroid [1] += polygon [pIdx].y;
205  }
206  pIdx = 0;
207  }
208 
209  num_points += result[nIdx] - pIdx;
210  for (; pIdx < result[nIdx]; ++pIdx)
211  {
212  covariance.coeffRef (0) += polygon [pIdx].x * polygon [pIdx].x;
213  covariance.coeffRef (1) += polygon [pIdx].x * polygon [pIdx].y;
214  covariance.coeffRef (3) += polygon [pIdx].y * polygon [pIdx].y;
215  centroid [0] += polygon [pIdx].x;
216  centroid [1] += polygon [pIdx].y;
217  }
218 
219  covariance.coeffRef (2) = covariance.coeff (1);
220 
221  float norm = 1.0f / float (num_points);
222  centroid *= norm;
223  covariance *= norm;
224  covariance.coeffRef (0) -= centroid [0] * centroid [0];
225  covariance.coeffRef (1) -= centroid [0] * centroid [1];
226  covariance.coeffRef (3) -= centroid [1] * centroid [1];
227 
228  float eval;
229  Eigen::Vector2f normal;
230  eigen22 (covariance, eval, normal);
231 
232  // select the one which is more "parallel" to the original line
233  Eigen::Vector2f direction;
234  direction [0] = polygon[result[nIdx]].x - polygon[result[rIdx]].x;
235  direction [1] = polygon[result[nIdx]].y - polygon[result[rIdx]].y;
236  direction.normalize ();
237 
238  if (fabs (direction.dot (normal)) > float(M_SQRT1_2))
239  {
240  std::swap (normal [0], normal [1]);
241  normal [0] = -normal [0];
242  }
243 
244  // needs to be on the left side of the edge
245  if (direction [0] * normal [1] < direction [1] * normal [0])
246  normal *= -1.0;
247 
248  lines [rIdx].head<2> ().matrix () = normal;
249  lines [rIdx] [2] = -normal.dot (centroid);
250  }
251 
252  float threshold2 = threshold * threshold;
253  for (unsigned rIdx = 0; rIdx < lines.size (); ++rIdx)
254  {
255  unsigned nIdx = rIdx + 1;
256  if (nIdx == result.size ())
257  nIdx = 0;
258 
259  Eigen::Vector3f vertex = lines [rIdx].cross (lines [nIdx]);
260  vertex /= vertex [2];
261  vertex [2] = 0.0;
262 
263  PointT point;
264  // test whether we need another edge since the intersection point is too far away from the original vertex
265  Eigen::Vector3f pq = polygon [result[nIdx]].getVector3fMap () - vertex;
266  pq [2] = 0.0;
267 
268  float distance = pq.squaredNorm ();
269  if (distance > threshold2)
270  {
271  // test whether the old point is inside the new polygon or outside
272  if ((pq [0] * lines [rIdx] [0] + pq [1] * lines [rIdx] [1] < 0.0) &&
273  (pq [0] * lines [nIdx] [0] + pq [1] * lines [nIdx] [1] < 0.0) )
274  {
275  float distance1 = lines [rIdx] [0] * polygon[result[nIdx]].x + lines [rIdx] [1] * polygon[result[nIdx]].y + lines [rIdx] [2];
276  float distance2 = lines [nIdx] [0] * polygon[result[nIdx]].x + lines [nIdx] [1] * polygon[result[nIdx]].y + lines [nIdx] [2];
277 
278  point.x = polygon[result[nIdx]].x - distance1 * lines [rIdx] [0];
279  point.y = polygon[result[nIdx]].y - distance1 * lines [rIdx] [1];
280 
281  approx_polygon.push_back (point);
282 
283  vertex [0] = polygon[result[nIdx]].x - distance2 * lines [nIdx] [0];
284  vertex [1] = polygon[result[nIdx]].y - distance2 * lines [nIdx] [1];
285  }
286  }
287  point.getVector3fMap () = vertex;
288  approx_polygon.push_back (point);
289  }
290  }
291  else
292  {
293  // we have a new polygon in results, but inverted (clockwise <-> counter-clockwise)
294  for (std::vector<unsigned>::reverse_iterator it = result.rbegin (); it != result.rend (); ++it)
295  approx_polygon.push_back (polygon [*it]);
296  }
297 }
298 
299 #endif // PCL_GEOMETRY_POLYGON_OPERATIONS_HPP_
size_t size() const
Definition: point_cloud.h:448
void reserve(size_t n)
Definition: point_cloud.h:449
pcl::PointCloud< PointT >::VectorType & getContour()
Getter for the contour / boundary.
void approximatePolygon2D(const typename PointCloud< PointT >::VectorType &polygon, typename PointCloud< PointT >::VectorType &approx_polygon, float threshold, bool refine=false, bool closed=true)
returns an approximate polygon to given 2D contour.
void approximatePolygon(const PlanarPolygon< PointT > &polygon, PlanarPolygon< PointT > &approx_polygon, float threshold, bool refine=false, bool closed=true)
see approximatePolygon2D
std::vector< PointT, Eigen::aligned_allocator< PointT > > VectorType
Definition: point_cloud.h:426
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
Definition: point_cloud.h:480
void eigen22(const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector)
determine the smallest eigenvalue and its corresponding eigenvector
Definition: eigen.hpp:126
PlanarPolygon represents a planar (2D) polygon, potentially in a 3D space.
Eigen::Vector4f & getCoefficients()
Getter for the coefficients.
void clear()
Removes all points in a cloud and sets the width and height to 0.
Definition: point_cloud.h:576
A point structure representing Euclidean xyz coordinates, and the RGB color.