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
approximatePolygon(const PlanarPolygon<PointT> & polygon,PlanarPolygon<PointT> & approx_polygon,float threshold,bool refine,bool closed)44 pcl::approximatePolygon(const PlanarPolygon<PointT>& polygon,
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
approximatePolygon2D(const typename pcl::PointCloud<PointT>::VectorType & polygon,typename pcl::PointCloud<PointT>::VectorType & approx_polygon,float threshold,bool refine,bool closed)80 pcl::approximatePolygon2D(const typename pcl::PointCloud<PointT>::VectorType& polygon,
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 / ::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_
300