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