cluster_conversion.h

Go to the documentation of this file.

#ifndef CLUSTER_CONVERSION_H__
#define CLUSTER_CONVERSION_H__

#include <pcl/point_types.h>

#include <cob_3d_mapping_common/point_types.h>
#include <cob_3d_mapping_common/polygon.h>
#include <cob_3d_segmentation/polygon_extraction/polygon_extraction.h>
#include <cob_3d_segmentation/cluster_handler.h>

namespace cob_3d_segmentation
{
  template<
    typename PointT = pcl::PointXYZRGB,
    typename ClusterHdlT = DepthClusterHandler<PointLabel, PointT, pcl::Normal>
  >
  class ClusterConversion
  {
    public:
    typedef pcl::PointCloud<PointT> PointCloud;
    typedef typename PointCloud::ConstPtr PointCloudConstPtr;
    typedef typename ClusterHdlT::Ptr ClusterHdlPtr;
    typedef typename ClusterHdlT::ClusterPtr ClusterPtr;

    public:
    ClusterConversion()
    : min_size_(100)
    , max_dist_(5.0f)
    , colorize_(false)
    {}

    virtual ~ClusterConversion() {}

    inline void setClusterHandler(const ClusterHdlPtr& ch) { ch_ = ch; }
    inline void setInputCloud(const PointCloudConstPtr& p) { p_ = p; }
    inline void setMinClusterSize(int size) { min_size_ = size; }
    inline void setMaxCentroidDistance(float z_distance) { max_dist_ = z_distance; }
    inline void setColor(bool enable) { colorize_ = enable; }

    bool clusterToPolygon(const ClusterPtr& c, cob_3d_mapping::Polygon::Ptr& pg)
    {
      cob_3d_segmentation::PolygonContours<cob_3d_segmentation::PolygonPoint> poly;
      pe_.outline(p_->width,p_->height,c->border_points,poly);
      if(!poly.polys_.size()) return false;
      /*for (int i = 0; i < (int)poly.polys_.size(); ++i)
      {
        std::cout << "c " << i << ": " << std::endl;
        for (int idx = 0; idx < poly.polys_[i].size(); ++idx)
        {
          std::cout << poly.polys_[i][idx].x << "," << poly.polys_[i][idx].y << "->";
        }
        std::cout << std::endl;
      }*/
      int max_idx=0, max_size=0;
      /*for (int i=0; i<(int)poly.polys_.size(); ++i)
      {
        if( (int)poly.polys_[i].size() > max_size)
        {
          max_idx = i;
          max_size = poly.polys_[i].size();
        }
      }
      std::cout << "max_idx " << max_idx << std::endl;*/
      //std::cout << c->border_points.size() << "->" << poly.polys_[0].size() << std::endl;

      std::vector<pcl::PointCloud<pcl::PointXYZ> > contours_3d;
      std::vector<bool> holes;
      for (int i = 0; i < (int)poly.polys_.size(); ++i)
      {
        int n_points = poly.polys_[i].size();
        int reverse;
        pcl::PointCloud<pcl::PointXYZ> contour;
        contour.resize(n_points);
        if (i == max_idx) { holes.push_back(false); reverse = 0; }
        else              { holes.push_back(true);  reverse = n_points - 1; }

        for (int idx = 0; idx < n_points; ++idx)
        {
          PolygonPoint& pp = (poly.polys_[i])[abs(reverse - idx)];
          contour[idx].getVector3fMap() = p_->points[ pp.x + pp.y * p_->width ].getVector3fMap();
        }
        contour.height = 1;
        contour.width = contour.size();
        contours_3d.push_back(contour);
      }

      std::vector<float> color(4, 1);
      if(colorize_)
      {
        Eigen::Vector3i col_tmp( c->computeDominantColorVector() );
        float temp_inv = 1.0f/255.0f;
        color[0] = float(col_tmp(0)) * temp_inv;
        color[1] = float(col_tmp(1)) * temp_inv;
        color[2] = float(col_tmp(2)) * temp_inv;
      }
      else
      {
        color[0] = 0.0f;
        color[1] = 0.0f;
        color[2] = 1.0f;
      }
      pg.reset(new cob_3d_mapping::Polygon(c->id(),
                                           c->pca_point_comp3,
                                           c->getCentroid(),
                                           contours_3d,
                                           holes,
                                           color));

      return true;
    }

    void convertToPolygons(std::vector<cob_3d_mapping::Polygon::Ptr> polygons)
    {
      for(ClusterPtr c = ch_->begin(); c != ch_->end(); ++c)
      {
        if(c->size() < min_size_) continue;
        if(c->getCentroid()(2) > max_dist_) continue;
        if(c->size() <= ceil(1.1f * (float)c->border_points.size())) continue;

        ch_->computeClusterComponents(c);
        if(!c->is_planar_) continue;

        cob_3d_mapping::Polygon::Ptr p;
        if(clusterToPolygon(c, p)) {
          polygons.push_back(p);
        }
      }
    }

    protected:
    int min_size_;
    float max_dist_;
    bool colorize_;

    PolygonExtraction pe_;
    ClusterHdlPtr ch_;
    PointCloudConstPtr p_;
  };
}

#endif