table_object_detector.cpp

Go to the documentation of this file.

/*
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2010, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * $Id: table_object_detector.cpp 36118 2011-02-22 00:10:08Z rusu $
 *
 */

#include <ros/ros.h>
#include "pcl/io/io.h"
#include "pcl_ros/publisher.h"
#include "pcl/point_types.h"

#include "pcl/io/io.h"
#include "pcl/io/pcd_io.h"
#include "pcl/filters/voxel_grid.h"
#include "pcl/filters/passthrough.h"
#include "pcl/filters/extract_indices.h"
#include "pcl/features/normal_3d.h"
#include "pcl/kdtree/kdtree.h"
#include "pcl/kdtree/kdtree_flann.h"
#include "pcl/kdtree/organized_data.h"
#include "pcl/sample_consensus/method_types.h"
#include "pcl/sample_consensus/model_types.h"
#include "pcl/segmentation/sac_segmentation.h"
#include "pcl/filters/project_inliers.h"
#include "pcl/surface/convex_hull.h"
#include "pcl/segmentation/extract_polygonal_prism_data.h"
#include "pcl/segmentation/extract_clusters.h"

// ROS messages
#include <sensor_msgs/PointCloud2.h>
#include "pcl/PointIndices.h"
#include "pcl/ModelCoefficients.h"

//#include <robot_msgs/Polygon3D.h>
//#include <robot_msgs/PolygonalMap.h>

//#include <robot_srvs/FindTable.h>
//#include <robot_msgs/Table.h>
//#include <robot_msgs/ObjectOnTable.h>

class TableObjectDetector
{
  typedef pcl::PointXYZRGB Point;
  typedef pcl::KdTree<Point>::Ptr KdTreePtr;

  public:
    ros::NodeHandle nh_;

    ros::Subscriber cloud_sub_;
    ros::Publisher cloud_pub_;

    // PCL objects
    KdTreePtr normals_tree_, clusters_tree_;
    pcl::PassThrough<Point> pass_;
    pcl::VoxelGrid<Point> grid_, grid_objects_;
    pcl::NormalEstimation<Point, pcl::Normal> n3d_;
    pcl::SACSegmentationFromNormals<Point, pcl::Normal> seg_;
    pcl::ProjectInliers<Point> proj_;
    pcl::ConvexHull<Point> hull_;
    pcl::ExtractPolygonalPrismData<Point> prism_;
    pcl::EuclideanClusterExtraction<Point> cluster_;

    //pcl::OrganizedDataIndex<Point> tree_;

    double downsample_leaf_, downsample_leaf_objects_;
    int k_;
    double min_z_bounds_, max_z_bounds_;
    double sac_distance_threshold_;
    double normal_distance_weight_;

    // Min/Max height from the table plane object points will be considered from/to
    double object_min_height_, object_max_height_;

    // Object cluster tolerance and minimum cluster size
    double object_cluster_tolerance_, object_cluster_min_size_;

    // The raw, input point cloud data
    pcl::PointCloud<Point>::ConstPtr cloud_;
    // The filtered and downsampled point cloud data
    pcl::PointCloud<Point>::ConstPtr cloud_filtered_, cloud_downsampled_;
    // The resultant estimated point cloud normals for \a cloud_filtered_
    pcl::PointCloud<pcl::Normal>::ConstPtr cloud_normals_;
    // The vector of indices from cloud_filtered_ that represent the planar table component
    pcl::PointIndices::ConstPtr table_inliers_;
    // The model coefficients of the planar table component
    pcl::ModelCoefficients::ConstPtr table_coefficients_;
    // The set of point inliers projected on the planar table component from \a cloud_filtered_
    pcl::PointCloud<Point>::ConstPtr table_projected_;
    // The convex hull of \a table_projected_
    pcl::PointCloud<Point>::ConstPtr table_hull_;
    // The remaining of the \a cloud_filtered_ which lies inside the \a table_hull_ polygon
    pcl::PointCloud<Point>::ConstPtr cloud_objects_, cloud_objects_downsampled_;

    TableObjectDetector () : nh_ ("~")
    {
      cloud_sub_ = nh_.subscribe ("input", 1, &TableObjectDetector::input_callback, this);
      cloud_pub_ = nh_.advertise<sensor_msgs::PointCloud2> ("cloud_out", 1);

      // ---[ Create all PCL objects and set their parameters

      // Filtering parameters
      downsample_leaf_ = 0.01;                          // 1cm voxel size by default
      downsample_leaf_objects_ = 0.003;                 // 3mm voxel size by default
      nh_.getParam ("downsample_leaf", downsample_leaf_);
      grid_.setLeafSize (downsample_leaf_, downsample_leaf_, downsample_leaf_);
      grid_objects_.setLeafSize (downsample_leaf_objects_, downsample_leaf_objects_, downsample_leaf_objects_);
      grid_.setFilterFieldName ("z");
      pass_.setFilterFieldName ("z");

      min_z_bounds_ = 0.4;                            // restrict the Z dimension between 0.4m
      max_z_bounds_ = 1.6;                            // and 1.6m
      nh_.getParam ("min_z_bounds", min_z_bounds_);
      nh_.getParam ("max_z_bounds", max_z_bounds_);
      grid_.setFilterLimits (min_z_bounds_, max_z_bounds_);
      pass_.setFilterLimits (min_z_bounds_, max_z_bounds_);
      grid_.setDownsampleAllData (false);
      grid_objects_.setDownsampleAllData (false);

      normals_tree_ = boost::make_shared<pcl::KdTreeFLANN<Point> > ();
      clusters_tree_ = boost::make_shared<pcl::KdTreeFLANN<Point> > ();
      clusters_tree_->setEpsilon (1);
      //tree_.setSearchWindowAsK (10);
      //tree_.setMaxDistance (0.5);

      // Normal estimation parameters
      k_ = 10;                                // 50 k-neighbors by default
      nh_.getParam ("search_k_closest", k_);
      n3d_.setKSearch (k_);
      //n3d_.setRadiusSearch (0.015);
      n3d_.setSearchMethod (normals_tree_);

      // Table model fitting parameters
      sac_distance_threshold_ = 0.1;               // 5cm
      nh_.getParam ("sac_distance_threshold", sac_distance_threshold_);
      seg_.setDistanceThreshold (sac_distance_threshold_);
      seg_.setMaxIterations (10000);

      normal_distance_weight_ = 0.1;
      nh_.getParam ("normal_distance_weight", normal_distance_weight_);
      seg_.setNormalDistanceWeight (normal_distance_weight_);
      seg_.setOptimizeCoefficients (true);
      seg_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
      seg_.setMethodType (pcl::SAC_RANSAC);
      seg_.setProbability (0.99);

      proj_.setModelType (pcl::SACMODEL_NORMAL_PLANE);

      // Consider objects starting at 1cm from the table and ending at 0.5m
      object_min_height_ = 0.01;
      object_max_height_ = 0.5;
      nh_.getParam ("object_min_height", object_min_height_);
      nh_.getParam ("object_max_height", object_max_height_);
      prism_.setHeightLimits (object_min_height_, object_max_height_);

      // Clustering parameters
      object_cluster_tolerance_ = 0.05;        // 5cm between two objects
      object_cluster_min_size_  = 100;         // 100 points per object cluster
      nh_.getParam ("object_cluster_tolerance", object_cluster_tolerance_);
      nh_.getParam ("object_cluster_min_size", object_cluster_min_size_);
      cluster_.setClusterTolerance (object_cluster_tolerance_);
      cluster_.setMinClusterSize (object_cluster_min_size_);
      cluster_.setSearchMethod (clusters_tree_);

      //      param ("~normal_eps_angle_", eps_angle_, 15.0);                   // 15 degrees
      //      eps_angle_ = cloud_geometry::deg2rad (eps_angle_);                // convert to radians
      //
      //      param ("~region_angle_threshold", region_angle_threshold_, 30.0);   // Difference between normals in degrees for cluster/region growing
      //      region_angle_threshold_ = cloud_geometry::deg2rad (region_angle_threshold_); // convert to radians
      //      if (publish_debug_)
      //      {
      //        advertise<PolygonalMap> ("semantic_polygonal_map", 1);
      //        advertise<PointCloud> ("cloud_annotated", 1);
      //      }
    }

    // Callback
    void
      input_callback (const sensor_msgs::PointCloud2ConstPtr &cloud2_in)
    {
      ros::Time t1 = ros::Time::now ();
      ROS_INFO ("[TableObjectDetector::input_callback] PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.", cloud2_in->width * cloud2_in->height, pcl::getFieldsList (*cloud2_in).c_str (), cloud2_in->header.stamp.toSec (), cloud2_in->header.frame_id.c_str (), nh_.resolveName ("input").c_str ());

      // ---[ Convert the dataset
      pcl::PointCloud<Point> cloud;
      pcl::fromROSMsg (*cloud2_in, cloud);
      cloud_ = boost::make_shared<const pcl::PointCloud<Point> > (cloud);

      // ---[ Create the voxel grid
      pcl::PointCloud<Point> cloud_filtered;
      pass_.setInputCloud (cloud_);
      pass_.filter (cloud_filtered);
      cloud_filtered_.reset (new pcl::PointCloud<Point> (cloud_filtered));
      ROS_INFO ("[TableObjectDetector::input_callback] Number of points left after filtering (%f -> %f): %d out of %d.", min_z_bounds_, max_z_bounds_, (int)cloud_filtered.points.size (), (int)cloud.points.size ());

      pcl::PointCloud<Point> cloud_downsampled;
      grid_.setInputCloud (cloud_filtered_);
//      grid_.setInputCloud (cloud_);
      grid_.filter (cloud_downsampled);
      cloud_downsampled_.reset (new pcl::PointCloud<Point> (cloud_downsampled));

      if ((int)cloud_filtered_->points.size () < k_)
      {
        ROS_WARN ("[TableObjectDetector::input_callback] Filtering returned %d points! Continuing.", (int)cloud_filtered_->points.size ());
        //pcl::PointCloud<pcl::PointXYZRGBNormal> cloud_temp;
        //pcl::concatenateFields <Point, pcl::Normal, pcl::PointXYZRGBNormal> (cloud_filtered, cloud_normals, cloud_temp);
        //pcl::io::savePCDFile ("test.pcd", cloud, false);
        return;
      }

      // ---[ Estimate the point normals
      pcl::PointCloud<pcl::Normal> cloud_normals;
//      n3d_.setSearchSurface (cloud_);
      n3d_.setInputCloud (cloud_downsampled_);
      n3d_.compute (cloud_normals);
      cloud_normals_.reset (new pcl::PointCloud<pcl::Normal> (cloud_normals));
      ROS_INFO ("[TableObjectDetector::input_callback] %d normals estimated.", (int)cloud_normals.points.size ());

      //ROS_ASSERT (cloud_normals_->points.size () == cloud_filtered_->points.size ());

      // ---[ Perform segmentation
      pcl::PointIndices table_inliers; pcl::ModelCoefficients table_coefficients;
      seg_.setInputCloud (cloud_downsampled_);
      seg_.setInputNormals (cloud_normals_);
      seg_.segment (table_inliers, table_coefficients);
      table_inliers_.reset (new pcl::PointIndices (table_inliers));
      table_coefficients_.reset (new pcl::ModelCoefficients (table_coefficients));
      if (table_coefficients.values.size () > 3)
        ROS_INFO ("[TableObjectDetector::input_callback] Model found with %d inliers: [%f %f %f %f].", (int)table_inliers.indices.size (),
                  table_coefficients.values[0], table_coefficients.values[1], table_coefficients.values[2], table_coefficients.values[3]);

      if (table_inliers_->indices.size () == 0)
        return;

      // ---[ Extract the table
      pcl::PointCloud<Point> table_projected;
      //proj_.setInputCloud (cloud_filtered_);
      proj_.setInputCloud (cloud_downsampled_);
      proj_.setIndices (table_inliers_);
      proj_.setModelCoefficients (table_coefficients_);
      proj_.filter (table_projected);
      table_projected_.reset (new pcl::PointCloud<Point> (table_projected));
      ROS_INFO ("[TableObjectDetector::input_callback] Number of projected inliers: %d.", (int)table_projected.points.size ());

      // ---[ Estimate the convex hull
      pcl::PointCloud<Point> table_hull;
      hull_.setInputCloud (table_projected_);
      hull_.reconstruct (table_hull);
      table_hull_.reset (new pcl::PointCloud<Point> (table_hull));

      // ---[ Extract the remaining of all-table
      //SampleConsensusModelNormalPlane<Point, pcl::Normal> model_refine;
      //model_refine.selectWithinDistance (model_coefficients, sac_distance_threshold_, std::vector<int> &inliers);

//      pcl::PointCloud<Point> cloud_all_minus_table;
//      pcl::ExtractIndices<Point> extract_all_minus_table;
//      extract_all_minus_table.setInputCloud (cloud_filtered_);
//      extract_all_minus_table.setIndices (table_inliers_);
//      extract_all_minus_table.setNegative (true);
//      extract_all_minus_table.filter (cloud_all_minus_table);
//      boost::shared_ptr<const pcl::PointCloud<Point> > cloud_all_minus_table_ptr = boost::make_shared<const pcl::PointCloud<Point> > (cloud_all_minus_table);

      // ---[ Get the objects on top of the table
      pcl::PointIndices cloud_object_indices;
      //prism_.setInputCloud (cloud_all_minus_table_ptr);
      prism_.setInputCloud (cloud_filtered_);
//      prism_.setInputCloud (cloud_downsampled_);
      prism_.setInputPlanarHull (table_hull_);
      prism_.segment (cloud_object_indices);
      ROS_INFO ("[TableObjectDetector::input_callback] Number of object point indices: %d.", (int)cloud_object_indices.indices.size ());

      pcl::PointCloud<Point> cloud_objects;
      pcl::ExtractIndices<Point> extract_object_indices;
      //extract_object_indices.setInputCloud (cloud_all_minus_table_ptr);
      extract_object_indices.setInputCloud (cloud_filtered_);
//      extract_object_indices.setInputCloud (cloud_downsampled_);
      extract_object_indices.setIndices (boost::make_shared<const pcl::PointIndices> (cloud_object_indices));
      extract_object_indices.filter (cloud_objects);
      cloud_objects_.reset (new pcl::PointCloud<Point> (cloud_objects));
      ROS_INFO ("[TableObjectDetector::input_callback] Number of object point candidates: %d.", (int)cloud_objects.points.size ());

      if (cloud_objects.points.size () == 0)
        return;

      // ---[ Downsample the points
      pcl::PointCloud<Point> cloud_objects_downsampled;
      grid_objects_.setInputCloud (cloud_objects_);
      grid_objects_.filter (cloud_objects_downsampled);
      cloud_objects_downsampled_.reset (new pcl::PointCloud<Point> (cloud_objects_downsampled));
      ROS_INFO ("[TableObjectDetector::input_callback] Number of object point candidates left after downsampling: %d.", (int)cloud_objects_downsampled.points.size ());

      // ---[ Split the objects into Euclidean clusters
      std::vector<pcl::PointIndices> clusters;
      cluster_.setInputCloud (cloud_objects_downsampled_);
      cluster_.extract (clusters);
      ROS_INFO ("[TableObjectDetector::input_callback] Number of clusters found matching the given constraints: %d.", (int)clusters.size ());

      for (size_t i = 0; i < clusters.size (); ++i)
      {
        std::stringstream ss;
        ss << "cluster_" << i << ".pcd";

        pcl::PointCloud<Point> cloud_object_cluster;
        pcl::copyPointCloud (cloud_objects_downsampled, clusters[i], cloud_object_cluster);

        pcl::io::savePCDFile (ss.str (), cloud_object_cluster);
      }

      // -temp-
      pcl::PointCloud<Point> cloud_objects_clusters;
      pcl::copyPointCloud (cloud_objects_downsampled, clusters, cloud_objects_clusters);

      // Publish the table convex hull (optional)
      sensor_msgs::PointCloud2 cloud_out;
      pcl::toROSMsg (cloud_objects_clusters, cloud_out);
      cloud_pub_.publish (cloud_out);
      ROS_WARN ("Spent %f seconds.", (ros::Time::now () - t1).toSec ());
    }
};

/* ---[ */
int
  main (int argc, char** argv)
{
  ros::init (argc, argv, "table_object_detector");

  TableObjectDetector p;
  ros::spin ();

  return (0);
}
/* ]--- */

---

pcl_tutorials
Author(s): Radu Bogdan Rusu, Bastian Steder
autogenerated on Fri Jan 11 09:12:59 2013