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"; }