Point Cloud Library (PCL)  1.12.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>
43 void
45  PlanarPolygon<PointT>& approx_polygon,
46  float threshold,
47  bool refine,
48  bool closed)
49 {
50  const Eigen::Vector4f& coefficients = polygon.getCoefficients();
51  const typename pcl::PointCloud<PointT>::VectorType& contour = polygon.getContour();
52 
53  Eigen::Vector3f rotation_axis(coefficients[1], -coefficients[0], 0.0f);
54  rotation_axis.normalize();
55 
56  float rotation_angle = acosf(coefficients[2]);
57  Eigen::Affine3f transformation = Eigen::Translation3f(0, 0, coefficients[3]) *
58  Eigen::AngleAxisf(rotation_angle, rotation_axis);
59 
60  typename pcl::PointCloud<PointT>::VectorType polygon2D(contour.size());
61  for (std::size_t pIdx = 0; pIdx < polygon2D.size(); ++pIdx)
62  polygon2D[pIdx].getVector3fMap() = transformation * contour[pIdx].getVector3fMap();
63 
64  typename pcl::PointCloud<PointT>::VectorType approx_polygon2D;
65  approximatePolygon2D<PointT>(polygon2D, approx_polygon2D, threshold, refine, closed);
66 
67  typename pcl::PointCloud<PointT>::VectorType& approx_contour =
68  approx_polygon.getContour();
69  approx_contour.resize(approx_polygon2D.size());
70 
71  Eigen::Affine3f inv_transformation = transformation.inverse();
72  for (std::size_t pIdx = 0; pIdx < approx_polygon2D.size(); ++pIdx)
73  approx_contour[pIdx].getVector3fMap() =
74  inv_transformation * approx_polygon2D[pIdx].getVector3fMap();
75 }
76 
77 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
78 template <typename PointT>
79 void
81  typename pcl::PointCloud<PointT>::VectorType& approx_polygon,
82  float threshold,
83  bool refine,
84  bool closed)
85 {
86  approx_polygon.clear();
87  if (polygon.size() < 3)
88  return;
89 
90  std::vector<std::pair<unsigned, unsigned>> intervals;
91  std::pair<unsigned, unsigned> interval(0, 0);
92 
93  if (closed) {
94  float max_distance = .0f;
95  for (std::size_t idx = 1; idx < polygon.size(); ++idx) {
96  float distance =
97  (polygon[0].x - polygon[idx].x) * (polygon[0].x - polygon[idx].x) +
98  (polygon[0].y - polygon[idx].y) * (polygon[0].y - polygon[idx].y);
99 
100  if (distance > max_distance) {
101  max_distance = distance;
102  interval.second = idx;
103  }
104  }
105 
106  for (std::size_t idx = 1; idx < polygon.size(); ++idx) {
107  float distance = (polygon[interval.second].x - polygon[idx].x) *
108  (polygon[interval.second].x - polygon[idx].x) +
109  (polygon[interval.second].y - polygon[idx].y) *
110  (polygon[interval.second].y - polygon[idx].y);
111 
112  if (distance > max_distance) {
113  max_distance = distance;
114  interval.first = idx;
115  }
116  }
117 
118  if (max_distance < threshold * threshold)
119  return;
120 
121  intervals.push_back(interval);
122  std::swap(interval.first, interval.second);
123  intervals.push_back(interval);
124  }
125  else {
126  interval.first = 0;
127  interval.second = static_cast<unsigned int>(polygon.size()) - 1;
128  intervals.push_back(interval);
129  }
130 
131  std::vector<unsigned> result;
132  // recursively refine
133  while (!intervals.empty()) {
134  std::pair<unsigned, unsigned>& currentInterval = intervals.back();
135  float line_x = polygon[currentInterval.first].y - polygon[currentInterval.second].y;
136  float line_y = polygon[currentInterval.second].x - polygon[currentInterval.first].x;
137  float line_d =
138  polygon[currentInterval.first].x * polygon[currentInterval.second].y -
139  polygon[currentInterval.first].y * polygon[currentInterval.second].x;
140 
141  float linelen = 1.0f / std::sqrt(line_x * line_x + line_y * line_y);
142 
143  line_x *= linelen;
144  line_y *= linelen;
145  line_d *= linelen;
146 
147  float max_distance = 0.0;
148  unsigned first_index = currentInterval.first + 1;
149  unsigned max_index = 0;
150 
151  // => 0-crossing
152  if (currentInterval.first > currentInterval.second) {
153  for (std::size_t idx = first_index; idx < polygon.size(); idx++) {
154  float distance =
155  std::abs(line_x * polygon[idx].x + line_y * polygon[idx].y + line_d);
156  if (distance > max_distance) {
157  max_distance = distance;
158  max_index = idx;
159  }
160  }
161  first_index = 0;
162  }
163 
164  for (unsigned int idx = first_index; idx < currentInterval.second; idx++) {
165  float distance =
166  std::abs(line_x * polygon[idx].x + line_y * polygon[idx].y + line_d);
167  if (distance > max_distance) {
168  max_distance = distance;
169  max_index = idx;
170  }
171  }
172 
173  if (max_distance > threshold) {
174  std::pair<unsigned, unsigned> interval(max_index, currentInterval.second);
175  currentInterval.second = max_index;
176  intervals.push_back(interval);
177  }
178  else {
179  result.push_back(currentInterval.second);
180  intervals.pop_back();
181  }
182  }
183 
184  approx_polygon.reserve(result.size());
185  if (refine) {
186  std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f>> lines(
187  result.size());
188  std::reverse(result.begin(), result.end());
189  for (std::size_t rIdx = 0; rIdx < result.size(); ++rIdx) {
190  std::size_t nIdx = rIdx + 1;
191  if (nIdx == result.size())
192  nIdx = 0;
193 
194  Eigen::Vector2f centroid = Eigen::Vector2f::Zero();
195  Eigen::Matrix2f covariance = Eigen::Matrix2f::Zero();
196  std::size_t pIdx = result[rIdx];
197  unsigned num_points = 0;
198  if (pIdx > result[nIdx]) {
199  num_points = static_cast<unsigned>(polygon.size()) - pIdx;
200  for (; pIdx < polygon.size(); ++pIdx) {
201  covariance.coeffRef(0) += polygon[pIdx].x * polygon[pIdx].x;
202  covariance.coeffRef(1) += polygon[pIdx].x * polygon[pIdx].y;
203  covariance.coeffRef(3) += polygon[pIdx].y * polygon[pIdx].y;
204  centroid[0] += polygon[pIdx].x;
205  centroid[1] += polygon[pIdx].y;
206  }
207  pIdx = 0;
208  }
209 
210  num_points += result[nIdx] - pIdx;
211  for (; pIdx < result[nIdx]; ++pIdx) {
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 (std::abs(direction.dot(normal)) > float(M_SQRT1_2)) {
239  std::swap(normal[0], normal[1]);
240  normal[0] = -normal[0];
241  }
242 
243  // needs to be on the left side of the edge
244  if (direction[0] * normal[1] < direction[1] * normal[0])
245  normal *= -1.0;
246 
247  lines[rIdx].head<2>().matrix() = normal;
248  lines[rIdx][2] = -normal.dot(centroid);
249  }
250 
251  float threshold2 = threshold * threshold;
252  for (std::size_t rIdx = 0; rIdx < lines.size(); ++rIdx) {
253  std::size_t nIdx = rIdx + 1;
254  if (nIdx == result.size())
255  nIdx = 0;
256 
257  Eigen::Vector3f vertex = lines[rIdx].cross(lines[nIdx]);
258  vertex /= vertex[2];
259  vertex[2] = 0.0;
260 
261  PointT point;
262  // test whether we need another edge since the intersection point is too far away
263  // from the original vertex
264  Eigen::Vector3f pq = polygon[result[nIdx]].getVector3fMap() - vertex;
265  pq[2] = 0.0;
266 
267  float distance = pq.squaredNorm();
268  if (distance > threshold2) {
269  // test whether the old point is inside the new polygon or outside
270  if ((pq[0] * lines[rIdx][0] + pq[1] * lines[rIdx][1] < 0.0) &&
271  (pq[0] * lines[nIdx][0] + pq[1] * lines[nIdx][1] < 0.0)) {
272  float distance1 = lines[rIdx][0] * polygon[result[nIdx]].x +
273  lines[rIdx][1] * polygon[result[nIdx]].y + lines[rIdx][2];
274  float distance2 = lines[nIdx][0] * polygon[result[nIdx]].x +
275  lines[nIdx][1] * polygon[result[nIdx]].y + lines[nIdx][2];
276 
277  point.x = polygon[result[nIdx]].x - distance1 * lines[rIdx][0];
278  point.y = polygon[result[nIdx]].y - distance1 * lines[rIdx][1];
279 
280  approx_polygon.push_back(point);
281 
282  vertex[0] = polygon[result[nIdx]].x - distance2 * lines[nIdx][0];
283  vertex[1] = polygon[result[nIdx]].y - distance2 * lines[nIdx][1];
284  }
285  }
286  point.getVector3fMap() = vertex;
287  approx_polygon.push_back(point);
288  }
289  }
290  else {
291  // we have a new polygon in results, but inverted (clockwise <-> counter-clockwise)
292  for (std::vector<unsigned>::reverse_iterator it = result.rbegin();
293  it != result.rend();
294  ++it)
295  approx_polygon.push_back(polygon[*it]);
296  }
297 }
298 
299 #endif // PCL_GEOMETRY_POLYGON_OPERATIONS_HPP_
M_SQRT1_2
#define M_SQRT1_2
Definition: pcl_macros.h:208
pcl::geometry::distance
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
pcl::approximatePolygon
void approximatePolygon(const PlanarPolygon< PointT > &polygon, PlanarPolygon< PointT > &approx_polygon, float threshold, bool refine=false, bool closed=true)
see approximatePolygon2D
Definition: polygon_operations.hpp:44
pcl::approximatePolygon2D
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.
Definition: polygon_operations.hpp:80
pcl::PointCloud::VectorType
std::vector< PointT, Eigen::aligned_allocator< PointT > > VectorType
Definition: point_cloud.h:411
pcl::PointXYZRGB
A point structure representing Euclidean xyz coordinates, and the RGB color.
Definition: point_types.hpp:674
pcl::eigen22
void eigen22(const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector)
determine the smallest eigenvalue and its corresponding eigenvector
Definition: eigen.hpp:133
pcl::PointCloud::reserve
void reserve(std::size_t n)
Definition: point_cloud.h:445
pcl::PlanarPolygon::getCoefficients
Eigen::Vector4f & getCoefficients()
Getter for the coefficients.
Definition: planar_polygon.h:116
pcl::PointCloud::resize
void resize(std::size_t count)
Resizes the container to contain count elements.
Definition: point_cloud.h:462
pcl::PointCloud::size
std::size_t size() const
Definition: point_cloud.h:443
pcl::PointCloud::clear
void clear()
Removes all points in a cloud and sets the width and height to 0.
Definition: point_cloud.h:874
pcl::PlanarPolygon
PlanarPolygon represents a planar (2D) polygon, potentially in a 3D space.
Definition: planar_polygon.h:52
pcl::PointCloud::push_back
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.
Definition: point_cloud.h:652
pcl::PlanarPolygon::getContour
pcl::PointCloud< PointT >::VectorType & getContour()
Getter for the contour / boundary.
Definition: planar_polygon.h:83