table_object_cluster.h

Go to the documentation of this file.

#ifndef __TABLE_OBJECT_CLUSTER_H__
#define __TABLE_OBJECT_CLUSTER_H__

// PCL includes
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <Eigen/StdVector>
#include <pcl/ModelCoefficients.h>
#include <pcl/PointIndices.h>

struct BoundingBox
{
  Eigen::Vector4f min_pt;
  Eigen::Vector4f max_pt;
  Eigen::Affine3f pose;
};


template<typename Point>
class TableObjectCluster
{
public:

  typedef typename pcl::PointCloud<Point>::Ptr PointCloudPtr;
  typedef typename pcl::PointCloud<Point>::ConstPtr PointCloudConstPtr;

  TableObjectCluster()
  {
    height_min_ = -0.5;
    height_max_ = -0.03;
    min_cluster_size_ = 30;
    cluster_tolerance_ = 0.03;

  };
  ~TableObjectCluster() {};

  void
  extractTableRoi(PointCloudPtr& hull,
                  pcl::PointIndices& pc_roi);

  void
  extractTableRoi2(const PointCloudConstPtr& pc_in,
                   PointCloudPtr& hull,
                   Eigen::Vector4f& plane_coeffs,
                   pcl::PointCloud<Point>& pc_roi);

  void
  removeKnownObjects(const PointCloudConstPtr& pc_roi,
                     std::vector<BoundingBox>& bounding_boxes,
                     const PointCloudPtr& pc_roi_red);


  void
  calculateBoundingBox(const PointCloudPtr& cloud,
                       const pcl::PointIndices& indices,
                       const Eigen::Vector3f& plane_normal,
                       const Eigen::Vector3f& plane_point,
                       Eigen::Vector3f& position,
                       Eigen::Quaternion<float>& orientation,
                       Eigen::Vector3f& size);

  void
  extractClusters(const pcl::PointIndices::Ptr& pc_roi,
                  std::vector<PointCloudPtr>& object_clusters,
                  std::vector<pcl::PointIndices>& object_cluster_indices);

  void
  calculateBoundingBoxesOld(pcl::PointIndices::Ptr& pc_roi,
                         std::vector<PointCloudPtr >& object_clusters,
                     std::vector<pcl::PointCloud<pcl::PointXYZ> >& bounding_boxes);


  void
  setInputCloud(const PointCloudPtr&  cloud) {input_ = cloud;}

  void
  setPrismHeight(double height_min, double height_max)
  {
    height_min_ = height_min;
    height_max_ = height_max;
  }

  void
  setClusterParams(int min_cluster_size, double cluster_tolerance)
  {
    min_cluster_size_ = min_cluster_size;
    cluster_tolerance_ = cluster_tolerance;
  }

protected:
  PointCloudPtr input_;
  double height_min_;           
  double height_max_;           
  int min_cluster_size_;        
  double cluster_tolerance_;    

};

#endif /* __TABLE_OBJECT_CLUSTER_H__ */