zyonz_obtain_two_clustered_leaves_alg.cpp

Go to the documentation of this file.

#include "zyonz_obtain_two_clustered_leaves_alg.h"
#include "pcl/kdtree/kdtree_flann.h"
#include "pcl/segmentation/extract_clusters.h"

ZyonzObtainTwoClusteredLeavesAlgorithm::ZyonzObtainTwoClusteredLeavesAlgorithm(void)
{
}

ZyonzObtainTwoClusteredLeavesAlgorithm::~ZyonzObtainTwoClusteredLeavesAlgorithm(void)
{
}

void ZyonzObtainTwoClusteredLeavesAlgorithm::config_update(Config& new_cfg, uint32_t level)
{
  this->lock();

  // save the current configuration
  this->config_=new_cfg;
  
  this->unlock();
}

// ZyonzObtainTwoClusteredLeavesAlgorithm Public API
bool ZyonzObtainTwoClusteredLeavesAlgorithm::extract_clusters(sensor_msgs::PointCloud2::ConstPtr& msg)
{

  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
  pcl::fromROSMsg(*msg, *cloud);

  cluster_a_img_.data.clear();
  cluster_b_img_.data.clear();
  cluster_a_img_.header   = cluster_b_img_.header          = msg->header;
  cluster_a_img_.height   = cluster_b_img_.height          = msg->height;
  cluster_a_img_.width    = cluster_b_img_.width           = msg->width;
  cluster_a_img_.encoding = cluster_b_img_.encoding        = "mono8";
  cluster_a_img_.step     = cluster_b_img_.step            = msg->width;
  cluster_a_img_.data.resize(msg->width * msg->height);
  cluster_b_img_.data.resize(msg->width * msg->height);
  for (unsigned int rr=0; rr<msg->height; ++rr)
    for (unsigned int cc=0; cc<msg->width; ++cc ){
      int idx0 = rr*msg->width + cc;
      cluster_a_img_.data[idx0] = 0.0; // Initialize image to white 
    }

  ROS_DEBUG("height: %d, width: %d", msg->height, msg->width);

  //Creating the KdTree object for the search method of the extraction
  pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);

  tree->setInputCloud (cloud);
  
  std::vector<pcl::PointIndices> cluster_indices;
  pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
  ec.setClusterTolerance (0.02);
  ec.setMinClusterSize (100);
  ec.setMaxClusterSize (25000);
  ec.setSearchMethod (tree);
  ec.setInputCloud (cloud);
  ec.extract (cluster_indices);
 
  if(cluster_indices.size() < 2)
  {
    ROS_INFO("ZyonzObtainTwoClusteredLeavesAlgorithm::extract_clusters: ERROR: Not enough clusters");
    return false;
  }

  cluster_a_.header   = cluster_b_.header   = cloud->header;
  cluster_a_.width    = cluster_b_.width    = cloud->width;
  cluster_a_.height   = cluster_b_.height   = cloud->height;
  cluster_a_.is_dense = cluster_b_.is_dense = false;
  cluster_a_.resize(cloud->width*cloud->height);
  cluster_b_.resize(cloud->width*cloud->height);

  // Initialize to NaN the pointclouds
  pcl::PointCloud<pcl::PointXYZRGB>::iterator b_it = cluster_b_.begin ();
  for(pcl::PointCloud<pcl::PointXYZRGB>::iterator a_it = cluster_a_.begin(); a_it != cluster_a_.end (); ++a_it, ++b_it)
     a_it->z = b_it->z = std::numeric_limits<float>::quiet_NaN ();

  // fill in cluster_a
  std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin ();
  for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit){
    cluster_a_img_.data[*pit] = 255;
    cluster_a_[*pit] = cloud->points[*pit];
  //  std::cout << "pixel_position: " << *pit << std::endl;
    //std::cout << "point: " << cloud->points[*pit] << std::endl;
  }

  // fill in cluster_b
  ++it;
  for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit){
    cluster_b_img_.data[*pit] = 255;
    cluster_b_[*pit] = cloud->points[*pit];
  }

  return true;
}

pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr ZyonzObtainTwoClusteredLeavesAlgorithm::get_cluster_a(void)
{
  return cluster_a_.makeShared();
}

pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr ZyonzObtainTwoClusteredLeavesAlgorithm::get_cluster_b(void)
{
  return cluster_b_.makeShared();
}

sensor_msgs::Image* ZyonzObtainTwoClusteredLeavesAlgorithm::get_cluster_a_img(void)
{
  return &cluster_a_img_;
}

sensor_msgs::Image* ZyonzObtainTwoClusteredLeavesAlgorithm::get_cluster_b_img(void)
{
  return &cluster_b_img_;
}