template<typename PointT> void
pcl16::approximatePolygon (const PlanarPolygon<PointT>& polygon, PlanarPolygon<PointT>& approx_polygon, float threshold, bool refine, bool closed)
{
const Eigen::Vector4f& coefficients = polygon.getCoefficients ();
const typename pcl16::PointCloud<PointT>::VectorType &contour = polygon.getContour ();
Eigen::Vector3f rotation_axis (coefficients[1], -coefficients[0], 0.0f);
rotation_axis.normalize ();
float rotation_angle = acosf (coefficients [2]);
Eigen::Affine3f transformation = Eigen::Translation3f (0, 0, coefficients [3]) * Eigen::AngleAxisf (rotation_angle, rotation_axis);
typename pcl16::PointCloud<PointT>::VectorType polygon2D (contour.size ());
for (unsigned pIdx = 0; pIdx < polygon2D.size (); ++pIdx)
polygon2D [pIdx].getVector3fMap () = transformation * contour [pIdx].getVector3fMap ();
typename pcl16::PointCloud<PointT>::VectorType approx_polygon2D;
approximatePolygon2D<PointT> (polygon2D, approx_polygon2D, threshold, refine, closed);
typename pcl16::PointCloud<PointT>::VectorType &approx_contour = approx_polygon.getContour ();
approx_contour.resize (approx_polygon2D.size ());
Eigen::Affine3f inv_transformation = transformation.inverse ();
for (unsigned pIdx = 0; pIdx < approx_polygon2D.size (); ++pIdx)
approx_contour [pIdx].getVector3fMap () = inv_transformation * approx_polygon2D [pIdx].getVector3fMap ();
}
static double getClusterCentroidHeight(const PointCloud& cluster, const PlanarPolygon& polygon)
{
Eigen::Vector4f centroid;
pcl16::compute3DCentroid(cluster, centroid);
centroid[3] = 1;
return centroid.dot(polygon.getCoefficients());
}
__attribute__((noinline)) void TimeVecGeom(PlanarPolygon const &poly, SOA3D<Precision> const &container, int &count)
{
high_resolution_clock::time_point t1 = high_resolution_clock::now();
const auto S = container.size();
for (size_t i = 0; i < S; ++i) {
if (poly.Contains(container[i])) count++;
}
high_resolution_clock::time_point t2 = high_resolution_clock::now();
duration<double> time_span = duration_cast<duration<double>>(t2 - t1);
std::cout << "It took me " << time_span.count() << " seconds.\n";
}
__attribute__((noinline)) void TimeVecGeomSafety(PlanarPolygon const &poly, SOA3D<Precision> const &container,
double &totalsafety)
{
high_resolution_clock::time_point t1 = high_resolution_clock::now();
const auto S = container.size();
for (size_t i = 0; i < S; ++i) {
int segmentid;
totalsafety += std::sqrt(poly.SafetySqr(container[i], segmentid));
}
high_resolution_clock::time_point t2 = high_resolution_clock::now();
duration<double> time_span = duration_cast<duration<double>>(t2 - t1);
std::cout << "It took me " << time_span.count() << " seconds.\n";
}