1 #include <ros/ros.h>
   2 #include <pcl/point_types.h>
   3 #include <pcl_ros/point_cloud.h>
   4 #include "cob_3d_mapping_common/point_types.h"
   5 #include "cob_3d_segmentation/impl/fast_segmentation.hpp"
   6 #include "cob_3d_features/organized_normal_estimation_omp.h"
   7 
   8 // specify point types (at least: XYZRGB, Normal, PointLabel)
   9 typedef cob_3d_segmentation::FastSegmentation<
  10   pcl::PointXYZRGB,
  11   pcl::Normal,
  12   PointLabel> Segmentation3d;
  13 typedef cob_3d_features::OrganizedNormalEstimationOMP<
  14   pcl::PointXYZRGB,
  15   pcl::Normal,
  16   PointLabel> NormalEstimation;
  17 
  18 void cloud_cb(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& input)
  19 {
  20   /************ compute segments ***************/
  21   ...
  22 
  23   /************ access results ***************/
  24   ...
  25 }
  26 
  27 int main(int argc, char** argv)
  28 {
  29   // Initialize ROS
  30   ros::init(argc,argv,"fast_segmentation_tutorial");
  31   ros::NodeHandle nh;
  32 
  33   // Create a ROS subscriber for the input point cloud
  34   ros::Subscriber sub = nh.subscribe ("input", 1, cloud_cb);
  35 
  36   // Spin
  37   ros::spin ();
  38 }

   1 ...
   2 
   3 void cloud_cb(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& input)
   4 {
   5   /************ compute segments ***************/
   6   pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
   7   pcl::PointCloud<PointLabel>::Ptr labels(new pcl::PointCloud<PointLabel>);
   8 
   9   // Segmentation requires estimated normals for every 3d point
  10   NormalEstimation one;
  11   // labels are first used to mark NaN and border points as a
  12   // preparation for the segmantation
  13   one.setOutputLabels(labels);
  14   // sets the pixelwindow size, radial step size and window step size
  15   one.setPixelSearchRadius(8,2,2);
  16   // sets the threshold for border point determination
  17   one.setSkipDistantPointThreshold(8);
  18   one.setInputCloud(input);
  19   one.compute(*normals);
  20 
  21   Segmentation3d seg;
  22   seg.setNormalCloudIn(normals);
  23   // labels are now assigned according to the cluster a point belongs to
  24   seg.setLabelCloudInOut(labels);
  25   // defines the method how seed points are initialized (SEED_RANDOM | SEED_LINEAR)
  26   seg.setSeedMethod(cob_3d_segmentation::SEED_RANDOM);
  27   seg.setInputCloud(input);
  28   seg.compute();
  29 
  30   /************ access results ***************/
  31   //....
  32 }

   1 ...
   2 
   3 void cloud_cb(const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr& input)
   4 {
   5   /************ compute segments ***************/
   6   ...
   7 
   8   /************ access results ***************/
   9   // for visualization you can map the segments to a rgb point cloud
  10   pcl::PointCloud<pcl::PointXYZRGB>::Ptr points(
  11     new pcl::PointCloud<pcl::PointXYZRGB>);
  12   *points = *input; // deep copy input coordinates
  13   seg.mapSegmentColor(points);
  14 
  15   // use the internal cluster structure:
  16   // the cluster handler manages all segments and is being reset
  17   // before every call of seg.compute()
  18   Segmentation3d::ClusterHdlPtr cluster_handler = seg.clusters();
  19   // use .begin() and .end() to iterate over all computed clusters
  20   Segmentation3d::ClusterPtr c = cluster_handler->begin();
  21   for(; c != cluster_handler->end(); ++c)
  22   {
  23     // now you can access several properties of a cluster such as
  24     // the number of points
  25     int size = c->size();
  26     // the centroid
  27     Eigen::Vector3f mean = c->getCentroid();
  28     // the average normal:
  29     Eigen::Vector3f orientation = c->getOrientation();
  30     // the average color value:
  31     Eigen::Vector3i mean_color = c->getMeanColorVector();
  32     // the dominant color value (based on HSV histogram):
  33     Eigen::Vector3i dom_color = c->computeDominantColorVector();
  34     // the border points:
  35     std::vector<cob_3d_segmentation::PolygonPoint> border = c->border_points;
  36     // or the principal components (v1 > v2 > v3):
  37     cluster_handler->computeClusterComponents(c);
  38     Eigen::Vector3f v1 = c->pca_point_comp1;
  39     Eigen::Vector3f v2 = c->pca_point_comp2;
  40     Eigen::Vector3f v3 = c->pca_point_comp3;
  41     Eigen::Vector3f values = c->pca_point_values;
  42     // and you can iterate over the original point indices:
  43     std::vector<int>::iterator it = c->begin();
  44     for (; it != c->end(); ++it)
  45     {
  46       // access point of point cloud:
  47       pcl::PointXYZRGB p = input->points[*it];
  48     }
  49   }
  50 
  51   // and of course you can also access each cluster by its id:
  52   // use the labeled point cloud:
  53   for(size_t i = 0; i<labels->size(); ++i)
  54   {
  55     int cluster_id = labels->points[i].label;
  56     c = cluster_handler->getCluster(cluster_id);
  57   }
  58 }