Exemplo n.º 1
0
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());
 }