36 #ifndef JSK_RECOGNITION_UTILS_GEO_CONVEX_POLYGON_H_
37 #define JSK_RECOGNITION_UTILS_GEO_CONVEX_POLYGON_H_
40 #include <pcl/segmentation/extract_polygonal_prism_data.h>
41 #include <pcl/filters/project_inliers.h>
42 #include <pcl/surface/convex_hull.h>
48 class ConvexPolygon:
public Polygon
52 typedef Eigen::Vector3f
Vertex;
53 typedef std::vector<Eigen::Vector3f,
54 Eigen::aligned_allocator<Eigen::Vector3f> >
Vertices;
58 const std::vector<float>& coefficients);
60 virtual void project(
const Eigen::Vector3f&
p, Eigen::Vector3f& output);
61 virtual void project(
const Eigen::Vector3d&
p, Eigen::Vector3d& output);
62 virtual void project(
const Eigen::Vector3d&
p, Eigen::Vector3f& output);
63 virtual void project(
const Eigen::Vector3f&
p, Eigen::Vector3d& output);
65 Eigen::Vector3f& output);
67 Eigen::Affine3f& output);
72 virtual Ptr magnify(
const double scale_factor);
78 const Eigen::Vector3f&
p,
double distance_threshold);
80 const Eigen::Vector3f&
p,
double distance_threshold,
81 double& output_distance);
90 template<
class Po
intT>
92 const typename pcl::PointCloud<PointT>::Ptr cloud,
93 const pcl::PointIndices::Ptr inliers,
94 const pcl::ModelCoefficients::Ptr coefficients) {
95 typedef typename pcl::PointCloud<PointT> POINTCLOUD;
96 typename POINTCLOUD::Ptr projected_cloud(
new pcl::PointCloud<PointT>);
98 if (inliers->indices.size() == 0) {
102 pcl::ProjectInliers<PointT> proj;
103 proj.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE);
104 proj.setInputCloud(cloud);
105 proj.setModelCoefficients(coefficients);
106 proj.setIndices(inliers);
107 proj.filter(*projected_cloud);
111 typename POINTCLOUD::Ptr convex_cloud(
new pcl::PointCloud<PointT>);
112 pcl::ConvexHull<PointT> chull;
113 chull.setDimension(2);
114 chull.setInputCloud (projected_cloud);
115 chull.reconstruct (*convex_cloud);
116 if (convex_cloud->points.size() > 0) {
119 for (
size_t i = 0;
i < convex_cloud->points.size();
i++) {
120 Eigen::Vector3f v(convex_cloud->points[
i].getVector3fMap());