segmentation_of_pointcloud.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2011, Lucian Cosmin Goron <goron@cs.tum.edu>, Zoltan-Csaba Marton <marton@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 "pcl/console/parse.h"
00037 
00038 // pcl dependencies
00039 #include "pcl/io/pcd_io.h"
00040 
00041 #include "pcl/surface/convex_hull.h"
00042 
00043 #include "pcl/filters/voxel_grid.h"
00044 #include "pcl/filters/passthrough.h"
00045 #include "pcl/filters/extract_indices.h"
00046 #include "pcl/filters/project_inliers.h"
00047 #include "pcl/filters/statistical_outlier_removal.h"
00048 
00049 #include "pcl/sample_consensus/method_types.h"
00050 #include "pcl/sample_consensus/impl/ransac.hpp"
00051 
00052 #include "pcl/segmentation/sac_segmentation.h"
00053 #include "pcl/segmentation/extract_clusters.h"
00054 #include "pcl/segmentation/extract_polygonal_prism_data.h"
00055 
00056 // pcl visualization dependencies
00057 #include "pcl/visualization/pcl_visualizer.h"
00058 
00059 // pcl ias sample consensus dependencies
00060 #include "pcl_ias_sample_consensus/pcl_sac_model_orientation.h"
00061 
00062 // dos pcl dependencies
00063 #include "dos_pcl/segmentation/door_detection_by_color_and_fixture.h"
00064 
00065 
00066 
00067 // Set up the right type definition of points
00068 typedef pcl::PointXYZ PointT;
00069 //typedef pcl::PointXYZRGB PointT;
00070 //typedef pcl::PointXYZINormal PointT;
00071 
00072 
00073 
00074 // Filtering's Parameters
00075 double height_of_floor = 0.025; 
00076 double height_of_ceiling = 0.050; 
00077 double height_of_walls = 0.750; 
00078 
00079 // Segmentation's Parameters
00080 double epsilon_angle = 0.010; 
00081 double plane_threshold = 0.100; 
00082 int minimum_plane_inliers = 1000; 
00083 int maximum_plane_iterations = 1000; 
00084 
00085 // Clustering's Parameters
00086 int minimum_size_of_plane_cluster = 100; 
00087 double plane_inliers_clustering_tolerance = 0.100; 
00088 int minimum_size_of_handle_cluster = 25; 
00089 double handle_clustering_tolerance = 0.050; 
00090 
00091 // Segmentation's Parameters
00092 double cluster_tolerance = 0.020; 
00093 double fixture_cluster_tolerance = 0.025; 
00094 double center_radius = 0.085; 
00095 double init_radius = 0.035; 
00096 double color_radius = 0.050; 
00097 
00098 int std_limit = 2; 
00099 int min_pts_per_cluster = 150; 
00100 int fixture_min_pts_per_cluster = 150; 
00101 
00102 // Visualization's Parameters
00103 bool step = false;
00104 bool clean = false;
00105 bool verbose = false;
00106 int size_of_points = 1;
00107 
00108 // Optional parameters
00109 bool find_box_model = true;
00110 bool segmentation_by_color_and_fixture = false;
00111 
00112 
00113 
00114 /*
00115 
00116 float getRGB (float r, float g, float b)
00117 {
00118   int res = (int (r * 255) << 16) | (int (g * 255) << 8) | int (b * 255);
00119   double rgb = *(float*)(&res);
00120   return (rgb);
00121 }
00122 
00123 */
00124 
00125 float getRGB (float r, float g, float b)
00126 {
00127   int res = (int (r * 255) << 16) | (int (g * 255) << 8) | int (b * 255);
00128   double rgb = *reinterpret_cast<float*>(&res);
00129   return (rgb);
00130 }
00131 
00133 
00134 void getMinAndMax (boost::shared_ptr<const pcl::PointCloud <PointT> > cloud_, Eigen::VectorXf model_coefficients, boost::shared_ptr<pcl::SACModelOrientation<pcl::Normal> > model, std::vector<int> &min_max_indices, std::vector<float> &min_max_distances)
00135 {
00136 
00137   boost::shared_ptr<std::vector<int> > inliers = model->getIndices();
00138   boost::shared_ptr<std::vector<int> > indices = model->getIndices();
00139 
00140   // Initialize result vectors
00141   min_max_indices.resize (6);
00142   min_max_distances.resize (6);
00143   min_max_distances[0] = min_max_distances[2] = min_max_distances[4] = +DBL_MAX;
00144   min_max_distances[1] = min_max_distances[3] = min_max_distances[5] = -DBL_MAX;
00145 
00146   // The 3 coordinate axes are nm, nc and axis_
00147   Eigen::Vector3f nm;
00148   nm[0] = model_coefficients[0];
00149   nm[1] = model_coefficients[1];
00150   nm[2] = model_coefficients[2];
00151   //Eigen::Vector3f nm = Eigen::Vector3d::Map(&(*model_coefficients)[0]).cast<float> ();
00152   Eigen::Vector3f nc = model->axis_.cross (nm);
00153 
00154   // Find minimum and maximum distances from origin along the three axes
00155   for (std::vector<int>::iterator it = inliers->begin (); it != inliers->end (); it++)
00156   //for (unsigned i = 0; i < inliers.size (); i++)
00157   {
00158     // @NOTE inliers is a list of indices of the indices_ array!
00159     Eigen::Vector3f point (cloud_->points[(*indices)[*it]].x, cloud_->points[(*indices)[*it]].y, cloud_->points[(*indices)[*it]].z);
00160     //Eigen::Vector3f point (cloud_->points[indices_[*it]].x - center.x, cloud_->points[indices_[*it]].y - center.y, cloud_->points[indices_[*it]].z - center.z);
00161     //Eigen::Vector3f point (cloud_->points[indices_[inliers[i]]].x, cloud_->points[indices_[inliers[i]]].y, cloud_->points[indices_[inliers[i]]].z);
00162     double dists[3];
00163     dists[0] = nm.dot(point);
00164     dists[1] = nc.dot(point);
00165     dists[2] = model->axis_.dot(point);
00166     for (int d=0; d<3; d++)
00167     {
00168       if (min_max_distances[2*d+0] > dists[d]) { min_max_distances[2*d+0] = dists[d]; min_max_indices[2*d+0] = *it; }
00169       if (min_max_distances[2*d+1] < dists[d]) { min_max_distances[2*d+1] = dists[d]; min_max_indices[2*d+1] = *it; }
00170     }
00171   }
00172 
00173 }
00174 
00175 //*/
00176 
00178 
00182 
00183 
00184 bool findBoxModel (boost::shared_ptr<const pcl::PointCloud <PointT> > cloud, pcl::PointCloud<pcl::Normal> nrmls, std::vector<double> &coeff, double eps_angle_ = 0.1, double success_probability_ = 0.99, int verbosity_level_ = 1)
00185 {
00186 
00187   if (verbosity_level_ > 0) ROS_INFO ("[RobustBoxEstimation] Looking for box in a cluster of %u points", (unsigned)cloud->points.size ());
00188 
00189   // Compute center point
00190   //cloud_geometry::nearest::computeCentroid (*cloud, box_centroid_);
00191 
00192 /* 
00193   pcl::PointCloud<pcl::Normal> nrmls ;
00194   nrmls.header = cloud->header;
00195   nrmls.points.resize(cloud->points.size());
00196   for(size_t i = 0 ; i < cloud->points.size(); i++)
00197   {
00198     nrmls.points[i].normal[0] = cloud->points[i].normal[0];
00199     nrmls.points[i].normal[1] = cloud->points[i].normal[1];
00200     nrmls.points[i].normal[2] = cloud->points[i].normal[2];
00201   }
00202 */
00203 
00204   // Create model
00205   pcl::SACModelOrientation<pcl::Normal>::Ptr model = boost::make_shared<pcl::SACModelOrientation<pcl::Normal> >(nrmls.makeShared ());
00206 
00207   model->axis_[0] = 0 ;
00208   model->axis_[1] = 0 ;
00209   model->axis_[2] = 1 ;
00210 
00211   //model->setDataSet ((sensor_msgs::PointCloud*)(cloud.get())); // TODO: this is nasty :)
00212   if (verbosity_level_ > 0) ROS_INFO ("[RobustBoxEstimation] Axis is (%g,%g,%g) and maximum angular difference %g",
00213       model->axis_[0], model->axis_[1], model->axis_[2], eps_angle_);
00214 
00215   // Check probability of success and decide on method
00216   Eigen::VectorXf refined;
00217   std::vector<int> inliers;
00219   if (success_probability_ > 0 && success_probability_ < 1)
00220   {
00221     if (verbosity_level_ > 0) ROS_INFO ("[RobustBoxEstimation] Using RANSAC with stop probability of %g and model refinement", success_probability_);
00222 
00223     // Fit model using RANSAC
00224     pcl::RandomSampleConsensus<pcl::Normal> *sac = new pcl::RandomSampleConsensus<pcl::Normal> (model, eps_angle_);
00225     sac->setProbability (success_probability_);
00226     if (!sac->computeModel ())
00227     {
00228       if (verbosity_level_ > -2) ROS_ERROR ("[RobustBoxEstimation] No model found using the angular threshold of %g!", eps_angle_);
00229       return false;
00230     }
00231 
00232     // Get inliers and refine result
00233     sac->getInliers(inliers);
00234     if (verbosity_level_ > 1) cerr << "number of inliers: " << inliers.size () << endl;
00235     // Exhaustive search for best model
00236     std::vector<int> best_sample;
00237     std::vector<int> best_inliers;
00238     Eigen::VectorXf model_coefficients;
00239     for (unsigned i = 0; i < cloud->points.size (); i++)
00240     {
00241       std::vector<int> selection (1);
00242       selection[0] = i;
00243       model->computeModelCoefficients (selection, model_coefficients);
00244 
00245       model->selectWithinDistance (model_coefficients, eps_angle_, inliers);
00246       if (best_inliers.size () < inliers.size ())
00247       {
00248         best_inliers = inliers;
00249         best_sample = selection;
00250       }
00251     }
00252 
00253     // Check if successful and save results
00254     if (best_inliers.size () > 0)
00255     {
00256       model->computeModelCoefficients (best_sample, refined);
00257       //model->getModelCoefficients (refined);
00259       inliers = best_inliers;
00260       //model->setBestModel (best_sample);
00261       //model->setBestInliers (best_inliers);
00262       // refine results: needs inliers to be set!
00263       // sac->refineCoefficients(refined);
00264     }
00266     //model->computeModelCoefficients(model->getBestModel ());
00267     //Eigen::VectorXf original;
00268     //model->getModelCoefficients (original);
00269     //if (verbosity_level_ > 1) cerr << "original direction: " << original[0] << " " << original[1] << " " << original[2] << ", found at point nr " << original[3] << endl;
00270     //sac->refineCoefficients(refined);
00271    // if (verbosity_level_ > 1) cerr << "refitted direction: " << refined.at (0) << " " << refined.at (1) << " " << refined.at (2) << ", initiated from point nr " << refined.at (3) << endl;
00272    // if (refined[3] == -1)
00273    //   refined = original;
00274   }
00275   else
00276   {
00277     if (verbosity_level_ > 0) ROS_INFO ("[RobustBoxEstimation] Using exhaustive search in %ld points", (long int) cloud->points.size ());
00278 
00279     // Exhaustive search for best model
00280     std::vector<int> best_sample;
00281     std::vector<int> best_inliers;
00282     Eigen::VectorXf model_coefficients;
00283     for (unsigned i = 0; i < cloud->points.size (); i++)
00284     {
00285       std::vector<int> selection (1);
00286       selection[0] = i;
00287       model->computeModelCoefficients (selection, model_coefficients);
00288 
00289       model->selectWithinDistance (model_coefficients, eps_angle_, inliers);
00290       if (best_inliers.size () < inliers.size ())
00291       {
00292         best_inliers = inliers;
00293         best_sample = selection;
00294       }
00295     }
00296 
00297     // Check if successful and save results
00298     if (best_inliers.size () > 0)
00299     {
00300       model->computeModelCoefficients (best_sample, refined);
00301       //model->getModelCoefficients (refined);
00303       inliers = best_inliers;
00304       //model->setBestModel (best_sample);
00305       //model->setBestInliers (best_inliers);
00306       // refine results: needs inliers to be set!
00307       // sac->refineCoefficients(refined);
00308     }
00309     else
00310     {
00311       if (verbosity_level_ > -2) ROS_ERROR ("[RobustBoxEstimation] No model found using the angular threshold of %g!", eps_angle_);
00312       return false;
00313     }
00314   }
00315 
00316   // Save fixed axis
00317   coeff[12+0] = model->axis_[0];
00318   coeff[12+1] = model->axis_[1];
00319   coeff[12+2] = model->axis_[2];
00320 
00321   // Save complementary axis (cross product)
00322   coeff[9+0] = model->axis_[1]*refined[2] - model->axis_[2]*refined[1];
00323   coeff[9+1] = model->axis_[2]*refined[0] - model->axis_[0]*refined[2];
00324   coeff[9+2] = model->axis_[0]*refined[1] - model->axis_[1]*refined[0];
00325 
00326   // Save principle axis (corrected)
00327   refined[0] = - (model->axis_[1]*coeff[9+2] - model->axis_[2]*coeff[9+1]);
00328   refined[1] = - (model->axis_[2]*coeff[9+0] - model->axis_[0]*coeff[9+2]);
00329   refined[2] = - (model->axis_[0]*coeff[9+1] - model->axis_[1]*coeff[9+0]);
00330 
00331   ROS_INFO("refined[0]: %f", refined[0]);
00332   ROS_INFO("refined[1]: %f", refined[1]);
00333   ROS_INFO("refined[2]: %f", refined[2]);
00334   coeff[6+0] = refined[0];
00335   coeff[6+1] = refined[1];
00336   coeff[6+2] = refined[2];
00337 
00338 //  // Save complementary axis (AGIAN, JUST TO MAKE SURE)
00339 //  coeff[9+0] = model->axis_[1]*refined[2] - model->axis_[2]*refined[1];
00340 //  coeff[9+1] = model->axis_[2]*refined[0] - model->axis_[0]*refined[2];
00341 //  coeff[9+2] = model->axis_[0]*refined[1] - model->axis_[1]*refined[0];
00342 
00343   // Compute minimum and maximum along each dimension for the whole cluster
00344   std::vector<int> min_max_indices;
00345   std::vector<float> min_max_distances;
00346   //boost::shared_ptr<vector<int> > indices (new vector<int>);
00347   //indices = model->getIndices();
00348 
00349   //model->getMinAndMax (&refined, &inliers, min_max_indices, min_max_distances);
00350   //getMinAndMax (&refined, model->getIndices (), min_max_indices, min_max_distances);
00351   getMinAndMax (cloud, refined, model, min_max_indices, min_max_distances);
00352   //vector<int> min_max_indices = model->getMinAndMaxIndices (refined);
00353 
00354   //cerr << min_max_distances.at (1) << " " << min_max_distances.at (0) << endl;
00355   //cerr << min_max_distances.at (3) << " " << min_max_distances.at (2) << endl;
00356   //cerr << min_max_distances.at (5) << " " << min_max_distances.at (4) << endl;
00357 
00358   // Save dimensions
00359   coeff[3+0] = min_max_distances.at (1) - min_max_distances.at (0);
00360   coeff[3+1] = min_max_distances.at (3) - min_max_distances.at (2);
00361   coeff[3+2] = min_max_distances.at (5) - min_max_distances.at (4);
00362 
00363   // Distance of box's geometric center relative to origin along orientation axes
00364   double dist[3];
00365   dist[0] = min_max_distances[0] + coeff[3+0] / 2;
00366   dist[1] = min_max_distances[2] + coeff[3+1] / 2;
00367   dist[2] = min_max_distances[4] + coeff[3+2] / 2;
00368 
00369   // Compute position of the box's geometric center in XYZ
00370   coeff[0] = dist[0]*coeff[6+0] + dist[1]*coeff[9+0] + dist[2]*coeff[12+0];
00371   coeff[1] = dist[0]*coeff[6+1] + dist[1]*coeff[9+1] + dist[2]*coeff[12+1];
00372   coeff[2] = dist[0]*coeff[6+2] + dist[1]*coeff[9+2] + dist[2]*coeff[12+2];
00373   //coeff[0] = box_centroid_.x + dist[0]*coeff[6+0] + dist[1]*coeff[9+0] + dist[2]*coeff[12+0];
00374   //coeff[1] = box_centroid_.y + dist[0]*coeff[6+1] + dist[1]*coeff[9+1] + dist[2]*coeff[12+1];
00375   //coeff[2] = box_centroid_.z + dist[0]*coeff[6+2] + dist[1]*coeff[9+2] + dist[2]*coeff[12+2];
00376   if (verbosity_level_ > 0) ROS_INFO ("[RobustBoxEstimation] Cluster center x: %g, y: %g, z: %g", coeff[0], coeff[1], coeff[2]);
00377 
00378   // Print info
00379   if (verbosity_level_ > 0) ROS_INFO ("[RobustBoxEstimation] Dimensions x: %g, y: %g, z: %g",
00380       coeff[3+0], coeff[3+1], coeff[3+2]);
00381   if (verbosity_level_ > 0) ROS_INFO ("[RobustBoxEstimation] Direction vectors: \n\t%g %g %g \n\t%g %g %g \n\t%g %g %g",
00382       coeff[3+3], coeff[3+4], coeff[3+5],
00383       coeff[3+6], coeff[3+7], coeff[3+8],
00384       coeff[3+9], coeff[3+10],coeff[3+11]);
00385 
00386   return true;
00387 }
00388 
00389 //*/
00390 
00391 void getAxesOrientedSurfaces (pcl::PointCloud<PointT> &input_cloud,
00392                               pcl::PointCloud<pcl::Normal> &normals_cloud,
00393                               Eigen::Vector3f axis,
00394                               double epsilon_angle,
00395                               double plane_threshold,
00396                               int minimum_plane_inliers,
00397                               int maximum_plane_iterations,
00398                               int minimum_size_of_plane_cluster,
00399                               double plane_inliers_clustering_tolerance,
00400                               std::vector<pcl::PointCloud<PointT>::Ptr> &planar_surfaces,
00401                               std::vector<std::string> &planar_surfaces_ids,
00402                               std::vector<pcl::PointIndices::Ptr> &planar_surfaces_indices,
00403                               std::vector<pcl::ModelCoefficients::Ptr> &planar_surfaces_coefficients,
00404                               pcl::visualization::PCLVisualizer &viewer)
00405 {
00406 
00407   // Count number of fitted planes 
00408   int plane_fit = 0;
00409 
00410   // Stop condition for fitting
00411   bool stop_planes = false;
00412 
00413 
00414   // Count one hundred mishaps
00415   int counter = 0;
00416 
00417   do
00418   {
00419     // Create the segmentation object and declare variables
00420     pcl::SACSegmentationFromNormals<PointT, pcl::Normal> segmentation_of_planes;
00421     pcl::PointIndices::Ptr plane_inliers (new pcl::PointIndices ());
00422     pcl::ModelCoefficients::Ptr plane_coefficients (new pcl::ModelCoefficients ());
00423 
00424     // Set all the parameters for segmenting vertical planes
00425     segmentation_of_planes.setOptimizeCoefficients (true);
00426     segmentation_of_planes.setModelType (pcl::SACMODEL_NORMAL_PLANE);
00427     segmentation_of_planes.setNormalDistanceWeight (0.05);
00428     segmentation_of_planes.setMethodType (pcl::SAC_RANSAC);
00429     segmentation_of_planes.setDistanceThreshold (plane_threshold);
00430     segmentation_of_planes.setMaxIterations (maximum_plane_iterations);
00431     segmentation_of_planes.setAxis (axis);
00432     segmentation_of_planes.setEpsAngle (epsilon_angle);
00433     segmentation_of_planes.setInputCloud (input_cloud.makeShared());
00434     segmentation_of_planes.setInputNormals (normals_cloud.makeShared());
00435 
00436     // Obtain the plane inliers and coefficients
00437     segmentation_of_planes.segment (*plane_inliers, *plane_coefficients);
00438 
00439     if ( verbose )
00440     {
00441       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 (), 
00442           plane_coefficients->values [0], plane_coefficients->values [1], plane_coefficients->values [2], plane_coefficients->values [3], maximum_plane_iterations);
00443     }
00444 
00445     // Check if the fitted circle has enough inliers in order to be accepted
00446     if ((int) plane_inliers->indices.size () < minimum_plane_inliers) 
00447     {
00448       ROS_ERROR ("NOT ACCEPTED !");
00449 
00450       counter++;
00451 
00452       cerr <<" COUNTER = " << counter << endl ;
00453 
00454       // TODO limit the number of re-fitting for better computation time
00455 
00456       if ( counter == 25 )
00457       {
00458         // No need for fitting planes anymore
00459         stop_planes = true;
00460       }
00461     }
00462     else
00463     {
00464       ROS_WARN ("ACCEPTED !");
00465 
00466       // Reset counter
00467       counter = 0;
00468 
00469       // ----------------------------------- //
00470       // Start processing the accepted plane //
00471       // ----------------------------------- //
00472 
00473       // Point cloud of plane inliers
00474       pcl::PointCloud<PointT>::Ptr pivot_input_cloud (new pcl::PointCloud<PointT> ());
00475 
00476       // Point cloud of plane inliers
00477       pcl::PointCloud<PointT>::Ptr plane_inliers_cloud (new pcl::PointCloud<PointT> ());
00478 
00479       // Extract the circular inliers from the input cloud
00480       pcl::ExtractIndices<PointT> extraction_of_plane_inliers;
00481       // Set point cloud from where to extract
00482       extraction_of_plane_inliers.setInputCloud (input_cloud.makeShared());
00483       // Set which indices to extract
00484       extraction_of_plane_inliers.setIndices (plane_inliers);
00485       // Return the points which represent the inliers
00486       extraction_of_plane_inliers.setNegative (false);
00487       // Call the extraction function
00488       extraction_of_plane_inliers.filter (*plane_inliers_cloud);
00489       // Return the remaining points of inliers
00490       extraction_of_plane_inliers.setNegative (true);
00491       // Call the extraction function
00492       extraction_of_plane_inliers.filter (*pivot_input_cloud);
00493 
00494       // UPDATE input_cloud VARIABLE //
00495       input_cloud = *pivot_input_cloud;
00496 
00497       // Point cloud of plane inliers
00498       pcl::PointCloud<pcl::Normal>::Ptr pivot_normals_cloud (new pcl::PointCloud<pcl::Normal> ());
00499 
00500       // Extract the normals of plane inliers 
00501       pcl::ExtractIndices<pcl::Normal> extraction_of_normals;
00502 
00503       // Set normals cloud from where to extract
00504       extraction_of_normals.setInputCloud (normals_cloud.makeShared());
00505       // Return the remaining normals
00506       extraction_of_normals.setNegative (true);
00507       // Set which indices to extract
00508       extraction_of_normals.setIndices (plane_inliers);
00509       // Call the extraction function
00510       extraction_of_normals.filter (*pivot_normals_cloud);
00511 
00512       // UPDATE input_cloud VARIABLE //
00513       normals_cloud = *pivot_normals_cloud;
00514 
00515       /*
00516 
00517       // Create ID for visualization
00518       std::stringstream id_of_plane;
00519       id_of_plane << "PLANE_" << ros::Time::now();
00520 
00521       // Add point cloud to viewer
00522       viewer.addPointCloud (plane_inliers_cloud, id_of_plane.str());
00523       // Set the size of points for cloud
00524       viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, size_of_points, id_of_plane.str()); 
00525 
00526       // Wait or not wait
00527       if ( step )
00528       {
00529         // And wait until Q key is pressed
00530         viewer.spin ();
00531       }
00532 
00533       // Remove or not remove the cloud from viewer
00534       if ( clean )
00535       {
00536         // Remove the point cloud data
00537         viewer.removePointCloud (id_of_plane.str());
00538 
00539         // Wait or not wait
00540         if ( step )
00541         {
00542           // And wait until Q key is pressed
00543           viewer.spin ();
00544         }
00545       }
00546 
00547       */
00548 
00549 
00550 
00552 
00553       // Vector of clusters from inliers
00554       std::vector<pcl::PointIndices> plane_clusters;
00555       // Build kd-tree structure for clusters
00556       pcl::KdTreeFLANN<PointT>::Ptr plane_clusters_tree (new pcl::KdTreeFLANN<PointT> ());
00557 
00558       // Instantiate cluster extraction object
00559       pcl::EuclideanClusterExtraction<PointT> clustering_of_plane_inliers;
00560       // Set as input the cloud of circle inliers
00561       clustering_of_plane_inliers.setInputCloud (plane_inliers_cloud);
00562       // Radius of the connnectivity threshold
00563       clustering_of_plane_inliers.setClusterTolerance (plane_inliers_clustering_tolerance);
00564       // Minimum size of clusters
00565       clustering_of_plane_inliers.setMinClusterSize (minimum_size_of_plane_cluster);
00566       // Provide pointer to the search method
00567       clustering_of_plane_inliers.setSearchMethod (plane_clusters_tree);
00568       // Call the extraction function
00569       clustering_of_plane_inliers.extract (plane_clusters);
00570 
00571       //*/
00572 
00573 
00574 
00575       /*
00576 
00577       if ( verbose )
00578       {
00579         ROS_WARN ("Plane model has %d clusters where", plane_clusters.size());
00580         for (int c = 0; c < (int) plane_clusters.size(); c++)
00581           ROS_WARN ("  Cluster %d has %d points", c, (int) plane_clusters.at(c).indices.size());
00582       }
00583 
00584       */
00585 
00586       // Point clouds which represent the clusters of the plane inliers
00587       std::vector<pcl::PointCloud<PointT>::Ptr> plane_clusters_clouds;
00588 
00589       for (int c = 0; c < (int) plane_clusters.size(); c++)
00590       {
00591         // Local variables
00592         pcl::PointIndices::Ptr pointer_of_plane_cluster (new pcl::PointIndices (plane_clusters.at(c)));
00593         pcl::PointCloud<PointT>::Ptr cluster (new pcl::PointCloud<PointT>);
00594 
00595         // Extract the circular inliers from the input cloud
00596         pcl::ExtractIndices<PointT> extraction_of_plane_clusters;
00597         // Set point cloud from where to extract
00598         extraction_of_plane_clusters.setInputCloud (plane_inliers_cloud);
00599         // Set which indices to extract
00600         extraction_of_plane_clusters.setIndices (pointer_of_plane_cluster);
00601         // Return the points which represent the inliers
00602         extraction_of_plane_clusters.setNegative (false);
00603         // Call the extraction function
00604         extraction_of_plane_clusters.filter (*cluster);
00605 
00606         // Save cluster
00607         plane_clusters_clouds.push_back (cluster);
00608 
00609         // Save planar surface
00610         planar_surfaces.push_back (cluster);
00611 
00612         // Save coefficients of plane's planar surface
00613         planar_surfaces_coefficients.push_back (plane_coefficients);
00614 
00615         // Save indices of planar surfaces
00616         planar_surfaces_indices.push_back (pointer_of_plane_cluster);
00617 
00618         if ( verbose )
00619         {
00620           ROS_INFO ("  Planar surface %d has %d points", c, (int) cluster->points.size());
00621         }
00622       }
00623 
00624 
00625 
00626       for (int c = 0; c < (int) plane_clusters.size(); c++)
00627       {
00628         // Create ID for visualization
00629         std::stringstream id_of_surface;
00630         id_of_surface << "SURFACE_" << ros::Time::now();
00631 
00632         // Save id of planar surface
00633         planar_surfaces_ids.push_back (id_of_surface.str());
00634 
00635         // Add point cloud to viewer
00636         viewer.addPointCloud (plane_clusters_clouds.at(c), id_of_surface.str());
00637 
00638         // Set the size of points for cloud
00639         viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, size_of_points, id_of_surface.str()); 
00640 
00641         // Wait or not wait
00642         if ( step )
00643         {
00644           // And wait until Q key is pressed
00645           viewer.spin ();
00646         }
00647 
00648         // Remove or not remove the cloud from viewer
00649         if ( clean )
00650         {
00651           // Remove the point cloud data
00652           viewer.removePointCloud (id_of_surface.str());
00653           // Wait or not wait
00654           if ( step )
00655           {
00656             // And wait until Q key is pressed
00657             viewer.spin ();
00658           }
00659         }
00660       }
00661 
00662 
00663       /*
00664 
00665       // Remove the point cloud data
00666       viewer.removePointCloud (id_of_plane.str());
00667 
00668       */
00669 
00670       // ------------------------------------- //
00671       // End of processing the accepted circle //
00672       // ------------------------------------- //
00673 
00674     }
00675 
00676     // number of fitted planes
00677     plane_fit++;
00678 
00679     // --------------------------------------- //
00680     // Check for stop condition and print info //
00681     // --------------------------------------- //
00682 
00683     // Print the number of points left for model fitting
00684     if ( (int) input_cloud.points.size () < minimum_plane_inliers )
00685       ROS_ERROR ("%d < %d | Stop !", (int) input_cloud.points.size (), minimum_plane_inliers);
00686     else
00687       if ( (int) input_cloud.points.size () > minimum_plane_inliers )
00688         ROS_INFO ("%d > %d | Continue... ", (int) input_cloud.points.size (), minimum_plane_inliers);
00689       else
00690         ROS_INFO ("%d = %d | Continue... ", (int) input_cloud.points.size (), minimum_plane_inliers);
00691 
00692   } while ((int) input_cloud.points.size () > minimum_plane_inliers && stop_planes == false);
00693 }
00694 
00695 
00696 
00698 
00700 int main (int argc, char** argv)
00701 {
00702 
00703   // --------------------------------------------------------------- //
00704   // ------------------ Check and parse arguments ------------------ //
00705   // --------------------------------------------------------------- //
00706 
00707   // Argument check and info about
00708   if (argc < 2)
00709   {
00710     ROS_INFO (" ");
00711     ROS_INFO ("Syntax is: %s <input>.pcd <options>", argv[0]);
00712     ROS_INFO ("  where <options> are:");
00713     ROS_INFO ("    -height_of_floor X                       = Height of floor layer of point cloud data.");
00714     ROS_INFO ("    -height_of_ceiling X                     = Height of ceiling layer of point cloud data.");
00715     ROS_INFO ("    -height_of_walls X                       = Minimum height of walls in the point cloud data.");
00716     ROS_INFO (" ");
00717     ROS_INFO ("    -epsilon_angle X                         = The maximum allowed difference between the plane normal and the given axis.");
00718     ROS_INFO ("    -plane_threshold X                       = Distance to the fitted plane model.");
00719     ROS_INFO ("    -minimum_plane_inliers X                 = Minimum number of inliers of the fitted plane in order to be accepted.");
00720     ROS_INFO ("    -maximum_plane_iterations X              = Maximum number of interations for fitting the plane model.");
00721     ROS_INFO (" ");
00722     ROS_INFO ("    -minimum_size_of_plane_cluster           = Minimum cluster size of plane inliers.");
00723     ROS_INFO ("    -plane_inliers_clustering_tolerance      = The clustering tolerance of plane inliers");
00724     ROS_INFO ("    -minimum_size_of_handle_cluster          = Minimum cluster size of handle points.");
00725     ROS_INFO ("    -handle_clustering_tolerance             = The clustering tolerance of handle points.");
00726     ROS_INFO (" ");
00727     ROS_INFO ("    -step B                                  = Wait or not wait.");
00728     ROS_INFO ("    -clean B                                 = Remove or not remove.");
00729     ROS_INFO ("    -verbose B                               = Display step by step info.");
00730     ROS_INFO ("    -size_of_points D                        = Set the size of points");
00731     ROS_INFO (" ");
00732     return (-1);
00733   }
00734 
00735   // Take only the first .pcd file into account
00736   std::vector<int> pFileIndicesPCD = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
00737   if (pFileIndicesPCD.size () == 0)
00738   {
00739     ROS_ERROR ("No .pcd file given as input!");
00740     return (-1);
00741   }
00742 
00743   // Parse the arguments for filtering
00744   pcl::console::parse_argument (argc, argv, "-height_of_floor", height_of_floor);
00745   pcl::console::parse_argument (argc, argv, "-height_of_ceiling", height_of_ceiling);
00746   pcl::console::parse_argument (argc, argv, "-height_of_walls", height_of_walls);
00747 
00748   // Parse arguments for fitting plane models
00749   pcl::console::parse_argument (argc, argv, "-epsilon_angle", epsilon_angle);
00750   pcl::console::parse_argument (argc, argv, "-plane_threshold", plane_threshold);
00751   pcl::console::parse_argument (argc, argv, "-minimum_plane_inliers", minimum_plane_inliers);
00752   pcl::console::parse_argument (argc, argv, "-maximum_plane_iterations", maximum_plane_iterations);
00753 
00754   // Parse arguments for clustering
00755   pcl::console::parse_argument (argc, argv, "-minimum_size_of_plane_cluster", minimum_size_of_plane_cluster);
00756   pcl::console::parse_argument (argc, argv, "-plane_inliers_clustering_tolerance", plane_inliers_clustering_tolerance);
00757   pcl::console::parse_argument (argc, argv, "-minimum_size_of_handle_cluster", minimum_size_of_handle_cluster);
00758   pcl::console::parse_argument (argc, argv, "-handle_clustering_tolerance", handle_clustering_tolerance);
00759 
00760   // Parse the arguments for segmenting by color and fixture
00761   pcl::console::parse_argument (argc, argv, "-cluster_tolerance", cluster_tolerance);
00762   pcl::console::parse_argument (argc, argv, "-fixture_cluster_tolerance", fixture_cluster_tolerance);
00763   pcl::console::parse_argument (argc, argv, "-center_radius", center_radius);
00764   pcl::console::parse_argument (argc, argv, "-init_radius", init_radius);
00765   pcl::console::parse_argument (argc, argv, "-color_radius", color_radius);
00766 
00767   pcl::console::parse_argument (argc, argv, "-std_limit", std_limit);
00768   pcl::console::parse_argument (argc, argv, "-min_pts_per_cluster", min_pts_per_cluster);
00769   pcl::console::parse_argument (argc, argv, "-fixture_min_pts_per_cluster", fixture_min_pts_per_cluster);
00770 
00771   // Parse arguments for visualization
00772   pcl::console::parse_argument (argc, argv, "-step", step);
00773   pcl::console::parse_argument (argc, argv, "-clean", clean);
00774   pcl::console::parse_argument (argc, argv, "-verbose", verbose);
00775   pcl::console::parse_argument (argc, argv, "-size_of_points", size_of_points);
00776 
00777   // Parsing the optional arguments
00778   pcl::console::parse_argument (argc, argv, "-find_box_model", find_box_model);
00779   pcl::console::parse_argument (argc, argv, "-segmentation_by_color_and_fixture", segmentation_by_color_and_fixture);
00780 
00781   // ----------------------------------------------------- //
00782   // ------------------ Initializations ------------------ //
00783   // ----------------------------------------------------- //
00784 
00785   // Initialize random number generator
00786   srand (time(0));
00787 
00788   // Initialize ros time
00789   ros::Time::init();
00790 
00791   // Declare the timer
00792   pcl::console::TicToc tt;
00793 
00794   // Starting timer
00795   tt.tic ();
00796 
00797   if ( verbose )
00798   {
00799     // Displaying when the timer starts
00800     ROS_WARN ("Timer started !");
00801   }
00802 
00803   // ---------------------------------------------------------------- //
00804   // ------------------ Visualize point cloud data ------------------ //
00805   // ---------------------------------------------------------------- //
00806 
00807   // Open a 3D viewer
00808   pcl::visualization::PCLVisualizer viewer ("3D VIEWER");
00809   // Set the background of viewer
00810   viewer.setBackgroundColor (0.0, 0.0, 0.0);
00811   // Add system coordiante to viewer
00812   viewer.addCoordinateSystem (0.75f);
00813   // Parse the camera settings and update the internal camera
00814   viewer.getCameraParameters (argc, argv);
00815   // Update camera parameters and render
00816   viewer.updateCamera ();
00817 
00818   // --------------------------------------------------------------- //
00819   // ------------------ Load the point cloud data ------------------ //
00820   // --------------------------------------------------------------- //
00821 
00822   // Input point cloud data
00823   pcl::PointCloud<PointT>::Ptr input_cloud (new pcl::PointCloud<PointT> ());
00824 
00825   // Auxiliary
00826   pcl::PointCloud<PointT>::Ptr auxiliary_input_cloud (new pcl::PointCloud<PointT> ());
00827 
00828   // Load point cloud data
00829   if (pcl::io::loadPCDFile (argv[pFileIndicesPCD[0]], *input_cloud) == -1)
00830   {
00831     ROS_ERROR ("Couldn't read file %s", argv[pFileIndicesPCD[0]]);
00832     return (-1);
00833   }
00834   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 ());
00835 
00836  
00837   // Add the input cloud
00838   viewer.addPointCloud (input_cloud, "INPUT");
00839 
00840   // Color the cloud in white
00841   viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0, 0.0, 0.0, "INPUT");
00842 
00843   // Set the size of points for cloud
00844   viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, size_of_points - 1, "INPUT"); 
00845 
00846   // Wait or not wait
00847   if ( step )
00848   {
00849     // And wait until Q key is pressed
00850     viewer.spin ();
00851   }
00852 
00853   // Remove or not remove the cloud from viewer
00854   if ( clean )
00855   {
00856     // Remove the point cloud data
00857     viewer.removePointCloud ("INPUT");
00858 
00859     // Wait or not wait
00860     if ( step )
00861     {
00862       // And wait until Q key is pressed
00863       viewer.spin ();
00864     }
00865   }
00866 
00867   // TODO declare a working cloud and always update it 
00868   // TODO never modify the original input cloud
00869 
00870   // Auxiliary input cloud
00871   pcl::io::loadPCDFile (argv [pFileIndicesPCD [0]], *auxiliary_input_cloud);
00872 
00873   // Get position of dot in path of file 
00874   std::string file = argv [pFileIndicesPCD[0]];
00875   size_t dot = file.find (".");
00876   size_t slash = file.find_last_of ("/");
00877   std::string directory = file.substr (slash, dot - slash);
00878 
00879   // TODO insert option for statistical outlier removal -statistical_outlier_removal B alog with its parameters
00880 
00881   /*
00882 
00883   // ------------------------------------------------------------------------ //
00884   // ------------------ Removal of outliers for input data ------------------ //
00885   // ------------------------------------------------------------------------ //
00886 
00887   // Filtered point cloud
00888   pcl::PointCloud<PointT>::Ptr filtered_cloud (new pcl::PointCloud<PointT> ());
00889 
00890   // Create the filtering object
00891   pcl::StatisticalOutlierRemoval<PointT> sor;
00892   // Set which point cloud to filter
00893   sor.setInputCloud (input_cloud);
00894   // Set number of points for mean distance estimation
00895   sor.setMeanK (50);
00896   // Set the standard deviation multiplier threshold
00897   sor.setStddevMulThresh (1.0);
00898   // Call the filtering method
00899   sor.filter (*filtered_cloud);
00900 
00901   ROS_INFO ("Statistical Outlier Removal ! before: %d points | after: %d points | filtered: %d points", input_cloud->points.size (),  filtered_cloud->points.size (), input_cloud->points.size () - filtered_cloud->points.size ());
00902 
00903   // Add the filtered cloud 
00904   viewer.addPointCloud (filtered_cloud, "FILTERED");
00905   // Set the size of points for cloud
00906   viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, size_of_points - 1, "FILTERED"); 
00907 
00908   // Wait or not wait
00909   if ( step )
00910   {
00911     // And wait until Q key is pressed
00912     viewer.spin ();
00913   }
00914 
00915   // Remove the point cloud data
00916   viewer.removePointCloud ("INPUT");
00917 
00918   // Wait or not wait
00919   if ( step )
00920   {
00921     // And wait until Q key is pressed
00922     viewer.spin ();
00923   }
00924 
00925   // Save the filtered cloud to the working cloud
00926   *input_cloud = *filtered_cloud;
00927 
00928   */
00929 
00930 
00931   
00932   // The minimum and maximum height of point cloud
00933   PointT point_with_minimum_height, point_with_maximum_height;
00934   pcl::getMinMax3D (*input_cloud, point_with_minimum_height, point_with_maximum_height);
00935   double height_of_cloud = point_with_maximum_height.z - point_with_minimum_height.z;
00936 
00937   height_of_floor = height_of_cloud * height_of_floor;
00938   height_of_ceiling = height_of_cloud * height_of_ceiling;
00939   height_of_walls = height_of_cloud * height_of_walls;
00940 
00941   if ( verbose )
00942   {
00943     ROS_INFO ("The point with minimum height is (%6.3f,%6.3f,%6.3f)", point_with_minimum_height.x, point_with_minimum_height.y, point_with_minimum_height.z);
00944     ROS_INFO ("The point with maximum height is (%6.3f,%6.3f,%6.3f)", point_with_maximum_height.x, point_with_maximum_height.y, point_with_maximum_height.z);
00945     ROS_INFO ("The height of point cloud is %5.3f meters", height_of_cloud);
00946 
00947     ROS_INFO ("Height of floor is %5.3f meters", height_of_floor);
00948     ROS_INFO ("Height of ceiling is %5.3f meters", height_of_ceiling);
00949     ROS_INFO ("Height of walls is %5.3f meters", height_of_walls);
00950   } 
00951 
00952 
00953 
00954   // -------------------------------------------------------------------- //
00955   // ------------------ Start with filtering the cloud ------------------ //
00956   // -------------------------------------------------------------------- //
00957   
00958   // Create the filtering object
00959   pcl::PassThrough<PointT> pass;
00960   pass.setInputCloud (input_cloud);
00961   pass.setFilterFieldName ("z");
00962 
00963   // Point cloud of floor points
00964   pcl::PointCloud<PointT>::Ptr floor_cloud (new pcl::PointCloud<PointT> ());
00965 
00966   // Set floor limits
00967   pass.setFilterLimits (point_with_minimum_height.z, point_with_minimum_height.z + height_of_floor);
00968 
00969   // Call the filtering function
00970   pass.setFilterLimitsNegative (false);
00971   pass.filter (*floor_cloud);
00972   pass.setFilterLimitsNegative (true);
00973   pass.filter (*input_cloud);
00974 
00975   // Create file name for saving
00976   std::string floor_filename = argv [pFileIndicesPCD [0]];
00977   floor_filename.insert (dot, "-floor");
00978 
00979   // Save these points to disk
00980   pcl::io::savePCDFile (floor_filename, *floor_cloud);
00981 
00982   if ( verbose )
00983   {
00984     // Show the floor's number of points
00985     ROS_INFO ("The floor has %d points and was saved to data/floor.pcd", (int) floor_cloud->points.size ());
00986   }
00987  
00988   // Point cloud of ceiling points
00989   pcl::PointCloud<PointT>::Ptr ceiling_cloud (new pcl::PointCloud<PointT> ());
00990 
00991   // Set ceiling limits
00992   pass.setFilterLimits (point_with_maximum_height.z - height_of_ceiling, point_with_maximum_height.z);
00993 
00994   // Call the filtering function
00995   pass.setFilterLimitsNegative (false);
00996   pass.filter (*ceiling_cloud);
00997   pass.setFilterLimitsNegative (true);
00998   pass.filter (*input_cloud);
00999 
01000   // Create file name for saving
01001   std::string ceiling_filename = argv [pFileIndicesPCD [0]];
01002   ceiling_filename.insert (dot, "-ceiling");
01003 
01004   // Save these points to disk
01005   pcl::io::savePCDFile (ceiling_filename, *ceiling_cloud);
01006 
01007   if ( verbose )
01008   {
01009     // Show the ceiling's number of points
01010     ROS_INFO ("The ceiling has %d points and was saved to data/ceiling.pcd", (int) ceiling_cloud->points.size ());
01011   }
01012 
01013   // Point cloud of walls points
01014   pcl::PointCloud<PointT>::Ptr walls_cloud (new pcl::PointCloud<PointT> ());
01015 
01016   // Save the cloud with the walls
01017   *walls_cloud = *input_cloud;
01018 
01019   // Create file name for saving
01020   std::string walls_filename = argv [pFileIndicesPCD [0]];
01021   walls_filename.insert (dot, "-walls");
01022 
01023   // Save these points to disk
01024   pcl::io::savePCDFile (walls_filename, *walls_cloud);
01025 
01026   if ( verbose )
01027   {
01028     // Show the walls' number of points
01029     ROS_INFO ("The walls have %d points and were saved to data/walls.pcd", (int) walls_cloud->points.size ());
01030   }
01031 
01032 
01033  
01034   // -------------------------------------------------------------- //
01035   // ------------------ Visulize filtered clouds ------------------ //
01036   // -------------------------------------------------------------- //
01037 
01038   // Add point cloud to viewer
01039   viewer.addPointCloud (floor_cloud, "FLOOR");
01040   // Color the cloud in red
01041   viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "FLOOR");
01042   // Set the size of points for cloud
01043   viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, size_of_points - 1, "FLOOR"); 
01044 
01045   // Wait or not wait
01046   if ( step )
01047   {
01048     // And wait until Q key is pressed
01049     viewer.spin ();
01050   }
01051 
01052   // Remove or not remove the cloud from viewer
01053   if ( clean )
01054   {
01055     // Remove the point cloud data
01056     viewer.removePointCloud ("FLOOR");
01057 
01058     // Wait or not wait
01059     if ( step )
01060     {
01061       // And wait until Q key is pressed
01062       viewer.spin ();
01063     }
01064   }
01065 
01066   // Add the input cloud
01067   viewer.addPointCloud (walls_cloud, "WALLS");
01068   // Color the cloud in green
01069   viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0, 1.0, 0.0, "WALLS");
01070   // Set the size of points for cloud
01071   viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, size_of_points - 1, "WALLS"); 
01072 
01073   // Wait or not wait
01074   if ( step )
01075   {
01076     // And wait until Q key is pressed
01077     viewer.spin ();
01078   }
01079 
01080   // Remove or not remove the cloud from viewer
01081   if ( clean )
01082   {
01083     // Remove the point cloud data
01084     viewer.removePointCloud ("WALLS");
01085 
01086     // Wait or not wait
01087     if ( step )
01088     {
01089       // And wait until Q key is pressed
01090       viewer.spin ();
01091     }
01092   }
01093 
01094   /*
01095 
01096   // Add point cloud to viewer
01097   viewer.addPointCloud (ceiling_cloud, "CEILING");
01098   // Color the cloud in blue
01099   viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0, 0.0, 1.0, "CEILING");
01100   // Set the size of points for cloud
01101   viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, size_of_points - 1, "CEILING"); 
01102 
01103   // Wait or not wait
01104   if ( step )
01105   {
01106     // And wait until Q key is pressed
01107     viewer.spin ();
01108   }
01109 
01110   // Remove or not remove the cloud from viewer
01111   if ( clean )
01112   {
01113     // Remove the point cloud data
01114     viewer.removePointCloud ("CEILING");
01115 
01116     // Wait or not wait
01117     if ( step )
01118     {
01119       // And wait until Q key is pressed
01120       viewer.spin ();
01121     }
01122   }
01123 
01124   */
01125 
01126 
01127 
01128   // ------------------------------------------------------ //
01129   // ------------------ Lose the ceiling ------------------ //
01130   // ------------------------------------------------------ //
01131 
01132   // Remove the point cloud data
01133   viewer.removePointCloud ("INPUT");
01134 
01135   // Wait or not wait
01136   if ( step )
01137   {
01138     // And wait until Q key is pressed
01139     viewer.spin ();
01140   }
01141 
01142   // Remove the point cloud data
01143   viewer.removePointCloud ("FLOOR");
01144 
01145   // Wait or not wait
01146   if ( step )
01147   {
01148     // And wait until Q key is pressed
01149     viewer.spin ();
01150   }
01151 
01152   // ------------------------------------------------------------------- //
01153   // ------------------ Estiamte 3D normals of points ------------------ //
01154   // ------------------------------------------------------------------- //
01155  
01156   // Point cloud of normals
01157   pcl::PointCloud<pcl::Normal>::Ptr normals_cloud (new pcl::PointCloud<pcl::Normal> ());
01158   // Build kd-tree structure for normals
01159   pcl::KdTreeFLANN<PointT>::Ptr normals_tree (new pcl::KdTreeFLANN<PointT> ());
01160 
01161   // Create object for normal estimation
01162   pcl::NormalEstimation<PointT, pcl::Normal> estimation_of_normals;
01163   // Provide pointer to the search method
01164   estimation_of_normals.setSearchMethod (normals_tree);
01165   // Set for which point cloud to compute the normals
01166   estimation_of_normals.setInputCloud (input_cloud);
01167   // Set number of k nearest neighbors to use
01168   estimation_of_normals.setKSearch (50);
01169   // Estimate the normals
01170   estimation_of_normals.compute (*normals_cloud);
01171 
01172   if ( verbose )
01173   {
01174     ROS_INFO ("Remaning cloud has %d points", (int) input_cloud->points.size());
01175     ROS_INFO ("With %d normals of course", (int) normals_cloud->points.size());
01176   }
01177 
01179   // Find Box Model of a point cloud
01180 
01181   Eigen::Vector3f X, Y, Z; 
01182 
01183   if (find_box_model)
01184   {
01185     pcl::VoxelGrid<PointT> vgrid;
01186     vgrid.setLeafSize (0.05, 0.05, 0.05);
01187     vgrid.setInputCloud (input_cloud);
01188     pcl::PointCloud<PointT>::Ptr cloud_downsampled (new pcl::PointCloud<PointT> ()); 
01189 //    pcl::PointCloud<PointT> cloud_downsampled;
01190     vgrid.filter (*cloud_downsampled);
01191     std::vector<double> coefficients (15, 0.0);
01192 
01193     //bool yes_found_model = find_model (cloud_downsampled.makeShared(), *normals_cloud, coefficients);
01194 
01195     if ( findBoxModel (cloud_downsampled, *normals_cloud, coefficients) )
01196     {
01197       ROS_INFO("Box Model found");
01198       X = Eigen::Vector3f (coefficients [6+0], coefficients [6+1], coefficients [6+2]); 
01199       Y = Eigen::Vector3f (coefficients [9+0], coefficients [9+1], coefficients [9+2]); 
01200       Z = Eigen::Vector3f (coefficients[12+0], coefficients[12+1], coefficients[12+2]); 
01201     }
01202 
01203     // Wait or not wait
01204     if ( step )
01205     {
01206       // And wait until Q key is pressed
01207       viewer.spin ();
01208     }
01209 
01211     // Add the input cloud
01212     viewer.addPointCloud (cloud_downsampled, "DOWNSAMPLED");
01213     // Color the cloud in green
01214     viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0, 1.0, 0.0, "DOWNSAMPLED");
01215     // Set the size of points for cloud
01216     viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, size_of_points * 2, "DOWNSAMPLED");
01217 
01218     // Wait or not wait
01219     if ( step )
01220     {
01221       // And wait until Q key is pressed
01222       viewer.spin ();
01223     }
01224 
01225     // Remove the point cloud data
01226     viewer.removePointCloud ("DOWNSAMPLED");
01227 
01228     // Wait or not wait
01229     if ( step )
01230     {
01231       // And wait until Q key is pressed
01232       viewer.spin ();
01233     }
01234     //*/
01235 
01236     // Create file name for saving
01237     std::string downsampled_filename = argv [pFileIndicesPCD [0]];
01238     downsampled_filename.insert (dot, "-downsampled");
01239 
01240     // Save these points to disk
01241     pcl::io::savePCDFile (downsampled_filename, *cloud_downsampled);
01242   }
01243   else 
01244   {
01245     // If no vectors, the stick with the unit vectors
01246     X = Eigen::Vector3f (1.0, 0.0, 0.0); 
01247     Y = Eigen::Vector3f (0.0, 1.0, 0.0); 
01248     Z = Eigen::Vector3f (0.0, 0.0, 1.0); 
01249   }
01250 
01251   /*
01252   // Already computed axes for PointXYZINormal cloud
01253   X = Eigen::Vector3f (-0.99613, -0.08719,  0.00000);
01254   Y = Eigen::Vector3f ( 0.08719, -0.99613,  0.00000);
01255   Z = Eigen::Vector3f ( 0.00000,  0.00000,  1.00000);
01256   */
01257 
01258   /*
01259   // Already computed axes for PointXYRGB cloud
01260   X = Eigen::Vector3f (-0.02082,  0.99978, -0.00000);
01261   Y = Eigen::Vector3f (-0.99978, -0.02082,  0.00000);
01262   Z = Eigen::Vector3f ( 0.00000,  0.00000,  1.00000);
01263   */
01264 
01265   /*
01266   // Already computed axes for PointXYZ cloud
01267   X = Eigen::Vector3f (-0.15565, -0.98777,  0.00000);
01268   Y = Eigen::Vector3f ( 0.98777, -0.15565,  0.00000);
01269   Z = Eigen::Vector3f ( 0.00000,  0.00000,  1.00000);
01270   */
01271 
01272   if ( verbose )
01273   {
01274     ROS_INFO ("The furniture surfaces will be detected along the following axes:");
01275     ROS_INFO ("  X = Eigen::Vector3f (%8.5f, %8.5f, %8.5f)", X[0], X[1], X[2]);
01276     ROS_INFO ("  Y = Eigen::Vector3f (%8.5f, %8.5f, %8.5f)", Y[0], Y[1], Y[2]);
01277     ROS_INFO ("  Z = Eigen::Vector3f (%8.5f, %8.5f, %8.5f)", Z[0], Z[1], Z[2]);
01278 
01279     // Wait or not wait
01280     if ( step )
01281     {
01282       // And wait until Q key is pressed
01283       viewer.spin ();
01284     }
01285   }
01286 
01287   // ------------------------------------------------------------------------------------- //
01288   // ------------------ Segment horizontal and vertical planar surfaces ------------------ //
01289   // ------------------------------------------------------------------------------------- //
01290 
01291   // Point clouds which represent the clusters of the plane inliers
01292   std::vector<pcl::PointCloud<PointT>::Ptr> planar_surfaces;
01293 
01294   // String ids of planar surfaces
01295   std::vector<std::string> planar_surfaces_ids;
01296 
01297   // Indices of planar surfaces
01298   std::vector<pcl::PointIndices::Ptr> planar_surfaces_indices;
01299 
01300   // Coefficients of planar surfaces
01301   std::vector<pcl::ModelCoefficients::Ptr> planar_surfaces_coefficients;
01302 
01303   // Pivot Auxiliary 
01304   pcl::PointCloud<PointT>::Ptr pivot_auxiliary_input_cloud (new pcl::PointCloud<PointT> ());
01305 
01306   // Give it some points
01307   *pivot_auxiliary_input_cloud = *input_cloud;
01308 
01309   ROS_ERROR ("Z axis aligned planes");
01310 //  Eigen::Vector3f Z = Eigen::Vector3f (0.0, 0.0, 1.0); 
01311 //  Eigen::Vector3f Z = Eigen::Vector3f (0.0, 0.0, 1.0); 
01312   getAxesOrientedSurfaces (*input_cloud, *normals_cloud, Z, epsilon_angle, plane_threshold, minimum_plane_inliers, maximum_plane_iterations, minimum_size_of_plane_cluster, plane_inliers_clustering_tolerance, planar_surfaces, planar_surfaces_ids, planar_surfaces_indices, planar_surfaces_coefficients, viewer);
01313 
01314   ROS_ERROR ("Y axis aligned planes");
01315 //  Eigen::Vector3f Y = Eigen::Vector3f (0.0, 1.0, 0.0); 
01316 //  Eigen::Vector3f Y = Eigen::Vector3f (0.0782345, -0.966764, 0.0);
01317   getAxesOrientedSurfaces (*input_cloud, *normals_cloud, Y, epsilon_angle, plane_threshold, minimum_plane_inliers, maximum_plane_iterations, minimum_size_of_plane_cluster, plane_inliers_clustering_tolerance, planar_surfaces, planar_surfaces_ids, planar_surfaces_indices, planar_surfaces_coefficients, viewer);
01318 
01319   ROS_ERROR ("X axis aligned planes");
01320 //  Eigen::Vector3f X = Eigen::Vector3f (1.0, 0.0, 0.0); 
01321 //  Eigen::Vector3f X = Eigen::Vector3f (-0.966764, -0.0782345, 0.0);
01322   getAxesOrientedSurfaces (*input_cloud, *normals_cloud, X, epsilon_angle, plane_threshold, minimum_plane_inliers, maximum_plane_iterations, minimum_size_of_plane_cluster, plane_inliers_clustering_tolerance, planar_surfaces, planar_surfaces_ids, planar_surfaces_indices, planar_surfaces_coefficients, viewer);
01323 
01324 
01325 
01326   // ------------------------------------------------------------------ //
01327   // ------------------ Lose the walls ans the floor ------------------ //
01328   // ------------------------------------------------------------------ //
01329 
01330   // Remove the point cloud data
01331   viewer.removePointCloud ("FLOOR");
01332 
01333   // Remove the point cloud data
01334   viewer.removePointCloud ("WALLS");
01335 
01336   // Wait or not wait
01337   if ( step )
01338   {
01339     // And wait until Q key is pressed
01340     viewer.spin ();
01341   }
01342 
01343 
01344 
01345   // --------------------------------------------------------------------------- //
01346   // ------------------ Sort out furniture and walls surfaces ------------------ //
01347   // --------------------------------------------------------------------------- //
01348 
01349   // // // // // //
01350   // Save all points of planar patches and handles which make up the furniture
01351   // // // // // //
01352   pcl::PointCloud<PointT>::Ptr furniture (new pcl::PointCloud<PointT> ());
01353 
01354   // Type of furniture 
01355   std::vector<std::string> type_of_furniture;
01356 
01357   if ( verbose )
01358   {
01359     ROS_INFO (" ");
01360     ROS_INFO ("THERE ARE %d PLANAR SURFACES", (int) planar_surfaces.size());
01361     ROS_INFO ("AND THERE ARE %d STRING IDS", (int) planar_surfaces_ids.size());
01362     ROS_INFO (" ");
01363   }
01364 
01365   // Point clouds which represent the surfaces of furniture complaints
01366   std::vector<pcl::PointCloud<PointT>::Ptr> furniture_surfaces;
01367 
01368   // String ids of furniture surfaces
01369   std::vector<std::string> furniture_surfaces_ids;
01370 
01371   // Point clouds which represent the surfaces of walls
01372   std::vector<pcl::PointCloud<PointT>::Ptr> surfaces_of_walls;
01373 
01374   // String ids for surfaces of walls
01375   std::vector<std::string> surfaces_of_walls_ids;
01376 
01377   // Point clouds which represent the horizontal surfaces of furniture
01378   std::vector<pcl::PointCloud<PointT>::Ptr> horizontal_furniture_surfaces;
01379 
01380   // Point clouds which represent the vertical surfaces of furniture
01381   std::vector<pcl::PointCloud<PointT>::Ptr> vertical_furniture_surfaces;
01382 
01383   // Vector of ids of projected surfaces
01384   std::vector<std::string> projs_ids;
01385 
01386   for (int surface = 0; surface < (int) planar_surfaces.size(); surface++)
01387   {
01388     // The minimum and maximum height of each planar surface
01389     PointT point_with_minimum_3D_values, point_with_maximum_3D_values;
01390     pcl::getMinMax3D (*planar_surfaces.at (surface), point_with_minimum_3D_values, point_with_maximum_3D_values);
01391 
01392     if ( verbose )
01393     {
01394       ROS_INFO ("FOR PLANAR SURFACE %2d MIN HEIGHT IS %f AND MAX IS %f METERS", surface, point_with_minimum_3D_values.z, point_with_maximum_3D_values.z);
01395     }
01396 
01397     if ( point_with_maximum_3D_values.z > height_of_walls )
01398     {
01399       // Save surface of wall
01400       surfaces_of_walls.push_back (planar_surfaces.at (surface));
01401 
01402       // Save id of that surface
01403       surfaces_of_walls_ids.push_back (planar_surfaces_ids.at (surface));
01404 
01405       // Remove the point cloud data
01406       viewer.removePointCloud (planar_surfaces_ids.at (surface));
01407 
01408       // Wait or not wait
01409       if ( step )
01410       {
01411         // And wait until Q key is pressed
01412         viewer.spin ();
01413       }
01414     }
01415     else
01416     {
01417 
01418       // // // // // //
01419       // Save all points of planar patches and handles which make up the furniture
01420       // // // // // //
01421       *furniture += *planar_surfaces.at (surface);
01422 
01423       // Sort horizontal and vertical pieces of furniture
01424       if ( std::abs(planar_surfaces_coefficients.at (surface)->values[2]) > std::max (std::abs(planar_surfaces_coefficients.at (surface)->values[0]), std::abs(planar_surfaces_coefficients.at (surface)->values[1])) )
01425       {
01426         type_of_furniture.push_back ("HORIZONTAL");
01427         ROS_WARN ("HORIZONTAL");
01428 
01429         // Save the surface furniture as horizontal
01430         horizontal_furniture_surfaces.push_back (planar_surfaces.at (surface));
01431       }
01432       else
01433       {
01434         type_of_furniture.push_back ("VERTICAL");
01435         ROS_WARN ("VERTICAL");
01436 
01437         // Save the surface furniture as vertical
01438         vertical_furniture_surfaces.push_back (planar_surfaces.at (surface));
01439       }
01440 
01441       ROS_INFO ("  planar surfaces indices size: %d ", (int) planar_surfaces_indices.at (surface)->indices.size());
01442       ROS_INFO ("  table model: [%f, %f, %f, %f]", planar_surfaces_coefficients.at (surface)->values[0], planar_surfaces_coefficients.at (surface)->values[1], planar_surfaces_coefficients.at (surface)->values[2], planar_surfaces_coefficients.at (surface)->values[3]);
01443 
01444       // TODO separate the part with projection of points from the sorting of surfaces
01445 
01446 //      pcl::PointCloud<PointT> cloud_projected;
01447       pcl::PointCloud<PointT>::Ptr cloud_projected (new pcl::PointCloud<PointT> ()); 
01448       // Project the table inliers using the planar model coefficients    
01449       pcl::ProjectInliers<PointT> proj_;   
01450       proj_.setModelType (pcl::SACMODEL_NORMAL_PLANE);
01451       proj_.setInputCloud (planar_surfaces.at (surface));
01452       proj_.setModelCoefficients (planar_surfaces_coefficients.at (surface));
01453       proj_.filter (*cloud_projected);
01454 
01455       // // // // // //
01456       // Save all points of planar patches and handles which make up the furniture
01457       // // // // // //
01458 //      *furniture += cloud_projected;
01459       // pcl::ProjectInliers DOES NOT KEEP RGB INFORMATION
01460 
01461       // Remove the point cloud data
01462       viewer.removePointCloud (planar_surfaces_ids.at (surface));
01463 
01464       // Wait or not wait
01465       if ( step )
01466       {
01467         // And wait until Q key is pressed
01468         viewer.spin ();
01469       }
01470 
01471       // Visualize Projecte Poitns
01472       std::stringstream id_of_proj;
01473       id_of_proj << "PROJ_" << ros::Time::now();
01474       viewer.addPointCloud (cloud_projected, id_of_proj.str());
01475       // Set the size of points for cloud
01476       viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, size_of_points * 2, id_of_proj.str());
01477 
01478       // Save id of handle
01479       projs_ids.push_back (id_of_proj.str());
01480 
01481       // Wait or not wait
01482       if ( step )
01483       {
01484         // And wait until Q key is pressed
01485         viewer.spin ();
01486       }
01487 
01488       // Save furniture surface
01489 //      furniture_surfaces.push_back (planar_surfaces.at (surface));
01490       furniture_surfaces.push_back (cloud_projected);
01491 
01492       // Save id of that surface
01493       furniture_surfaces_ids.push_back (planar_surfaces_ids.at (surface));
01494     }
01495   }
01496 
01497   if ( verbose )
01498   {
01499     ROS_INFO (" ");
01500     ROS_INFO ("%d FURNITURE SURFACES", (int) furniture_surfaces.size());
01501     ROS_INFO ("AND %d SURFACES OF WALLS", (int) surfaces_of_walls.size());
01502     ROS_INFO (" ");
01503   }
01504 
01505    ROS_ERROR (" OVER !!! ");
01506  // Wait or not wait
01507     if ( step )
01508     {
01509       // And wait until Q key is pressed
01510       viewer.spin ();
01511     }
01512    ROS_ERROR (" OVER !!! ");
01513  // Wait or not wait
01514     if ( step )
01515     {
01516       // And wait until Q key is pressed
01517       viewer.spin ();
01518     }
01519    ROS_ERROR (" OVER !!! ");
01520  // Wait or not wait
01521     if ( step )
01522     {
01523       // And wait until Q key is pressed
01524       viewer.spin ();
01525     }
01526 
01527 
01528   // ---------------------------------------------------------------------- //
01529   // ------------------ Extraction of polygon prism data ------------------ //
01530   // ---------------------------------------------------------------------- //
01531 
01532   // // // // // //
01533   // Save all points of furniture fixtures
01534   // // // // // //
01535   pcl::PointCloud<PointT> fixtures;
01536 
01537   // Vector of ids of handles
01538   std::vector<std::string> handle_ids;
01539 
01540   // Vector of ids of handles clusters
01541   std::vector<std::string> handles_clusters_ids;
01542 
01543   for (int furniture = 0; furniture < (int) furniture_surfaces.size(); furniture++)
01544   {
01545     
01546     // Select only the vertical furniture pieces
01547     if ( type_of_furniture. at (furniture) == "VERTICAL" )
01548     { 
01549  
01550       ROS_WARN ("VERTICAL");
01551 
01552       ROS_INFO ("Furniture surface %d has %d points", furniture, (int) furniture_surfaces.at (furniture)->points.size ());
01553 
01554       pcl::PointCloud<PointT> cloud_hull;
01555       
01556       // Create a Convex Hull representation of the projected inliers
01557       pcl::ConvexHull<PointT> chull_;  
01558       chull_.setInputCloud (furniture_surfaces.at (furniture));
01559       ROS_ERROR (" BEFORE IT WORKS ");
01560       chull_.reconstruct (cloud_hull);      
01561       ROS_ERROR (" AFTERWARDS NOT ");
01562 
01563       ROS_INFO ("Convex hull %d has %d data points.", furniture, (int) cloud_hull.points.size ());
01564 
01565       /*
01566       // Visualize Convex Hulls
01567       std::stringstream id_of_hull;
01568       id_of_hull << "HULL_" << ros::Time::now();
01569       // Visualize hull of cloud 
01570       viewer.addPointCloud (cloud_hull, id_of_hull.str());
01571       // Set the size of points for cloud
01572       viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, size_of_points, id_of_hull.str());     
01573       // Wait or not wait
01574       if ( step )
01575       {
01576       // And wait until Q key is pressed
01577       viewer.spin ();
01578       }
01579       */
01580       pcl::PointIndices::Ptr cloud_object_indices (new pcl::PointIndices ());
01581 
01582       // ---[ Get the objects on top of the table
01583       pcl::ExtractPolygonalPrismData<PointT> prism_;
01584       prism_.setHeightLimits (0.025, 0.100);
01585       prism_.setInputCloud (auxiliary_input_cloud);
01586       prism_.setInputPlanarHull (cloud_hull.makeShared());
01587       prism_.setViewPoint (0.0, 0.0, 1.5);
01588       prism_.segment (*cloud_object_indices);
01589       
01590       ROS_INFO ("For %d the number of object point indices is %d", furniture, (int) cloud_object_indices->indices.size ());
01591 
01592       if ( (int) cloud_object_indices->indices.size () != 0 )
01593       {
01594         // Extract handles
01595         //pcl::PointCloud<PointT> handle_cloud;
01596         pcl::PointCloud<PointT>::Ptr handle_cloud (new pcl::PointCloud<PointT> ()); 
01597         pcl::ExtractIndices<PointT> extraction_of_handle_inliers;
01598         // Set point cloud from where to extract
01599         extraction_of_handle_inliers.setInputCloud (auxiliary_input_cloud);
01600         // Set which indices to extract
01601         extraction_of_handle_inliers.setIndices (cloud_object_indices);
01602         // Return the points which represent the inliers
01603         extraction_of_handle_inliers.setNegative (false);
01604         // Call the extraction function
01605         extraction_of_handle_inliers.filter (*handle_cloud);
01606 
01607         // // // // // //
01608         // Save all points of furniture fixtures
01609         // // // // // //
01610         //      *furniture += handle_cloud;
01611         fixtures += *handle_cloud;
01612 
01613         // Visualize handles
01614         std::stringstream id_of_handle;
01615         id_of_handle << "HANDLE_" << ros::Time::now();
01616         // Visualize handle
01617         viewer.addPointCloud (handle_cloud, id_of_handle.str());
01618         // Set the size of points for cloud
01619         viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, size_of_points * 3, id_of_handle.str());     
01620         // Wait or not wait
01621         if ( step )
01622         {
01623           // And wait until Q key is pressed
01624           viewer.spin ();
01625         }
01626 
01627         // TODO extract clusters of handle clouds
01628 
01629         // Vector of clusters from handles
01630         std::vector<pcl::PointIndices> handle_clusters;
01631         // Build kd-tree structure for clusters
01632         pcl::KdTreeFLANN<PointT>::Ptr handle_clusters_tree (new pcl::KdTreeFLANN<PointT> ());
01633 
01634         // Instantiate cluster extraction object
01635         pcl::EuclideanClusterExtraction<PointT> clustering_of_handles;
01636         // Set as input the cloud of handle
01637         clustering_of_handles.setInputCloud (handle_cloud);
01638         // Radius of the connnectivity threshold
01639         clustering_of_handles.setClusterTolerance (handle_clustering_tolerance);
01640         // Minimum size of clusters
01641         clustering_of_handles.setMinClusterSize (minimum_size_of_handle_cluster);
01642         // Provide pointer to the search method
01643         clustering_of_handles.setSearchMethod (handle_clusters_tree);
01644         // Call the extraction function
01645         clustering_of_handles.extract (handle_clusters);
01646 
01647         // Point clouds which represent the clusters of the handle
01648         std::vector<pcl::PointCloud<PointT>::Ptr> handle_clusters_clouds;
01649 
01650         // Vector of the indices of handles
01651         std::vector<pcl::PointIndices::Ptr> handle_indices;
01652 
01653         for (int c = 0; c < (int) handle_clusters.size(); c++)
01654         {
01655           // Local variables
01656           pcl::PointIndices::Ptr pointer_of_handle_cluster (new pcl::PointIndices (handle_clusters.at(c)));
01657           pcl::PointCloud<PointT>::Ptr cluster_of_handle (new pcl::PointCloud<PointT>);
01658 
01659           // Extract handle points from the input cloud
01660           pcl::ExtractIndices<PointT> extraction_of_handle_clusters;
01661           // Set point cloud from where to extract
01662           extraction_of_handle_clusters.setInputCloud (handle_cloud);
01663           // Set which indices to extract
01664           extraction_of_handle_clusters.setIndices (pointer_of_handle_cluster);
01665           // Return the points which represent the inliers
01666           extraction_of_handle_clusters.setNegative (false);
01667           // Call the extraction function
01668           extraction_of_handle_clusters.filter (*cluster_of_handle);
01669 
01670           // Fit a line to the cluster of handle //
01671 
01672           // TODO try to cluster the inleirs of line handles, usefull for the cases where handles are clustered togheter due to noise
01673 
01674           // Create the segmentation object and declare variables
01675           pcl::SACSegmentation<PointT> segmentation_of_line;
01676           pcl::PointIndices::Ptr line_inliers (new pcl::PointIndices ());
01677           pcl::ModelCoefficients::Ptr line_coefficients (new pcl::ModelCoefficients ());
01678 
01679           // Set all the parameters for segmenting vertical planes
01680           segmentation_of_line.setOptimizeCoefficients (true);
01681           segmentation_of_line.setModelType (pcl::SACMODEL_LINE);
01682           //segmentation_of_line.setNormalDistanceWeight (0.05);
01683           segmentation_of_line.setMethodType (pcl::SAC_RANSAC);
01684           segmentation_of_line.setDistanceThreshold (0.025);
01685           segmentation_of_line.setMaxIterations (maximum_plane_iterations);
01686           //segmentation_of_line.setAxis (axis);
01687           segmentation_of_line.setEpsAngle (epsilon_angle);
01688           segmentation_of_line.setInputCloud (cluster_of_handle);
01689           //segmentation_of_line.setInputNormals (normals_cloud.makeShared());
01690 
01691           // Obtain the line inliers and coefficients
01692           segmentation_of_line.segment (*line_inliers, *line_coefficients);
01693 
01694           if ( verbose )
01695           {
01696             ROS_INFO ("Line has %3d inliers with parameters P1 = (%6.3f,%6.3f,%6.3f) and P2 = (%6.3f,%6.3f,%6.3f) found in maximum %d iterations",
01697                 (int) line_inliers->indices.size (), line_coefficients->values [0], line_coefficients->values [1], line_coefficients->values [2], line_coefficients->values [3],  line_coefficients->values [4], line_coefficients->values [5], maximum_plane_iterations);
01698           }
01699 
01700           // Check if the fitted line has enough inliers in order to be accepted
01701           if ((int) line_inliers->indices.size () < 10 /*minimum_size_of_handle_cluster*/)
01702           {
01703             ROS_WARN ("NOT ACCEPTED !");
01704           }
01705           else
01706           {
01707             ROS_WARN ("ACCEPTED !");
01708 
01709             // ---------------------------- //
01710             // Start the extraction process //
01711             // ---------------------------- //
01712 
01713             // Point cloud of line inliers
01714             pcl::PointCloud<PointT>::Ptr line_inliers_cloud (new pcl::PointCloud<PointT> ());
01715 
01716             // Extract the circular inliers from the input cloud
01717             pcl::ExtractIndices<PointT> extraction_of_line;
01718             // Set point cloud from where to extract
01719             extraction_of_line.setInputCloud (cluster_of_handle);
01720             // Set which indices to extract
01721             extraction_of_line.setIndices (line_inliers);
01722 
01723             // Return the points which represent the inliers
01724             extraction_of_line.setNegative (false);
01725             // Call the extraction function
01726             extraction_of_line.filter (*line_inliers_cloud);
01727 
01728             /*
01729 
01730                ROS_INFO ("Line has %d inliers", line_inliers_cloud->points.size());
01731                ROS_INFO ("%d points remain after extraction", filtered_cloud->points.size ());
01732 
01733             */
01734 
01735             // --------------------------- //
01736             // Start visualization process //
01737             // --------------------------- //
01738 
01739             // Create id for visualization
01740             std::stringstream id_of_line;
01741             id_of_line << "LINE_" << ros::Time::now();
01742 
01743             // Add point cloud to viewer
01744             viewer.addPointCloud (line_inliers_cloud, id_of_line.str());
01745             // Set the size of points for cloud
01746             viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, size_of_points * 4, id_of_line.str()); 
01747 
01748             // Wait or not wait
01749             if ( step )
01750             {
01751               // And wait until Q key is pressed
01752               viewer.spin ();
01753             }
01754 
01755             // Save id of handles clusters
01756             handles_clusters_ids.push_back (id_of_line.str());
01757 
01758           }
01759 
01760           // // // // // //
01761           // Save all points of furniture fixtures
01762           // // // // // //
01763           //        *furniture += *cluster_of_handle;
01764           //        fixtures += *cluster_of_handle;
01765 
01766           if ( verbose )
01767           {
01768             ROS_INFO ("  Cluster of handle %d has %d points", c, (int) cluster_of_handle->points.size());
01769           }
01770 
01771           // Create id for visualization
01772           std::stringstream id_of_handle_cluster;
01773           id_of_handle_cluster << "HANDLE_CLUSTER_" << ros::Time::now();
01774           /*
01775           // Add point cloud to viewer
01776           viewer.addPointCloud (cluster_of_handle, id_of_handle_cluster.str());
01777           // Set the size of points for cloud
01778           viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, size_of_points * 4, id_of_handle_cluster.str()); 
01779 
01780           // Wait or not wait
01781           if ( step )
01782           {
01783           // And wait until Q key is pressed
01784           viewer.spin ();
01785           }
01786 
01787           // Remove or not remove the cloud from viewer
01788           if ( clean )
01789           {
01790           // Remove the point cloud data
01791           viewer.removePointCloud (id_of_handle.str());
01792           // Wait or not wait
01793           if ( step )
01794           {
01795           // And wait until Q key is pressed
01796           viewer.spin ();
01797           }
01798           }
01799           */
01800           // Save cluster of handle
01801           handle_clusters_clouds.push_back (cluster_of_handle);
01802 
01803           // Save indices of handles
01804           handle_indices.push_back (pointer_of_handle_cluster);
01805 
01806           // Save id of handle
01807           handle_ids.push_back (id_of_handle.str());
01808 
01809           //  OUT OF SERVICE BEACUSE OF THE FITTING OF LINES FOR FIXTURES 
01810           //        // Save id of handles clusters
01811           //        handles_clusters_ids.push_back (id_of_line.str());
01812         }
01813 
01814         // Remove the cloud of handle
01815         viewer.removePointCloud (id_of_handle.str());
01816       }
01817     }
01818   }
01819 
01820   /*
01821 
01822   // // // // // //
01823   // Save cloud with furniture to disk
01824   // // // // // //
01825 
01826   // Create file name for saving
01827   std::string furniture_filename = argv [pFileIndicesPCD [0]];
01828   furniture_filename.insert (dot, "-furniture");
01829 
01830   // Save these points to disk
01831   pcl::io::savePCDFile (furniture_filename, *furniture);
01832 
01833   // // // // // //
01834   // Save cloud with fixtures to disk
01835   // // // // // //
01836 
01837   // Create file name for saving
01838   std::string fixtures_filename = argv [pFileIndicesPCD [0]];
01839   fixtures_filename.insert (dot, "-fixtures");
01840 
01841   // Save these points to disk
01842   pcl::io::savePCDFile (fixtures_filename, fixtures);
01843 
01844   */
01845 
01846   /*
01847 
01848   double red24bit = getRGB (1.0, 0.0, 0.0);
01849   ROS_INFO (" red24bit = %.50f ", red24bit);
01850 
01851   for (int p = 0; p < (int) fixtures.points.size (); p++)
01852   {
01853     ROS_INFO (" RGB %.50f ", fixtures.points[p].rgb);
01854 
01855     ROS_INFO (" RGB %f ", floor (fixtures.points[p].rgb * pow(10, 40)));
01856 
01857     if ( furniture->points[p].rgb == red24bit )
01858     {
01859       ROS_INFO (" RGB %.100f ", fixtures.points[p].rgb);
01860     }
01861   }
01862 
01863   */
01864 
01865   // // // // // //
01866   // Save furniture and fixtures to disk
01867   // // // // // //
01868 
01869   pcl::PointCloud<PointT>::Ptr furniture_and_fixtures (new pcl::PointCloud<PointT> ());
01870   *furniture_and_fixtures += *furniture;
01871   *furniture_and_fixtures += fixtures;
01872 
01873   // Create file name for saving
01874   std::string furniture_and_fixtures_filename = argv [pFileIndicesPCD [0]];
01875   furniture_and_fixtures_filename.insert (dot, "-furniture_and_fixtures");
01876 
01877   // Save these points to disk
01878   pcl::io::savePCDFile (furniture_and_fixtures_filename, *furniture_and_fixtures);
01879 
01880   if ( verbose )
01881   {
01882     ROS_WARN (" ");
01883     ROS_WARN ("FURNITURE HAS %d POINTS", (int) furniture->points.size());
01884     ROS_WARN ("FIXTURES HAVE %d POINTS", (int) fixtures.points.size());
01885     ROS_WARN (" ");
01886   }
01887 
01888   // Wait or not wait
01889   if ( step )
01890   {
01891     // And wait until Q key is pressed
01892     viewer.spin ();
01893   }
01894 
01895   // Wait or not wait
01896   if ( step )
01897   {
01898     // And wait until Q key is pressed
01899     viewer.spin ();
01900   }
01901 
01902   // Wait or not wait
01903   if ( step )
01904   {
01905     // And wait until Q key is pressed
01906     viewer.spin ();
01907   }
01908 
01909   /*
01910 
01912   // Clean the 3D viewer for the segmentation by color and fixture //
01914 
01915   for (int id = 0; id < (int) handles_clusters_ids.size (); id++)
01916   {
01917     // Remove the cloud of handle
01918     viewer.removePointCloud (handles_clusters_ids.at(id));
01919 
01920     // Wait or not wait
01921     if ( step )
01922     {
01923       // And wait until Q key is pressed
01924       viewer.spin ();
01925     }
01926   }
01927 
01928   for (int id = 0; id < (int) projs_ids.size (); id++)
01929   {
01930     // Remove the cloud of handle
01931     viewer.removePointCloud (projs_ids.at(id));
01932 
01933     // Wait or not wait
01934     if ( step )
01935     {
01936       // And wait until Q key is pressed
01937       viewer.spin ();
01938     }
01939   }
01940 
01942   // The 3D viewer is now clean //
01944 
01945   */
01946 
01947   /*
01948 
01949   if ( verbose )
01950   {
01951     ROS_INFO (" ");
01952     ROS_INFO ("THERE ARE %d FURNITURE SURFACES OUT OF WHICH", (int) furniture_surfaces.size());
01953     ROS_INFO ("%d ARE HORIZONTAL AND", (int) horizontal_furniture_surfaces.size());
01954     ROS_INFO ("%d ARE VERTICAL", (int) indices_of_points_on_the_surfaces.size());
01955     ROS_INFO (" ");
01956   }
01957 
01958   */
01959 
01960   /*
01961 
01962   // Add the input cloud
01963   viewer.addPointCloud (furniture_and_fixtures, "FURNITURE_AND_FIXTURES");
01964 
01965   // Color the cloud in white
01966   viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0, 0.0, 0.0, "FURNITURE_AND_FIXTURES");
01967 
01968   // Set the size of points for cloud
01969   viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, size_of_points, "FURNITURE_AND_FIXTURES"); 
01970 
01971   // Wait or not wait
01972   if ( step )
01973   {
01974     // And wait until Q key is pressed
01975     viewer.spin ();
01976   }
01977 
01978   // Remove or not remove the cloud from viewer
01979   if ( clean )
01980   {
01981     // Remove the point cloud data
01982     viewer.removePointCloud ("FURNITURE_AND_FIXTURES");
01983 
01984     // Wait or not wait
01985     if ( step )
01986     {
01987       // And wait until Q key is pressed
01988       viewer.spin ();
01989     }
01990   }
01991 
01992   int number_of_furniture_points = furniture->points.size ();
01993  
01994   // Declare the indices of furniture points
01995   pcl::PointIndices furniture_indices;
01996 
01997   for (int idx = 0; idx < number_of_furniture_points; idx++)
01998     furniture_indices.indices.push_back (idx);
01999 
02000   // Pointer of the indices of furniture points
02001   pcl::PointIndices::Ptr furniture_indices_pointer (new pcl::PointIndices (furniture_indices));
02002 
02003   // Declare the indices of fixture points
02004   pcl::PointIndices fixtures_indices;
02005 
02006   for (int idx = number_of_furniture_points; idx < (int) furniture_and_fixtures->points.size(); idx++)
02007     fixtures_indices.indices.push_back (idx);
02008 
02009   // Pointer of the indices of fixture points
02010   pcl::PointIndices::Ptr fixtures_indices_pointer (new pcl::PointIndices (fixtures_indices));
02011 
02012   if ( verbose )
02013   {
02014     ROS_INFO ("The number of furniture points is %d", number_of_furniture_points);
02015     ROS_INFO ("Size of furniture indices is %d", (int) furniture_indices.indices.size ());
02016     ROS_INFO ("Size of fixture %d", (int) fixtures_indices.indices.size ());
02017   }
02018 
02019   */
02020 
02021   /*
02022 
02023   // Clusters segmented by color and fixture
02024   std::vector<pcl::PointIndices> clusters;
02025 
02026   // Create the object for segmenting by color and fixture
02027   dos_pcl::DoorDetectionByColorAndFixture<PointT> segmentation_by_color_and_fixture;
02028 
02029   segmentation_by_color_and_fixture.setInputCloud (furniture_and_fixtures);
02030   segmentation_by_color_and_fixture.setIndices (furniture_indices_pointer);
02031   segmentation_by_color_and_fixture.setFixtureIndices (fixtures_indices_pointer);
02032 
02033   segmentation_by_color_and_fixture.setClusterTolerance (cluster_tolerance);
02034   segmentation_by_color_and_fixture.setFixtureClusterTolerance (fixture_cluster_tolerance);
02035   segmentation_by_color_and_fixture.setCenterRadius (center_radius);
02036   segmentation_by_color_and_fixture.setInitRadius (init_radius);
02037   segmentation_by_color_and_fixture.setColorRadius (color_radius);
02038 
02039   segmentation_by_color_and_fixture.setSTDLimit (std_limit);
02040   segmentation_by_color_and_fixture.setMinClusterSize (min_pts_per_cluster);
02041   segmentation_by_color_and_fixture.setFixtureMinClusterSize (fixture_min_pts_per_cluster);
02042 
02043   // Extract the clusters by color and fixture
02044   segmentation_by_color_and_fixture.extract (clusters);
02045 
02046   // Save the clouds which represet the individual clusters
02047   std::vector<pcl::PointCloud<PointT>::Ptr > clusters_clouds;
02048 
02049   for (int c = 0; c < (int) clusters.size(); c++)
02050   {
02051     pcl::PointCloud<PointT>::Ptr cluster_cloud (new pcl::PointCloud<PointT>);
02052     pcl::PointIndices::Ptr cluster_pointer (new pcl::PointIndices (clusters.at (c)));
02053 
02054     // Extract handle points from the input cloud
02055     pcl::ExtractIndices<PointT> extraction_of_clusters;
02056     // Set point cloud from where to extract
02057     extraction_of_clusters.setInputCloud (furniture_and_fixtures);
02058     // Set which indices to extract
02059     extraction_of_clusters.setIndices (cluster_pointer);
02060     // Return the points which represent the inliers
02061     extraction_of_clusters.setNegative (false);
02062     // Call the extraction function
02063     extraction_of_clusters.filter (*cluster_cloud);
02064 
02065     // Save the clouds which represet the individual clusters
02066     clusters_clouds.push_back (cluster_cloud);
02067 
02068     if ( verbose )
02069     {
02070       ROS_INFO ("  Cluster %d has %5d points", c, (int) cluster_cloud->points.size());
02071     }
02072 
02073     // Create id for visualization
02074     std::stringstream id_of_cluster;
02075     id_of_cluster << "CLUSTER_" << ros::Time::now();
02076 
02077     // Add point cloud to viewer
02078     viewer.addPointCloud (cluster_cloud, id_of_cluster.str());
02079 
02080     // Set the size of points for cloud
02081     viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, size_of_points + 1, id_of_cluster.str()); 
02082 
02083     // Wait or not wait
02084     if ( step )
02085     {
02086       // And wait until Q key is pressed
02087       viewer.spin ();
02088     }
02089 
02090     // Remove or not remove the cloud from viewer
02091     if ( clean )
02092     {
02093       // Remove the point cloud data
02094       viewer.removePointCloud (id_of_cluster.str());
02095       // Wait or not wait
02096       if ( step )
02097       {
02098         // And wait until Q key is pressed
02099         viewer.spin ();
02100       }
02101     }
02102   }
02103 
02104   */
02105 
02106   if ( verbose )
02107   {
02108     // Displaying the overall time
02109     ROS_WARN ("Finished in %5.3g [s] !", tt.toc ());
02110   }
02111 
02112   // And wait until Q key is pressed
02113   viewer.spin ();
02114 
02115   return (0);
02116 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


autonomous_mapping
Author(s): Gheorghe Rus
autogenerated on Sun Oct 6 2013 12:04:23