handle_removal.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2011, Lucian Cosmin Goron <goron@cs.tum.edu>
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 
00031 
00032 // ros dependencies
00033 #include "ros/ros.h"
00034 
00035 // terminal tools dependecies
00036 #include "terminal_tools/parse.h"
00037 
00038 // pcl dependencies
00039 #include "pcl/io/pcd_io.h"
00040 #include "pcl/features/normal_3d.h"
00041 #include "pcl/surface/convex_hull.h"
00042 #include "pcl/filters/extract_indices.h"
00043 #include "pcl/filters/project_inliers.h"
00044 #include "pcl/sample_consensus/method_types.h"
00045 #include "pcl/segmentation/sac_segmentation.h"
00046 #include "pcl/segmentation/extract_clusters.h"
00047 #include "pcl/segmentation/extract_polygonal_prism_data.h"
00048 
00049 // pcl visualization dependencies
00050 #include "pcl_visualization/pcl_visualizer.h"
00051 
00052 
00053 
00054 // Definition of point type 
00055 typedef pcl::PointXYZ PointT;
00056 
00057 
00058 
00059 // Segmentation's Parameters
00060 double epsilon_angle = 0.010; 
00061 double plane_threshold = 0.100; 
00062 int minimum_plane_inliers = 1000; 
00063 int maximum_plane_iterations = 1000; 
00064 
00065 // Visualization's Parameters
00066 bool step = false;
00067 bool clean = false;
00068 bool verbose = false;
00069 int size_of_points = 1;
00070 
00071 
00072 
00074 
00076 int main (int argc, char** argv)
00077 {
00078 
00079   // --------------------------------------------------------------- //
00080   // ------------------ Check and parse arguments ------------------ //
00081   // --------------------------------------------------------------- //
00082 
00083   // Argument check and info about
00084   if (argc < 2)
00085   {
00086     ROS_INFO (" ");
00087     ROS_INFO ("Syntax is: %s <input>.pcd <options>", argv[0]);
00088     ROS_INFO ("  where <options> are: ");
00089     ROS_INFO ("    -epsilon_angle X                        = ");
00090     ROS_INFO ("    -plane_threshold X                      = ");
00091     ROS_INFO ("    -minimum_plane_inliers X                = ");
00092     ROS_INFO ("    -maximum_plane_iterations X             = ");
00093     ROS_INFO (" ");
00094     ROS_INFO ("    -step B                                 = wait or not wait");
00095     ROS_INFO ("    -clean B                                = remove or not remove");
00096     ROS_INFO ("    -verbose B                              = display step by step info");
00097     ROS_INFO ("    -size_of_points D                       = set the size of points");
00098     ROS_INFO (" ");
00099     return (-1);
00100   }
00101 
00102   // Take only the first .pcd file into account
00103   std::vector<int> pFileIndicesPCD = terminal_tools::parse_file_extension_argument (argc, argv, ".pcd");
00104   if (pFileIndicesPCD.size () == 0)
00105   {
00106     ROS_ERROR ("No .pcd file given as input!");
00107     return (-1);
00108   }
00109 
00110   // Parse arguments for fitting plane models
00111   terminal_tools::parse_argument (argc, argv, "-epsilon_angle", epsilon_angle);
00112   terminal_tools::parse_argument (argc, argv, "-plane_threshold", plane_threshold);
00113   terminal_tools::parse_argument (argc, argv, "-minimum_plane_inliers", minimum_plane_inliers);
00114   terminal_tools::parse_argument (argc, argv, "-maximum_plane_iterations", maximum_plane_iterations);
00115 
00116   // Parse arguments for visualization
00117   terminal_tools::parse_argument (argc, argv, "-step", step);
00118   terminal_tools::parse_argument (argc, argv, "-clean", clean);
00119   terminal_tools::parse_argument (argc, argv, "-verbose", verbose);
00120   terminal_tools::parse_argument (argc, argv, "-size_of_points", size_of_points);
00121 
00122   // ----------------------------------------------------- //
00123   // ------------------ Initializations ------------------ //
00124   // ----------------------------------------------------- //
00125 
00126   // Initialize random number generator
00127   srand (time(0));
00128 
00129   // Initialize ros time
00130   ros::Time::init();
00131 
00132   // Declare the timer
00133   terminal_tools::TicToc tt;
00134 
00135   // Starting timer
00136   tt.tic ();
00137 
00138   if ( verbose )
00139   {
00140     // Displaying when the timer starts
00141     ROS_WARN ("Timer started !");
00142   }
00143 
00144   // ---------------------------------------------------------------- //
00145   // ------------------ Visualize point cloud data ------------------ //
00146   // ---------------------------------------------------------------- //
00147 
00148   // Open a 3D viewer
00149   pcl_visualization::PCLVisualizer viewer ("3D VIEWER");
00150   // Set the background of viewer
00151   viewer.setBackgroundColor (1.0, 1.0, 1.0);
00152   // Add system coordiante to viewer
00153   viewer.addCoordinateSystem (0.75f);
00154   // Parse the camera settings and update the internal camera
00155   viewer.getCameraParameters (argc, argv);
00156   // Update camera parameters and render
00157   viewer.updateCamera ();
00158 
00159   // --------------------------------------------------------------- //
00160   // ------------------ Load the point cloud data ------------------ //
00161   // --------------------------------------------------------------- //
00162 
00163   // Input point cloud data
00164   pcl::PointCloud<PointT>::Ptr input_cloud (new pcl::PointCloud<PointT> ());
00165  
00166   // Load point cloud data
00167   if (pcl::io::loadPCDFile (argv[pFileIndicesPCD[0]], *input_cloud) == -1)
00168   {
00169     ROS_ERROR ("Couldn't read file %s", argv[pFileIndicesPCD[0]]);
00170     return (-1);
00171   }
00172   ROS_INFO ("Loaded %d data points from %s with the following fields: %s", (int) (input_cloud->points.size ()), argv[pFileIndicesPCD[0]], pcl::getFieldsList (*input_cloud).c_str ());
00173 
00174   // Add the input cloud
00175   viewer.addPointCloud (*input_cloud, "INPUT");
00176 
00177   // Color the cloud in white
00178   viewer.setPointCloudRenderingProperties (pcl_visualization::PCL_VISUALIZER_COLOR, 0.0, 0.0, 0.0, "INPUT");
00179 
00180   // Set the size of points for cloud
00181   viewer.setPointCloudRenderingProperties (pcl_visualization::PCL_VISUALIZER_POINT_SIZE, size_of_points - 1, "INPUT"); 
00182 
00183   // Wait or not wait
00184   if ( step )
00185   {
00186     // And wait until Q key is pressed
00187     viewer.spin ();
00188   }
00189 
00190   if ( clean )
00191   {
00192     // Remove the point cloud data
00193     viewer.removePointCloud ("INPUT");
00194 
00195     // Wait or not wait
00196     if ( step )
00197     {
00198       // And wait until Q key is pressed
00199       viewer.spin ();
00200     }
00201   }
00202 
00203   // -------------------------------------------------------------------- //
00204   // ------------------ Declare working data structure ------------------ //
00205   // -------------------------------------------------------------------- //
00206 
00207   // Working point cloud data
00208   pcl::PointCloud<PointT>::Ptr working_cloud (new pcl::PointCloud<PointT> ());
00209 
00210   // Never modify the original point cloud data 
00211   *working_cloud = *input_cloud;
00212 
00213   // ------------------------------------------------------------------- //
00214   // ------------------ Estiamte 3D normals of points ------------------ //
00215   // ------------------------------------------------------------------- //
00216  
00217   // Point cloud of normals
00218   pcl::PointCloud<pcl::Normal>::Ptr normals_cloud (new pcl::PointCloud<pcl::Normal> ());
00219   // Build kd-tree structure for normals
00220   pcl::KdTreeFLANN<PointT>::Ptr normals_tree (new pcl::KdTreeFLANN<PointT> ());
00221 
00222   // Create object for normal estimation
00223   pcl::NormalEstimation<PointT, pcl::Normal> estimation_of_normals;
00224   // Provide pointer to the search method
00225   estimation_of_normals.setSearchMethod (normals_tree);
00226   // Set for which point cloud to compute the normals
00227   estimation_of_normals.setInputCloud (working_cloud);
00228   // Set number of k nearest neighbors to use
00229   estimation_of_normals.setKSearch (50);
00230   // Estimate the normals
00231   estimation_of_normals.compute (*normals_cloud);
00232 
00233   if ( verbose )
00234   {
00235     ROS_INFO ("Remaning cloud has %d points", (int) working_cloud->points.size());
00236     ROS_INFO ("With %d normals of course", (int) normals_cloud->points.size());
00237   }
00238 
00239   // ------------------------------------------------------------ //
00240   // ------------------ Segment planar surface ------------------ //
00241   // ------------------------------------------------------------ //
00242 
00243   // Create the segmentation object and declare variables
00244   pcl::SACSegmentationFromNormals<PointT, pcl::Normal> segmentation_of_plane;
00245   pcl::PointIndices::Ptr plane_inliers (new pcl::PointIndices ());
00246   pcl::ModelCoefficients::Ptr plane_coefficients (new pcl::ModelCoefficients ());
00247 
00248   // Set all the parameters for segmenting vertical planes
00249   segmentation_of_plane.setOptimizeCoefficients (true);
00250   segmentation_of_plane.setModelType (pcl::SACMODEL_NORMAL_PLANE);
00251   segmentation_of_plane.setNormalDistanceWeight (0.05);
00252   segmentation_of_plane.setMethodType (pcl::SAC_RANSAC);
00253   segmentation_of_plane.setDistanceThreshold (plane_threshold);
00254   segmentation_of_plane.setMaxIterations (maximum_plane_iterations);
00255 //  Eigen::Vector3f X = Eigen::Vector3f (-0.966764, -0.0782345, 0.0);
00256 //  Eigen::Vector3f Y = Eigen::Vector3f (0.0782345, -0.966764, 0.0);
00257 //  segmentation_of_plane.setAxis ();
00258   segmentation_of_plane.setEpsAngle (epsilon_angle);
00259   segmentation_of_plane.setInputCloud (working_cloud);
00260   segmentation_of_plane.setInputNormals (normals_cloud);
00261 
00262   // Obtain the plane inliers and coefficients
00263   segmentation_of_plane.segment (*plane_inliers, *plane_coefficients);
00264 
00265   if ( verbose )
00266   {
00267     ROS_INFO ("Plane has %5d inliers with parameters A = %f B = %f C = %f and D = %f found in maximum %d iterations", (int) plane_inliers->indices.size (), 
00268         plane_coefficients->values [0], plane_coefficients->values [1], plane_coefficients->values [2], plane_coefficients->values [3], maximum_plane_iterations);
00269   }
00270 
00271   // Check if the fitted circle has enough inliers in order to be accepted
00272   if ((int) plane_inliers->indices.size () < minimum_plane_inliers) 
00273   {
00274     if ( verbose )
00275     {
00276       ROS_ERROR ("NOT ACCEPTED !");
00277     }
00278   }
00279   else
00280   {
00281     if ( verbose )
00282     {
00283       ROS_WARN ("ACCEPTED !");
00284     }
00285 
00286     // ------------------------------------------------------------------------
00287 
00288     // Point cloud of plane inliers
00289     pcl::PointCloud<PointT>::Ptr plane_cloud (new pcl::PointCloud<PointT> ());
00290 
00291     // Extract the circular inliers from the input cloud
00292     pcl::ExtractIndices<PointT> extraction_of_inliers;
00293     extraction_of_inliers.setInputCloud (working_cloud);
00294     extraction_of_inliers.setIndices (plane_inliers);
00295     extraction_of_inliers.setNegative (false);
00296     extraction_of_inliers.filter (*plane_cloud);
00297     extraction_of_inliers.setNegative (true);
00298     extraction_of_inliers.filter (*working_cloud);
00299 
00300     // Extract the normals of plane inliers 
00301     pcl::ExtractIndices<pcl::Normal> extraction_of_normals;
00302     extraction_of_normals.setInputCloud (normals_cloud);
00303     extraction_of_normals.setIndices (plane_inliers);
00304     extraction_of_normals.setNegative (true);
00305     extraction_of_normals.filter (*normals_cloud);
00306 
00307     if ( verbose )
00308     {
00309       ROS_INFO ("Remaning cloud has %d points", (int) working_cloud->points.size());
00310       ROS_INFO ("With %d normals of course", (int) normals_cloud->points.size());
00311     }
00312 
00313     // Create id for visualization
00314     std::stringstream id_of_plane;
00315     id_of_plane << "PLANE_" << ros::Time::now ();
00316 
00317     // Add point cloud to viewer
00318     viewer.addPointCloud (*plane_cloud, id_of_plane.str());
00319 
00320     // Set the size of points for cloud
00321     viewer.setPointCloudRenderingProperties (pcl_visualization::PCL_VISUALIZER_POINT_SIZE, size_of_points, id_of_plane.str()); 
00322 
00323     // Wait or not wait
00324     if ( step )
00325     {
00326       // And wait until Q key is pressed
00327       viewer.spin ();
00328     }
00329 
00330     if ( clean )
00331     {
00332       // Remove the point cloud data
00333       viewer.removePointCloud (id_of_plane.str());
00334 
00335       // Wait or not wait
00336       if ( step )
00337       {
00338         // And wait until Q key is pressed
00339         viewer.spin ();
00340       } 
00341     }
00342 
00343     // Create id for saving
00344     std::string file_of_plane = argv [pFileIndicesPCD[0]];
00345     size_t dot_of_plane = file_of_plane.find (".");
00346     file_of_plane.insert (dot_of_plane, "-plane");
00347 
00348     // Save these points to disk
00349     pcl::io::savePCDFile (file_of_plane, *plane_cloud);
00350 
00351     // ------------------------------------------------------------------------
00352 
00353     // Declare the projected point cloud
00354     pcl::PointCloud<PointT> projected_cloud;
00355 
00356     // Project the table inliers using the planar model coefficients    
00357     pcl::ProjectInliers<PointT> projection_on_plane;   
00358     projection_on_plane.setModelType (pcl::SACMODEL_NORMAL_PLANE);
00359     projection_on_plane.setInputCloud (plane_cloud);
00360     projection_on_plane.setModelCoefficients (plane_coefficients);
00361     projection_on_plane.filter (projected_cloud);
00362 
00363     if ( verbose )
00364     {
00365       ROS_INFO ("Furniture surface has %d points", (int) projected_cloud.points.size ());
00366     }
00367 
00368     // Create id for visualization
00369     std::stringstream id_of_projected;
00370     id_of_projected << "PROJECTED_" << ros::Time::now ();
00371 
00372     // Add point cloud to viewer
00373     viewer.addPointCloud (projected_cloud, id_of_projected.str());
00374 
00375     // Set the size of points for cloud
00376     viewer.setPointCloudRenderingProperties (pcl_visualization::PCL_VISUALIZER_POINT_SIZE, size_of_points, id_of_projected.str());
00377 
00378     // Wait or not wait
00379     if ( step )
00380     {
00381       // And wait until Q key is pressed
00382       viewer.spin ();
00383     }
00384 
00385     // Create id for saving
00386     std::string file_of_projected = argv [pFileIndicesPCD[0]];
00387     size_t dot_of_projected = file_of_projected.find (".");
00388     file_of_projected.insert (dot_of_projected, "-projected");
00389 
00390     // Save these points to disk
00391     pcl::io::savePCDFile (file_of_projected, projected_cloud);
00392 
00393     // ------------------------------------------------------------------------
00394 
00395     // Declare the hull of cloud
00396     pcl::PointCloud<PointT> hull_cloud;
00397 
00398     // Create a Convex Hull representation of the projected inliers
00399     pcl::ConvexHull<PointT> hull;  
00400     hull.setInputCloud (projected_cloud.makeShared ());
00401     ROS_ERROR (" BEFORE IT WORKS ");
00402     hull.reconstruct (hull_cloud);      
00403     ROS_ERROR (" AFTERWARDS NOT ");
00404 
00405     if ( verbose )
00406     {
00407       ROS_INFO ("Convex hull has %d points.", (int) hull_cloud.points.size ());
00408     }
00409 
00410     // Create id for visualization
00411     std::stringstream id_of_hull;
00412     id_of_hull << "HULL_" << ros::Time::now ();
00413 
00414     // Add point cloud to viewer
00415     viewer.addPointCloud (hull_cloud, id_of_hull.str());
00416 
00417     // Set the size of points for cloud
00418     viewer.setPointCloudRenderingProperties (pcl_visualization::PCL_VISUALIZER_POINT_SIZE, size_of_points, id_of_hull.str());
00419 
00420     // Wait or not wait
00421     if ( step )
00422     {
00423       // And wait until Q key is pressed
00424       viewer.spin ();
00425     }
00426 
00427     if ( clean )
00428     {
00429       // Remove the point cloud data
00430       viewer.removePointCloud (id_of_hull.str());
00431 
00432       // Wait or not wait
00433       if ( step )
00434       {
00435         // And wait until Q key is pressed
00436         viewer.spin ();
00437       } 
00438     }
00439 
00440     // Create id for saving
00441     std::string file_of_hull = argv [pFileIndicesPCD[0]];
00442     size_t dot_of_hull = file_of_hull.find (".");
00443     file_of_hull.insert (dot_of_hull, "-hull");
00444 
00445     // Save these points to disk
00446     pcl::io::savePCDFile (file_of_hull, hull_cloud);
00447 
00448     // ------------------------------------------------------------------------
00449 
00450     // Declare the indices od handle
00451     pcl::PointIndices::Ptr handle_indices (new pcl::PointIndices ());
00452 
00453     //  Get the objects on top of surface
00454     pcl::ExtractPolygonalPrismData<PointT> prism;
00455     prism.setHeightLimits (0.025, 0.100);
00456     prism.setInputCloud (input_cloud);
00457     prism.setInputPlanarHull (hull_cloud.makeShared());
00458     prism.setViewPoint (0.0, 0.0, 1.5);
00459     prism.segment (*handle_indices);
00460 
00461     if ( verbose )
00462     {
00463       ROS_INFO ("The number of handle indices %d", (int) handle_indices->indices.size ());
00464     }
00465 
00466     // Declare the cloud which represents the handle
00467     pcl::PointCloud<PointT> handle_cloud;
00468 
00469     // Extract the handle 
00470     pcl::ExtractIndices<PointT> extraction_of_handle;
00471     extraction_of_handle.setInputCloud (input_cloud);
00472     extraction_of_handle.setIndices (handle_indices);
00473     extraction_of_handle.setNegative (false);
00474     extraction_of_handle.filter (handle_cloud);
00475 
00476     // Create id for visualization
00477     std::stringstream id_of_handle;
00478     id_of_handle << "HANDLE_" << ros::Time::now ();
00479 
00480     // Add point cloud to viewer
00481     viewer.addPointCloud (handle_cloud, id_of_handle.str());
00482 
00483     // Set the size of points for cloud
00484     viewer.setPointCloudRenderingProperties (pcl_visualization::PCL_VISUALIZER_POINT_SIZE, size_of_points, id_of_handle.str());
00485 
00486     // Wait or not wait
00487     if ( step )
00488     {
00489       // And wait until Q key is pressed
00490       viewer.spin ();
00491     }
00492 
00493     // Create id for saving
00494     std::string file_of_handle = argv [pFileIndicesPCD[0]];
00495     size_t dot_of_handle = file_of_handle.find (".");
00496     file_of_handle.insert (dot_of_handle, "-handle");
00497 
00498     // Save these points to disk
00499     pcl::io::savePCDFile (file_of_handle, handle_cloud);
00500 
00501     // ------------------------------------------------------------------------
00502 
00503   }
00504 
00505   if ( verbose )
00506   {
00507     // Displaying the overall time
00508     ROS_WARN ("Finished in %5.3g [s] !", tt.toc ());
00509   }
00510 
00511   // And wait until Q key is pressed
00512   viewer.spin ();
00513 
00514   return (0);
00515 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


autonomous_mapping
Author(s): Gheorghe Rus
autogenerated on Thu May 23 2013 08:58:16