36 #ifndef PCL_GEOMETRY_POLYGON_OPERATIONS_HPP_
37 #define PCL_GEOMETRY_POLYGON_OPERATIONS_HPP_
39 #include <pcl/geometry/polygon_operations.h>
42 template<
typename Po
intT>
void
48 Eigen::Vector3f rotation_axis (coefficients[1], -coefficients[0], 0.0f);
49 rotation_axis.normalize ();
51 float rotation_angle = acosf (coefficients [2]);
52 Eigen::Affine3f transformation = Eigen::Translation3f (0, 0, coefficients [3]) * Eigen::AngleAxisf (rotation_angle, rotation_axis);
55 for (std::size_t pIdx = 0; pIdx < polygon2D.
size (); ++pIdx)
56 polygon2D [pIdx].getVector3fMap () = transformation * contour [pIdx].getVector3fMap ();
59 approximatePolygon2D<PointT> (polygon2D, approx_polygon2D, threshold, refine, closed);
62 approx_contour.
resize (approx_polygon2D.
size ());
64 Eigen::Affine3f inv_transformation = transformation.inverse ();
65 for (std::size_t pIdx = 0; pIdx < approx_polygon2D.
size (); ++pIdx)
66 approx_contour [pIdx].getVector3fMap () = inv_transformation * approx_polygon2D [pIdx].getVector3fMap ();
70 template <
typename Po
intT>
void
73 float threshold,
bool refine,
bool closed)
75 approx_polygon.
clear ();
76 if (polygon.
size () < 3)
79 std::vector<std::pair<unsigned, unsigned> > intervals;
80 std::pair<unsigned,unsigned> interval (0, 0);
84 float max_distance = .0f;
85 for (std::size_t idx = 1; idx < polygon.
size (); ++idx)
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);
93 interval.second = idx;
97 for (std::size_t idx = 1; idx < polygon.
size (); ++idx)
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);
105 interval.first = idx;
109 if (max_distance < threshold * threshold)
112 intervals.push_back (interval);
113 std::swap (interval.first, interval.second);
114 intervals.push_back (interval);
119 interval.second =
static_cast<unsigned int> (polygon.
size ()) - 1;
120 intervals.push_back (interval);
123 std::vector<unsigned> result;
125 while (!intervals.empty ())
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;
132 float linelen = 1.0f / std::sqrt (line_x * line_x + line_y * line_y);
138 float max_distance = 0.0;
139 unsigned first_index = currentInterval.first + 1;
140 unsigned max_index = 0;
143 if (currentInterval.first > currentInterval.second)
145 for (std::size_t idx = first_index; idx < polygon.
size(); idx++)
147 float distance = std::abs (line_x * polygon[idx].x + line_y * polygon[idx].y + line_d);
157 for (
unsigned int idx = first_index; idx < currentInterval.second; idx++)
159 float distance = std::abs (line_x * polygon[idx].x + line_y * polygon[idx].y + line_d);
167 if (max_distance > threshold)
169 std::pair<unsigned, unsigned> interval (max_index, currentInterval.second);
170 currentInterval.second = max_index;
171 intervals.push_back (interval);
175 result.push_back (currentInterval.second);
176 intervals.pop_back ();
180 approx_polygon.
reserve (result.size ());
183 std::vector<Eigen::Vector3f, Eigen::aligned_allocator<Eigen::Vector3f> > lines (result.size ());
184 std::reverse (result.begin (), result.end ());
185 for (std::size_t rIdx = 0; rIdx < result.size (); ++rIdx)
187 std::size_t nIdx = rIdx + 1;
188 if (nIdx == result.size ())
191 Eigen::Vector2f centroid = Eigen::Vector2f::Zero ();
192 Eigen::Matrix2f covariance = Eigen::Matrix2f::Zero ();
193 std::size_t pIdx = result[rIdx];
194 unsigned num_points = 0;
195 if (pIdx > result[nIdx])
197 num_points =
static_cast<unsigned> (polygon.
size ()) - pIdx;
198 for (; pIdx < polygon.
size (); ++pIdx)
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;
209 num_points += result[nIdx] - pIdx;
210 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;
219 covariance.coeffRef (2) = covariance.coeff (1);
221 float norm = 1.0f / float (num_points);
224 covariance.coeffRef (0) -= centroid [0] * centroid [0];
225 covariance.coeffRef (1) -= centroid [0] * centroid [1];
226 covariance.coeffRef (3) -= centroid [1] * centroid [1];
229 Eigen::Vector2f normal;
230 eigen22 (covariance, eval, normal);
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 ();
238 if (std::abs (direction.dot (normal)) >
float(
M_SQRT1_2))
240 std::swap (normal [0], normal [1]);
241 normal [0] = -normal [0];
245 if (direction [0] * normal [1] < direction [1] * normal [0])
248 lines [rIdx].head<2> ().matrix () = normal;
249 lines [rIdx] [2] = -normal.dot (centroid);
252 float threshold2 = threshold * threshold;
253 for (std::size_t rIdx = 0; rIdx < lines.size (); ++rIdx)
255 std::size_t nIdx = rIdx + 1;
256 if (nIdx == result.size ())
259 Eigen::Vector3f vertex = lines [rIdx].cross (lines [nIdx]);
260 vertex /= vertex [2];
265 Eigen::Vector3f pq = polygon [result[nIdx]].getVector3fMap () - vertex;
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) )
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];
278 point.x = polygon[result[nIdx]].x - distance1 * lines [rIdx] [0];
279 point.y = polygon[result[nIdx]].y - distance1 * lines [rIdx] [1];
283 vertex [0] = polygon[result[nIdx]].x - distance2 * lines [nIdx] [0];
284 vertex [1] = polygon[result[nIdx]].y - distance2 * lines [nIdx] [1];
287 point.getVector3fMap () = vertex;
294 for (std::vector<unsigned>::reverse_iterator it = result.rbegin (); it != result.rend (); ++it)
295 approx_polygon.
push_back (polygon [*it]);
299 #endif // PCL_GEOMETRY_POLYGON_OPERATIONS_HPP_