cluster_ros_conversion.h

Go to the documentation of this file.

#ifndef CLUSTER_ROS_CONVERSION_H__
#define CLUSTER_ROS_CONVERSION_H__

#include <cob_3d_segmentation/cluster_conversion.h>
#include <cob_3d_mapping_msgs/ShapeArray.h>

namespace cob_3d_segmentation
{
  template<
    typename PointT = pcl::PointXYZRGB,
    typename ClusterHdlT = DepthClusterHandler<PointLabel, PointT, pcl::Normal>
    >
  class ClusterROSConversion : public ClusterConversion<PointT, ClusterHdlT>
  {
  public:
    typedef ClusterConversion<PointT, ClusterHdlT> Base;

    using Base::min_size_;
    using Base::max_dist_;

    using Base::pe_;
    using Base::ch_;
    using Base::p_;

    bool clusterToShapeMsg(const typename Base::ClusterPtr& c, cob_3d_mapping_msgs::Shape& s)
    {
      PolygonContours<PolygonPoint> poly;
      pe_.outline(p_->width, p_->height, c->border_points, poly);
      if(!poly.polys_.size()) false; // continue, if no contours were found
      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(); }
      }

      Eigen::Vector3f centroid = c->getCentroid();

      s.id = 0;
      s.points.resize(poly.polys_.size());
      s.header.frame_id = p_->header.frame_id.c_str();

      typename Base::PointCloud::Ptr hull(new typename Base::PointCloud);
      for (int i = 0; i < (int)poly.polys_.size(); ++i)
      {
        if (i == max_idx)
        {
          s.holes.push_back(false);
          std::vector<PolygonPoint>::iterator it = poly.polys_[i].begin();
          for ( ; it != poly.polys_[i].end(); ++it) {
            hull->push_back( p_->points[ it->x + it->y * p_->width ] );
          }
        }
        else
        {
          s.holes.push_back(true);
          std::vector<PolygonPoint>::reverse_iterator it = poly.polys_[i].rbegin();
          for ( ; it != poly.polys_[i].rend(); ++it) {
            hull->push_back( p_->points[ it->x + it->y * p_->width ] );
          }
        }
        hull->height = 1;
        hull->width = hull->size();
        pcl::toROSMsg(*hull, s.points[i]);
        hull->clear();
      }

      s.centroid.x = centroid[0];
      s.centroid.y = centroid[1];
      s.centroid.z = centroid[2];
      s.type = cob_3d_mapping_msgs::Shape::POLYGON;
      s.params.resize(4);
      s.params[0] = c->pca_point_comp3(0);
      s.params[1] = c->pca_point_comp3(1);
      s.params[2] = c->pca_point_comp3(2);
      s.params[3] = fabs(centroid.dot(c->pca_point_comp3)); // d
      return true;
    }

    void convertToShapeArray(cob_3d_mapping_msgs::ShapeArray& sa)
    {
      sa.header = p_->header;
      sa.header.frame_id = p_->header.frame_id.c_str();

      for(typename Base::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_save_plane) continue;

        sa.shapes.push_back(cob_3d_mapping_msgs::Shape());
        if(!clusterToShapeMsg(c, sa.shapes.back())) {
          sa.shapes.pop_back();
        }
      }
    }
  };
}

#endif