stick_segmentation.cpp
Go to the documentation of this file.
00001 #include <pcl/common/distances.h>
00002 #include <pcl/console/parse.h>
00003 #include <pcl/console/time.h>
00004 #include <pcl/point_types.h>
00005 #include <pcl/io/pcd_io.h>
00006 #include <pcl/filters/passthrough.h>
00007 #include <pcl/filters/voxel_grid.h>
00008 #include <pcl/sample_consensus/method_types.h>
00009 #include <pcl/segmentation/sac_segmentation.h>
00010 #include <pcl/visualization/pcl_visualizer.h>
00011 #include <pcl/filters/conditional_removal.h>
00012 #include <pcl/segmentation/extract_clusters.h>
00013 
00014 #define MIN_NR_INLIERS_LINE 40
00015 
00017 template <typename PointT>
00018 class ConditionThresholdHSV : public pcl::ConditionBase<PointT>
00019 {
00020   public:
00021     typedef typename boost::shared_ptr<ConditionThresholdHSV<PointT> > Ptr;
00022     
00023     ConditionThresholdHSV (float min_h, float max_h, float min_s, float max_s, float min_v, float max_v) :
00024       min_h_(min_h), max_h_(max_h), min_s_(min_s), max_s_(max_s), min_v_(min_v), max_v_(max_v)
00025     {
00026       // Make min_h_ and max_h_ fall within [0, 360)
00027       assert (!pcl_isnan(min_h) && !pcl_isnan(max_h));
00028       while (min_h_ < 0) min_h_ += 360;
00029       while (min_h_ >= 360) min_h_ -= 360;
00030       while (max_h_ < 0) max_h_ += 360;
00031       while (max_h_ >= 360) max_h_ -= 360;
00032     }
00033     
00034     // Evaluate whether the color of the given point falls within the specified thresholds
00035     virtual bool evaluate(const PointT & p) const
00036     {
00037       float h, s, v;
00038       rgb2hsv (p.r, p.g, p.b, h, s, v);
00039       return (!pcl_isnan(h) && !pcl_isnan(s) && !pcl_isnan(v) && 
00040               ((min_h_ < max_h_) ? ((min_h_ <= h) && (h <= max_h_)) : ((min_h_ <= h) || (h <= max_h_))) &&
00041               (min_s_ <= s) && (s <= max_s_) &&
00042               (min_v_ <= v) && (v <= max_v_));
00043     }
00044     
00045     void rgb2hsv (uint8_t r, uint8_t g, uint8_t b, float & h, float & s, float & v) const
00046     {
00047       float maxval = (r > g) ? ((r > b) ? r : b) : ((g > b) ? g : b);
00048       float minval = (r < g) ? ((r < b) ? r : b) : ((g < b) ? g : b);
00049       float minmaxdiff = maxval - minval;
00050       
00051       if (maxval == minval)
00052       {
00053         h = 0;
00054         s = 0;
00055         v = maxval;
00056         return;
00057       }   
00058       else if (maxval == r)
00059       {
00060         h = 60.0*((g - b)/minmaxdiff);
00061         if (h < 0) h += 360.0;
00062       }
00063       else if (maxval == g)
00064       {
00065         h = 60.0*((b - r)/minmaxdiff + 2.0);
00066       }
00067       else // (maxval == b)
00068       {
00069         h = 60.0*((r - g)/minmaxdiff + 4.0);
00070       }
00071       s = 100.0 * minmaxdiff / maxval;
00072       v = maxval;
00073     }
00074 
00075   protected:
00076     float min_h_, max_h_, min_s_, max_s_, min_v_, max_v_;
00077 };
00078 
00080 void 
00081 filterRed (const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &input, pcl::PointCloud<pcl::PointXYZRGB>::Ptr &output)
00082 {
00083   pcl::ConditionalRemoval<pcl::PointXYZRGB> removal_filter;
00084   removal_filter.setKeepOrganized (false);
00085   ConditionThresholdHSV<pcl::PointXYZRGB>::Ptr condition (new ConditionThresholdHSV<pcl::PointXYZRGB> (-20,20, 75,100, 25,255));
00086   removal_filter.setCondition (condition);
00087 
00088   removal_filter.setInputCloud (input);
00089   removal_filter.filter (*output);
00090 }
00091 
00093 void 
00094 filterGreen (const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &input, pcl::PointCloud<pcl::PointXYZRGB>::Ptr &output)
00095 {
00096   pcl::ConditionalRemoval<pcl::PointXYZRGB> removal_filter;
00097   removal_filter.setKeepOrganized (false);
00098   ConditionThresholdHSV<pcl::PointXYZRGB>::Ptr condition (new ConditionThresholdHSV<pcl::PointXYZRGB> (90,150, 15,100, 25,255));
00099   removal_filter.setCondition (condition);
00100 
00101   removal_filter.setInputCloud (input);
00102   removal_filter.filter (*output);
00103 }
00104 
00106 void
00107 downsample (const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &input, 
00108             pcl::PointCloud<pcl::PointXYZRGB>::Ptr &output)
00109 {
00110   pcl::VoxelGrid<pcl::PointXYZRGB> pass;
00111   pass.setInputCloud (input);
00112   pass.setLeafSize (0.005, 0.005, 0.005);
00113   pass.setFilterFieldName ("z");
00114   pass.setFilterLimits (0.0, 2.0);
00115   pass.filter (*output);
00116 }
00117 
00119 void
00120 extractLargestCluster (const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &input, 
00121                        const pcl::PointIndices::Ptr &inliers_all,
00122                        pcl::PointIndices &inliers)
00123 {
00124   pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ece;
00125   ece.setInputCloud (input);
00126   ece.setIndices (inliers_all);
00127   ece.setClusterTolerance (0.3);   // 30cm cluster separation
00128   std::vector<pcl::PointIndices> clusters;
00129   ece.extract (clusters);
00130   inliers = clusters[0];
00131 }
00132 
00134 void
00135 compute (const pcl::PointCloud<pcl::PointXYZRGB>::Ptr &input, 
00136          pcl::PointCloud<pcl::PointXYZRGB>::Ptr &output,
00137          pcl::ModelCoefficients &coefficients,
00138          pcl::PointIndices &inliers)
00139 {
00140   // Filter
00141   pcl::PointCloud<pcl::PointXYZRGB>::Ptr output_down (new pcl::PointCloud<pcl::PointXYZRGB>);
00142   downsample (input, output_down);
00143 
00144   if (output_down->points.empty ())
00145   {
00146     inliers.indices.clear ();
00147     coefficients.values.clear ();
00148     return;
00149   }
00150   filterGreen (output_down, output);
00151 
00152   if (output->points.empty ())
00153   {
00154     inliers.indices.clear ();
00155     coefficients.values.clear ();
00156     return;
00157   }
00158   pcl::SACSegmentation<pcl::PointXYZRGB> seg;
00159   seg.setInputCloud (output);
00160   seg.setOptimizeCoefficients (false);
00161   seg.setProbability (0.99);
00162   seg.setMaxIterations (10000);
00163   seg.setModelType (pcl::SACMODEL_STICK);
00164   seg.setMethodType (pcl::SAC_RANSAC);
00165   seg.setDistanceThreshold (0.02);
00166   //seg.setRadiusLimits (0.02, 0.08);
00167   pcl::PointIndices::Ptr inliers_all (new pcl::PointIndices);
00168   seg.segment (*inliers_all, coefficients);
00169   if (inliers_all->indices.size () < MIN_NR_INLIERS_LINE)
00170   {
00171     inliers.indices.clear ();
00172     coefficients.values.clear ();
00173     return;
00174   }
00175 
00176   extractLargestCluster (output, inliers_all, inliers);
00177 }
00178 
00180 int
00181 main (int argc, char** argv)
00182 {
00183   srand (time (0));
00184 
00185   pcl::visualization::PCLVisualizer p (argc, argv, "Line segmentation");
00186 
00187   pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
00188   pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_d (new pcl::PointCloud<pcl::PointXYZRGB>);
00189   pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZRGB>);
00190   pcl::ModelCoefficients coefficients;
00191   pcl::PointIndices inliers;
00192 
00193   std::vector<int> p_file_indices = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
00194 
00195   for (size_t i = 0; i < p_file_indices.size (); ++i)
00196   {
00197     pcl::io::loadPCDFile (argv[p_file_indices[i]], *cloud);
00198     
00199     // Compute
00200     pcl::console::TicToc tt;
00201     tt.tic ();
00202     compute (cloud, cloud_f, coefficients, inliers);
00203     tt.toc_print ();
00204 
00205     if (inliers.indices.empty ())
00206     {
00207       p.removeShape ("line");
00208       continue;
00209     }
00210 
00211     // Display
00212     PCL_INFO ("Found %zu inliers.\n", inliers.indices.size ());
00213 
00214     pcl::PointCloud<pcl::PointXYZ>::Ptr line (new pcl::PointCloud<pcl::PointXYZ>);
00215     pcl::copyPointCloud (*cloud_f, inliers, *line);
00216 
00217     if (!p.updatePointCloud (cloud, "all"))
00218     {
00219       p.addPointCloud (cloud, "all");
00220       p.resetCameraViewpoint ("all");
00221     }
00222 
00223     if (!p.updatePointCloud (cloud_f, "filter"))
00224       p.addPointCloud (cloud_f, "filter");
00225     p.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 10.0, "filter");
00226     p.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 0.2, "filter");
00227 
00228     if (!p.updatePointCloud (line, "line inliers")) 
00229       p.addPointCloud (line, "line inliers");
00230 
00231     pcl::PointXYZRGB pmin, pmax;
00232     if (pcl::getMaxSegment (*cloud_f, inliers.indices, pmin, pmax) != std::numeric_limits<double>::min ())
00233       p.addLine<pcl::PointXYZRGB> (pmin, pmax);
00234     else
00235     {
00236       pmin.x = coefficients.values[0]; pmin.y = coefficients.values[1]; pmin.z = coefficients.values[2];
00237       pmax.x = coefficients.values[3]; pmax.y = coefficients.values[4]; pmax.z = coefficients.values[5];
00238       PCL_ERROR ("Couldn't compute the maximum segment!\n");
00239       p.addLine<pcl::PointXYZRGB> (pmin, pmax);
00240       //p.addLine (coefficients);
00241     }
00242     p.setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 50.0, "line");
00243     p.setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 1.0, 0.0, "line");
00244 
00245     if (p_file_indices.size () == 1)
00246       p.spin ();
00247     p.spinOnce ();
00248     p.removeShape ("line");
00249   }
00250 
00251   if (p_file_indices.size () != 1)
00252     p.spin ();
00253 
00254   return (0);
00255 }


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:18:09