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());
 }
Beispiel #3
0
__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";
}
Beispiel #4
0
__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";
}