void RegionGrowingMultiplePlaneSegmentation::segment(
    const sensor_msgs::PointCloud2::ConstPtr& msg,
    const sensor_msgs::PointCloud2::ConstPtr& normal_msg)
  {
    boost::mutex::scoped_lock lock(mutex_);
    if (!done_initialization_) {
      return;
    }
    vital_checker_->poke();
    {
      jsk_recognition_utils::ScopedWallDurationReporter r
        = timer_.reporter(pub_latest_time_, pub_average_time_);
      pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
      pcl::PointCloud<pcl::Normal>::Ptr normal(new pcl::PointCloud<pcl::Normal>);
      pcl::fromROSMsg(*msg, *cloud);
      pcl::fromROSMsg(*normal_msg, *normal);
      // concatenate fields
      pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr
        all_cloud (new pcl::PointCloud<pcl::PointXYZRGBNormal>);
      pcl::concatenateFields(*cloud, *normal, *all_cloud);
      pcl::PointIndices::Ptr indices (new pcl::PointIndices);
      for (size_t i = 0; i < all_cloud->points.size(); i++) {
        if (!isnan(all_cloud->points[i].x) &&
            !isnan(all_cloud->points[i].y) &&
            !isnan(all_cloud->points[i].z) &&
            !isnan(all_cloud->points[i].normal_x) &&
            !isnan(all_cloud->points[i].normal_y) &&
            !isnan(all_cloud->points[i].normal_z) &&
            !isnan(all_cloud->points[i].curvature)) {
          if (all_cloud->points[i].curvature < max_curvature_) {
            indices->indices.push_back(i);
          }
        }
      }
      pcl::ConditionalEuclideanClustering<pcl::PointXYZRGBNormal> cec (true);
    
      // vector of pcl::PointIndices
      pcl::IndicesClustersPtr clusters (new pcl::IndicesClusters);
      cec.setInputCloud (all_cloud);
      cec.setIndices(indices);
      cec.setClusterTolerance(cluster_tolerance_);
      cec.setMinClusterSize(min_size_);
      //cec.setMaxClusterSize(max_size_);
      {
        boost::mutex::scoped_lock lock2(global_custom_condigion_function_mutex);
        setCondifionFunctionParameter(angular_threshold_, distance_threshold_);
        cec.setConditionFunction(
          &RegionGrowingMultiplePlaneSegmentation::regionGrowingFunction);
        //ros::Time before = ros::Time::now();
        cec.segment (*clusters);
        // ros::Time end = ros::Time::now();
        // ROS_INFO("segment took %f sec", (before - end).toSec());
      }
      // Publish raw cluster information 
      // pcl::IndicesCluster is typdefed as std::vector<pcl::PointIndices>
      jsk_recognition_msgs::ClusterPointIndices ros_clustering_result;
      ros_clustering_result.cluster_indices
        = pcl_conversions::convertToROSPointIndices(*clusters, msg->header);
      ros_clustering_result.header = msg->header;
      pub_clustering_result_.publish(ros_clustering_result);

      // estimate planes
      std::vector<pcl::PointIndices::Ptr> all_inliers;
      std::vector<pcl::ModelCoefficients::Ptr> all_coefficients;
      jsk_recognition_msgs::PolygonArray ros_polygon;
      ros_polygon.header = msg->header;
      for (size_t i = 0; i < clusters->size(); i++) {
        pcl::PointIndices::Ptr plane_inliers(new pcl::PointIndices);
        pcl::ModelCoefficients::Ptr plane_coefficients(new pcl::ModelCoefficients);
        pcl::PointIndices::Ptr cluster = boost::make_shared<pcl::PointIndices>((*clusters)[i]);
        ransacEstimation(cloud, cluster,
                         *plane_inliers, *plane_coefficients);
        if (plane_inliers->indices.size() > 0) {
          jsk_recognition_utils::ConvexPolygon::Ptr convex
            = jsk_recognition_utils::convexFromCoefficientsAndInliers<pcl::PointXYZRGB>(
              cloud, plane_inliers, plane_coefficients);
          if (convex) {
            if (min_area_ > convex->area() || convex->area() > max_area_) {
              continue;
            }
            {
              // check direction consistency of coefficients and convex
              Eigen::Vector3f coefficient_normal(plane_coefficients->values[0],
                                                 plane_coefficients->values[1],
                                                 plane_coefficients->values[2]);
              if (convex->getNormalFromVertices().dot(coefficient_normal) < 0) {
                convex = boost::make_shared<jsk_recognition_utils::ConvexPolygon>(convex->flipConvex());
              }
            }
            // Normal should direct to origin
            {
              Eigen::Vector3f p = convex->getPointOnPlane();
              Eigen::Vector3f n = convex->getNormal();
              if (p.dot(n) > 0) {
                convex = boost::make_shared<jsk_recognition_utils::ConvexPolygon>(convex->flipConvex());
                Eigen::Vector3f new_normal = convex->getNormal();
                plane_coefficients->values[0] = new_normal[0];
                plane_coefficients->values[1] = new_normal[1];
                plane_coefficients->values[2] = new_normal[2];
                plane_coefficients->values[3] = convex->getD();
              }
            }
          
            geometry_msgs::PolygonStamped polygon;
            polygon.polygon = convex->toROSMsg();
            polygon.header = msg->header;
            ros_polygon.polygons.push_back(polygon);
            all_inliers.push_back(plane_inliers);
            all_coefficients.push_back(plane_coefficients);
          }
        }
      }
    
      jsk_recognition_msgs::ClusterPointIndices ros_indices;
      ros_indices.cluster_indices =
        pcl_conversions::convertToROSPointIndices(all_inliers, msg->header);
      ros_indices.header = msg->header;
      pub_inliers_.publish(ros_indices);
      jsk_recognition_msgs::ModelCoefficientsArray ros_coefficients;
      ros_coefficients.header = msg->header;
      ros_coefficients.coefficients =
        pcl_conversions::convertToROSModelCoefficients(
          all_coefficients, msg->header);
      pub_coefficients_.publish(ros_coefficients);
      pub_polygons_.publish(ros_polygon);
    }
  }
  bool SnapIt::processModelPlane(jsk_pcl_ros::CallSnapIt::Request& req,
                                 jsk_pcl_ros::CallSnapIt::Response& res) {
    // first build plane model
    geometry_msgs::PolygonStamped target_plane = req.request.target_plane;
    // check the size of the points
    if (target_plane.polygon.points.size() < 3) {
      NODELET_ERROR("not enough points included in target_plane");
      return false;
    }
    pcl::PointCloud<pcl::PointXYZ>::Ptr points_inside_pole (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
    EigenVector3fVector points;
    Eigen::Vector3f n, p;
    if (!extractPointsInsidePlanePole(target_plane, inliers, points, n, p)) {
      return false;
    }

    if (inliers->indices.size() < 3) {
      NODELET_ERROR("not enough points inside of the target_plane");
      return false;
    }
    
    pcl::ExtractIndices<pcl::PointXYZ> extract;
    extract.setInputCloud(input_);
    extract.setIndices(inliers);
    extract.filter(*points_inside_pole);

    publishPointCloud(debug_candidate_points_pub_, points_inside_pole);
    
    // estimate plane
    
    pcl::ModelCoefficients::Ptr plane_coefficients (new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr plane_inliers (new pcl::PointIndices);
    extractPlanePoints(points_inside_pole, plane_inliers, plane_coefficients,
                       n, req.request.eps_angle);

    if (plane_inliers->indices.size () == 0)
    {
      NODELET_ERROR ("Could not estimate a planar model for the given dataset.");
      return false;
    }

    // extract plane points
    pcl::PointCloud<pcl::PointXYZ>::Ptr plane_points (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::ProjectInliers<pcl::PointXYZ> proj;
    proj.setModelType (pcl::SACMODEL_PLANE);
    proj.setIndices (plane_inliers);
    proj.setInputCloud (points_inside_pole);
    proj.setModelCoefficients (plane_coefficients);
    proj.filter (*plane_points);
    publishPointCloud(debug_candidate_points_pub3_, plane_points);
    
    // next, compute convexhull and centroid
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_hull (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::ConcaveHull<pcl::PointXYZ> chull;
    chull.setInputCloud (plane_points);
    chull.setDimension(2);
    chull.setAlpha (0.1);
    
    chull.reconstruct (*cloud_hull);

    if (cloud_hull->points.size() < 3) {
      NODELET_ERROR("failed to estimate convex hull");
      return false;
    }
    publishConvexHullMarker(cloud_hull);
    
    Eigen::Vector4f C_new_4f;
    pcl::compute3DCentroid(*cloud_hull, C_new_4f);
    Eigen::Vector3f C_new;
    for (size_t i = 0; i < 3; i++) {
      C_new[i] = C_new_4f[i];
    }
    
    Eigen::Vector3f n_prime;
    n_prime[0] = plane_coefficients->values[0];
    n_prime[1] = plane_coefficients->values[1];
    n_prime[2] = plane_coefficients->values[2];
    plane_coefficients->values[3] = plane_coefficients->values[3] / n_prime.norm();
    n_prime.normalize();
    
    if (n_prime.dot(n) < 0) {
      n_prime = - n_prime;
      plane_coefficients->values[3] = - plane_coefficients->values[3];
    }
    
    Eigen::Vector3f n_cross = n.cross(n_prime);
    double theta = asin(n_cross.norm());

    Eigen::Quaternionf trans (Eigen::AngleAxisf(theta, n_cross.normalized()));
    
    // compute C
    Eigen::Vector3f C_orig(0, 0, 0);
    for (size_t i = 0; i < points.size(); i++) {
      C_orig = C_orig + points[i];
    }
    C_orig = C_orig / points.size();
    // compute C
    Eigen::Vector3f C = trans * C_orig;
    

    Eigen::Affine3f A = Eigen::Translation3f(C) * Eigen::Translation3f(C_new - C) * trans * Eigen::Translation3f(C_orig).inverse();
    tf::poseEigenToMsg((Eigen::Affine3d)A, res.transformation);

    geometry_msgs::PointStamped centroid;
    centroid.point.x = C_orig[0];
    centroid.point.y = C_orig[1];
    centroid.point.z = C_orig[2];
    centroid.header = target_plane.header;
    debug_centroid_pub_.publish(centroid);

    geometry_msgs::PointStamped centroid_transformed;
    centroid_transformed.point.x = C_new[0];
    centroid_transformed.point.y = C_new[1];
    centroid_transformed.point.z = C_new[2];
    centroid_transformed.header = target_plane.header;
    debug_centroid_after_trans_pub_.publish(centroid_transformed);
    
    return true;
  }
  bool SnapIt::processModelCylinder(jsk_pcl_ros::CallSnapIt::Request& req,
                                    jsk_pcl_ros::CallSnapIt::Response& res) {
    pcl::PointCloud<pcl::PointXYZ>::Ptr candidate_points (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
    Eigen::Vector3f n, C_orig;
    if (!extractPointsInsideCylinder(req.request.center,
                                     req.request.direction,
                                     req.request.radius,
                                     req.request.height,
                                     inliers, n, C_orig,
                                     1.3)) {
      return false;
    }
    if (inliers->indices.size() < 3) {
      NODELET_ERROR("not enough points inside of the target_plane");
      return false;
    }
    
    geometry_msgs::PointStamped centroid;
    centroid.point.x = C_orig[0];
    centroid.point.y = C_orig[1];
    centroid.point.z = C_orig[2];
    centroid.header = req.request.header;
    
    
    pcl::ExtractIndices<pcl::PointXYZ> extract;
    extract.setInputCloud(input_);
    extract.setIndices(inliers);
    extract.filter(*candidate_points);
    
    publishPointCloud(debug_candidate_points_pub_, candidate_points);


    // first, to remove plane we estimate the plane
    pcl::ModelCoefficients::Ptr plane_coefficients (new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr plane_inliers (new pcl::PointIndices);
    extractPlanePoints(candidate_points, plane_inliers, plane_coefficients,
                       n, req.request.eps_angle);
    if (plane_inliers->indices.size() == 0) {
      NODELET_ERROR ("plane estimation failed");
      return false;
    }

    // remove the points blonging to the plane
    pcl::PointCloud<pcl::PointXYZ>::Ptr points_inside_pole_wo_plane (new pcl::PointCloud<pcl::PointXYZ>);
    extract.setInputCloud (candidate_points);
    extract.setIndices (plane_inliers);
    extract.setNegative (true);
    extract.filter (*points_inside_pole_wo_plane);

    publishPointCloud(debug_candidate_points_pub2_, points_inside_pole_wo_plane);
    
    // normal estimation
    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
    ne.setSearchMethod (tree);
    ne.setInputCloud (points_inside_pole_wo_plane);
    ne.setKSearch (50);
    ne.compute (*cloud_normals);
    
    
    // segmentation
    pcl::ModelCoefficients::Ptr cylinder_coefficients (new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr cylinder_inliers (new pcl::PointIndices);
    // Create the segmentation object
    pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> seg;
    // Optional
    seg.setOptimizeCoefficients (true);
    // Mandatory
    seg.setModelType (pcl::SACMODEL_CYLINDER);
    seg.setMethodType (pcl::SAC_RANSAC);
    seg.setRadiusLimits (0.01, req.request.radius * 1.2);
    seg.setDistanceThreshold (0.05);
    
    seg.setInputCloud(points_inside_pole_wo_plane);
    seg.setInputNormals (cloud_normals);
    seg.setMaxIterations (10000);
    seg.setNormalDistanceWeight (0.1);
    seg.setAxis(n);
    if (req.request.eps_angle != 0.0) {
      seg.setEpsAngle(req.request.eps_angle);
    }
    else {
      seg.setEpsAngle(0.35);
    }
    seg.segment (*cylinder_inliers, *cylinder_coefficients);
    if (cylinder_inliers->indices.size () == 0)
    {
      NODELET_ERROR ("Could not estimate a cylinder model for the given dataset.");
      return false;
    }

    debug_centroid_pub_.publish(centroid);
    
    pcl::PointCloud<pcl::PointXYZ>::Ptr cylinder_points (new pcl::PointCloud<pcl::PointXYZ>);
    extract.setInputCloud (points_inside_pole_wo_plane);
    extract.setIndices (cylinder_inliers);
    extract.setNegative (false);
    extract.filter (*cylinder_points);

    publishPointCloud(debug_candidate_points_pub3_, cylinder_points);

    Eigen::Vector3f n_prime;
    Eigen::Vector3f C_new;
    for (size_t i = 0; i < 3; i++) {
      C_new[i] = cylinder_coefficients->values[i];
      n_prime[i] = cylinder_coefficients->values[i + 3];
    }

    double radius = fabs(cylinder_coefficients->values[6]);
    // inorder to compute centroid, we project all the points to the center line.
    // and after that, get the minimum and maximum points in the coordinate system of the center line
    double min_alpha = DBL_MAX;
    double max_alpha = -DBL_MAX;
    for (size_t i = 0; i < cylinder_points->points.size(); i++ ) {
      pcl::PointXYZ q = cylinder_points->points[i];
      double alpha = (q.getVector3fMap() - C_new).dot(n_prime);
      if (alpha < min_alpha) {
        min_alpha = alpha;
      }
      if (alpha > max_alpha) {
        max_alpha = alpha;
      }
    }
    // the center of cylinder
    Eigen::Vector3f C_new_prime = C_new + (max_alpha + min_alpha) / 2.0 * n_prime;
    
    Eigen::Vector3f n_cross = n.cross(n_prime);
    if (n.dot(n_prime)) {
      n_cross = - n_cross;
    }
    double theta = asin(n_cross.norm());
    Eigen::Quaternionf trans (Eigen::AngleAxisf(theta, n_cross.normalized()));
    Eigen::Vector3f C = trans * C_orig;
    Eigen::Affine3f A = Eigen::Translation3f(C) * Eigen::Translation3f(C_new_prime - C) * trans * Eigen::Translation3f(C_orig).inverse();
    tf::poseEigenToMsg((Eigen::Affine3d)A, res.transformation);

    geometry_msgs::PointStamped centroid_transformed;
    centroid_transformed.point.x = C_new_prime[0];
    centroid_transformed.point.y = C_new_prime[1];
    centroid_transformed.point.z = C_new_prime[2];
    centroid_transformed.header = req.request.header;
    debug_centroid_after_trans_pub_.publish(centroid_transformed);

    // publish marker
    visualization_msgs::Marker marker;
    marker.type = visualization_msgs::Marker::CYLINDER;
    marker.scale.x = radius;
    marker.scale.y = radius;
    marker.scale.z = (max_alpha - min_alpha);
    marker.pose.position.x = C_new_prime[0];
    marker.pose.position.y = C_new_prime[1];
    marker.pose.position.z = C_new_prime[2];

    // n_prime -> z
    // n_cross.normalized() -> x
    Eigen::Vector3f z_axis = n_prime.normalized();
    Eigen::Vector3f y_axis = n_cross.normalized();
    Eigen::Vector3f x_axis = (y_axis.cross(z_axis)).normalized();
    Eigen::Matrix3f M;
    for (size_t i = 0; i < 3; i++) {
      M(i, 0) = x_axis[i];
      M(i, 1) = y_axis[i];
      M(i, 2) = z_axis[i];
    }
    
    Eigen::Quaternionf q (M);
    marker.pose.orientation.x = q.x();
    marker.pose.orientation.y = q.y();
    marker.pose.orientation.z = q.z();
    marker.pose.orientation.w = q.w();
    marker.color.g = 1.0;
    marker.color.a = 1.0;
    marker.header = input_header_;
    marker_pub_.publish(marker);
    
    return true;
  }
bool seg_cb(bwi_perception::ButtonDetection::Request &req, bwi_perception::ButtonDetection::Response &res)
{
	//get the point cloud by aggregating k successive input clouds
	waitForCloudK(15);
	cloud = cloud_aggregated;

	//**Step 1: z-filter and voxel filter**//
	
	// Create the filtering object
	pcl::PassThrough<PointT> pass;
	pass.setInputCloud (cloud);
	pass.setFilterFieldName ("z");
	pass.setFilterLimits (0.0, 1.15);
	pass.filter (*cloud);
	
	// Create the filtering object: downsample the dataset using a leaf size of 1cm
	pcl::VoxelGrid<PointT> vg;
	pcl::PointCloud<PointT>::Ptr cloud_filtered (new pcl::PointCloud<PointT>);
	vg.setInputCloud (cloud);
	vg.setLeafSize (0.0025f, 0.0025f, 0.0025f);
	vg.filter (*cloud_filtered);

	//publish point cloud for debugging
	ROS_INFO("Publishing point cloud...");
	/*pcl::toROSMsg(*cloud_filtered,cloud_ros);
	cloud_ros.header.frame_id = cloud->header.frame_id;
	cloud_pub.publish(cloud_ros);*/

    ROS_INFO("After voxel grid filter: %i points",(int)cloud_filtered->points.size());
    
    //**Step 2: plane fitting**//
    
    //find palne
    //one cloud contains plane other cloud contains other objects
    pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
	pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
    // Create the segmentation object
	pcl::SACSegmentation<PointT> seg;
	// Optional
    seg.setOptimizeCoefficients (true);
	// Mandatory
	seg.setModelType (pcl::SACMODEL_PLANE);
	seg.setMethodType (pcl::SAC_RANSAC);
	seg.setMaxIterations (1000);
	seg.setDistanceThreshold (0.02);

	// Create the filtering object
	pcl::ExtractIndices<PointT> extract;

	// Segment the largest planar component from the remaining cloud
	seg.setInputCloud (cloud_filtered);
	seg.segment (*inliers, *coefficients);

	// Extract the plane
	extract.setInputCloud (cloud_filtered);
	extract.setIndices (inliers);
	extract.setNegative (false);
	extract.filter (*cloud_plane);
     
    //for everything else, cluster extraction; segment extraction
    //extract everything else
	extract.setNegative (true);
	extract.filter (*cloud_blobs);
	
    //get the plane coefficients
	Eigen::Vector4f plane_coefficients;
	plane_coefficients(0)=coefficients->values[0];
	plane_coefficients(1)=coefficients->values[1];
	plane_coefficients(2)=coefficients->values[2];
	plane_coefficients(3)=coefficients->values[3];

    	
    //**Step 3: Eucledian Cluster Extraction**//
	std::vector<PointCloudT::Ptr > clusters = computeClusters(cloud_blobs,0.04);
	
	ROS_INFO("clustes found: %i", (int)clusters.size());
	
	clusters_on_plane.clear();
	
	//if clusters are touching the table put them in a vector
	for (unsigned int i = 0; i < clusters.size(); i++){
		Eigen::Vector4f centroid_i;
		pcl::compute3DCentroid(*clusters.at(i), centroid_i);
		pcl::PointXYZ center;
		center.x=centroid_i(0);center.y=centroid_i(1);center.z=centroid_i(2);

		double distance = pcl::pointToPlaneDistance(center, plane_coefficients);
		if (distance < 0.1 /*&& clusters.at(i).get()->points.size() >*/ ){
			clusters_on_plane.push_back(clusters.at(i));
		}
	}
	ROS_INFO("clustes_on_plane found: %i", (int)clusters_on_plane.size());
	
	// if the clousters size == 0 return false
	if(clusters_on_plane.size() == 0) {
		
		cloud_mutex.unlock ();

		res.button_found = false; 
		return true;
	}
	//**Step 4: detect the button among the remaining clusters**//
	int max_index = -1;

	double max_red = 0.0;
	// Find the max red value
	for (unsigned int i = 0; i < clusters_on_plane.size(); i++){
		double red_i = computeAvgRedValue(clusters_on_plane.at(i));
		//ROS_INFO("Cluster %i: %i points, red_value = %f",i,(int)clusters_on_plane.at(i)->points.size(),red_i);

		if (red_i > max_red){
			max_red = red_i;
			max_index = i;
		}
	}
	
	//publish  cloud if we think it's a button
	/*max_red > 170 && max_red < 250 && */
	ROS_INFO("max_red=%f", max_red);

	
	if (max_index >= 0 && max_red > red_min) {
			
	    ROS_INFO("Button_found");

		pcl::toROSMsg(*clusters_on_plane.at(max_index),cloud_ros);
		cloud_ros.header.frame_id = cloud->header.frame_id;
		cloud_pub.publish(cloud_ros);

		//fill in response
	    res.button_found = true;
	    res.cloud_button = cloud_ros;

		/*Eigen::Vector4f centroid;
		pcl::compute3DCentroid(*clusters_on_plane.at(max_index), centroid);

		//transforms the pose into /map frame
		geometry_msgs::Pose pose_i;
		pose_i.position.x=centroid(0);
		pose_i.position.y=centroid(1);
		pose_i.position.z=centroid(2);
		pose_i.orientation = tf::createQuaternionMsgFromRollPitchYaw(0,0,-3.14/2);

		geometry_msgs::PoseStamped stampedPose;

		stampedPose.header.frame_id = cloud->header.frame_id;
		stampedPose.header.stamp = ros::Time(0);
		stampedPose.pose = pose_i;*/

		//geometry_msgs::PoseStamped stampOut;
		//listener.waitForTransform(cloud->header.frame_id, "m1n6s200_link_base", ros::Time(0), ros::Duration(3.0));
		//listener.transformPose("m1n6s200_link_base", stampedPose, stampOut);

		//stampOut.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0,-3.14/2,0);
		//pose_pub.publish(stampOut);

	}
	
	else {
		res.button_found = false;
		
	}
	
	cloud_mutex.unlock ();

	return true;
}
Exemple #5
-1
/**
 * extract handles from a pointcloud
 * @param[pointCloudIn] input point cloud
 * @param[pointCloudNormals] normals for the input cloud
 * @param[handle_indices] vector of indices, each item being a set of indices representing a handle
 * @param[handle_coefficients] vector of cofficients, each item being the coefficients of the line representing the handle
 */
void HandleExtractor::extractHandles(PointCloud::Ptr &cloudInput, PointCloudNormal::Ptr &pointCloudNormals,
                                     std::vector< pcl::PointIndices> &handle_indices, std::vector< pcl::ModelCoefficients> &handle_coefficients
                                    )
{

  // setup handle cluster object
  pcl::EuclideanClusterExtraction<Point> handle_cluster_;
  KdTreePtr clusters_tree_(new KdTree);
  handle_cluster_.setClusterTolerance(handle_cluster_tolerance);
  handle_cluster_.setMinClusterSize(min_handle_cluster_size);
  handle_cluster_.setSearchMethod(clusters_tree_);
  handle_cluster_.setInputCloud(cloudInput);

  pcl::PointCloud<Point>::Ptr cloud_filtered(new pcl::PointCloud<Point>());
  pcl::VoxelGrid<Point> vg;
  vg.setInputCloud(cloudInput);
  vg.setLeafSize(0.01, 0.01, 0.01);
  vg.filter(*cloud_filtered);

  // setup line ransac object
  pcl::SACSegmentation<Point> seg_line_;
  seg_line_.setModelType(pcl::SACMODEL_LINE);
  seg_line_.setMethodType(pcl::SAC_RANSAC);
  seg_line_.setInputCloud(cloudInput);
  seg_line_.setDistanceThreshold(line_ransac_distance);
  seg_line_.setOptimizeCoefficients(true);
  seg_line_.setMaxIterations(line_ransac_max_iter);

  // setup Inlier projection object
  pcl::ProjectInliers<Point> proj_;
  proj_.setInputCloud(cloud_filtered);
  proj_.setModelType(pcl::SACMODEL_PARALLEL_PLANE);

  // setup polygonal prism
  pcl::ExtractPolygonalPrismData<Point> prism_;
  prism_.setHeightLimits(min_handle_height, max_handle_height);
  prism_.setInputCloud(cloudInput);

  //setup Plane Segmentation
  outInfo("Segmenting planes");
  pcl::SACSegmentation<Point> seg_plane_;
  seg_plane_.setOptimizeCoefficients(true);
  seg_plane_.setModelType(pcl::SACMODEL_PLANE);
  seg_plane_.setMethodType(pcl::SAC_RANSAC);
  seg_plane_.setDistanceThreshold(0.03);
  seg_plane_.setMaxIterations(500);
  seg_plane_.setInputCloud(cloud_filtered);
  pcl::ModelCoefficients::Ptr plane_coefficients(new pcl::ModelCoefficients());
  pcl::PointIndices::Ptr plane_inliers(new pcl::PointIndices());
  seg_plane_.segment(*plane_inliers, *plane_coefficients);

  if(plane_inliers->indices.size() != 0)
  {
    outDebug("Plane model: "
             << plane_coefficients->values[0] << ","
             << plane_coefficients->values[1] << ","
             << plane_coefficients->values[2] << ","
             << plane_coefficients->values[3] << " with "
             << (int)plane_inliers->indices.size() << "inliers ");

    //Project inliers of the planes into a perfect plane
    PointCloud::Ptr cloud_projected(new PointCloud());
    proj_.setIndices(plane_inliers);
    proj_.setModelCoefficients(plane_coefficients);
    proj_.filter(*cloud_projected);

    // Create a Convex Hull representation using the projected inliers
    PointCloud::Ptr cloud_hull(new PointCloud());
    pcl::ConvexHull<Point> chull_;
    chull_.setDimension(2);
    chull_.setInputCloud(cloud_projected);
    chull_.reconstruct(*cloud_hull);
    outInfo("Convex hull has: " << (int)cloud_hull->points.size() << " data points.");
    if((int) cloud_hull->points.size() == 0)
    {
      outInfo("Convex hull has: no data points. Returning.");
      return;
    }

    // Extract the handle clusters using a polygonal prism
    pcl::PointIndices::Ptr handlesIndicesPtr(new pcl::PointIndices());
    prism_.setInputPlanarHull(cloud_hull);
    prism_.segment(*handlesIndicesPtr);

    // cluster the points found in the prism
    std::vector< pcl::PointIndices> handle_clusters;
    handle_cluster_.setIndices(handlesIndicesPtr);
    handle_cluster_.extract(handle_clusters);
    for(size_t j = 0; j < handle_clusters.size(); j++)
    {
      // for each cluster in the prism, attempt to fit a line using ransac
      pcl::PointIndices single_handle_indices;
      pcl::ModelCoefficients handle_line_coefficients;
      seg_line_.setIndices(getIndicesPointerFromObject(handle_clusters[j]));
      seg_line_.segment(single_handle_indices, handle_line_coefficients);
      if(single_handle_indices.indices.size() > 0)
      {
        outInfo("Found a handle with " << single_handle_indices.indices.size() << " inliers.");
        outDebug("Handle Line coefficients: " << handle_line_coefficients);
        handle_indices.push_back(single_handle_indices);
        handle_coefficients.push_back(handle_line_coefficients);
      }
    }
  }
  else
  {
    outInfo("no planes found");
    return;
  }
}