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 (
unsigned 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 (
unsigned 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 (
unsigned 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);
90 if (distance > max_distance)
92 max_distance = distance;
93 interval.second = idx;
97 for (
unsigned 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);
102 if (distance > max_distance)
104 max_distance = distance;
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 / sqrtf (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 (
unsigned idx = first_index; idx < polygon.
size(); idx++)
147 float distance = fabsf (line_x * polygon[idx].x + line_y * polygon[idx].y + line_d);
148 if (distance > max_distance)
150 max_distance = distance;
157 for (
unsigned int idx = first_index; idx < currentInterval.second; idx++)
159 float distance = fabsf (line_x * polygon[idx].x + line_y * polygon[idx].y + line_d);
160 if (distance > max_distance)
162 max_distance = distance;
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> lines (result.size ());
184 std::reverse (result.begin (), result.end ());
185 for (
unsigned rIdx = 0; rIdx < result.size (); ++rIdx)
187 unsigned nIdx = rIdx + 1;
188 if (nIdx == result.size ())
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])
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 (fabs (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 (
unsigned rIdx = 0; rIdx < lines.size (); ++rIdx)
255 unsigned 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;
268 float distance = pq.squaredNorm ();
269 if (distance > threshold2)
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_
Eigen::Vector4f & getCoefficients()
Getter for the coefficients.
PlanarPolygon represents a planar (2D) polygon, potentially in a 3D space.
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 eigen22(const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector)
determine the smallest eigenvalue and its corresponding eigenvector
pcl::PointCloud< PointT >::VectorType & getContour()
Getter for the contour / boundary.
std::vector< PointT, Eigen::aligned_allocator< PointT > > VectorType
void approximatePolygon(const PlanarPolygon< PointT > &polygon, PlanarPolygon< PointT > &approx_polygon, float threshold, bool refine=false, bool closed=true)
see approximatePolygon2D
void resize(size_t n)
Resize the cloud.
A point structure representing Euclidean xyz coordinates, and the RGB color.
void clear()
Removes all points in a cloud and sets the width and height to 0.
void push_back(const PointT &pt)
Insert a new point in the cloud, at the end of the container.