visualize_segments.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"
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 // Visualization's Parameters
00060 bool step = false;
00061 bool clean = false;
00062 bool verbose = false;
00063 int size_of_hull = 10;
00064 int size_of_projected = 1;
00065 
00066 
00067 
00069 
00071 int main (int argc, char** argv)
00072 {
00073 
00074   // --------------------------------------------------------------- //
00075   // ------------------ Check and parse arguments ------------------ //
00076   // --------------------------------------------------------------- //
00077 
00078   // Argument check and info about
00079   if (argc < 2)
00080   {
00081     ROS_INFO (" ");
00082     ROS_INFO ("Syntax is: %s <input>.pcd <options>", argv[0]);
00083     ROS_INFO ("  where <options> are: ");
00084     ROS_INFO ("    -step B                                 = wait or not wait");
00085     ROS_INFO ("    -clean B                                = remove or not remove");
00086     ROS_INFO ("    -verbose B                              = display step by step info");
00087     ROS_INFO ("    -size_of_hull D                         = set the size of hull points");
00088     ROS_INFO ("    -size_of_projected D                    = set the size of projected points");
00089     ROS_INFO (" ");
00090     return (-1);
00091   }
00092 
00093   // Take only the first .pcd file into account
00094   std::vector<int> pFileIndicesPCD = terminal_tools::parse_file_extension_argument (argc, argv, ".pcd");
00095   if (pFileIndicesPCD.size () == 0)
00096   {
00097     ROS_ERROR ("No .pcd files given as input !");
00098     return (-1);
00099   }
00100 
00101   // Parse arguments for visualization
00102   terminal_tools::parse_argument (argc, argv, "-step", step);
00103   terminal_tools::parse_argument (argc, argv, "-clean", clean);
00104   terminal_tools::parse_argument (argc, argv, "-verbose", verbose);
00105   terminal_tools::parse_argument (argc, argv, "-size_of_hull", size_of_hull);
00106   terminal_tools::parse_argument (argc, argv, "-size_of_projected", size_of_projected);
00107 
00108   // ----------------------------------------------------- //
00109   // ------------------ Initializations ------------------ //
00110   // ----------------------------------------------------- //
00111 
00112   // Initialize random number generator
00113   srand (time(0));
00114 
00115   // Initialize ros time
00116   ros::Time::init();
00117 
00118   // Declare the timer
00119   terminal_tools::TicToc tt;
00120 
00121   // Starting timer
00122   tt.tic ();
00123 
00124   if ( verbose )
00125   {
00126     // Displaying when the timer starts
00127     ROS_WARN ("Timer started !");
00128   }
00129 
00130   // ----------------------------------------------------------------- //
00131   // ------------------ Visualize point clouds data ------------------ //
00132   // ----------------------------------------------------------------- //
00133 
00134   // Open a 3D viewer
00135   pcl_visualization::PCLVisualizer viewer ("3D VIEWER");
00136   // Set the background of viewer
00137   viewer.setBackgroundColor (1.0, 1.0, 1.0);
00138   // Parse the camera settings and update the internal camera
00139   viewer.getCameraParameters (argc, argv);
00140   // Update camera parameters and render
00141   viewer.updateCamera ();
00142 
00143   // ---------------------------------------------------------------- //
00144   // ------------------ Load the point clouds data ------------------ //
00145   // ---------------------------------------------------------------- //
00146 
00147   // Cloud of projected clouds
00148   pcl::PointCloud<PointT>::Ptr cloud_of_projected (new pcl::PointCloud<PointT> ());
00149 
00150   // Cloud of hull clouds
00151   pcl::PointCloud<PointT>::Ptr cloud_of_hull (new pcl::PointCloud<PointT> ());
00152 
00153   // Vector of projected clouds
00154   std::vector<pcl::PointCloud<PointT>::Ptr> vector_of_projected;
00155 
00156   // Vector of hull clouds
00157   std::vector<pcl::PointCloud<PointT>::Ptr> vector_of_hull;
00158 
00159   for (int cloud = 0; cloud < (int) pFileIndicesPCD.size (); cloud++)
00160   {
00161     // Set the name of file
00162     std::string file = argv [pFileIndicesPCD [cloud]];
00163 
00164     if ( ( (int) file.find ("projected") != -1 ) || ( (int) file.find ("hull") != -1 ) )
00165     {
00166       // Input poin cloud data
00167       pcl::PointCloud<PointT>::Ptr input (new pcl::PointCloud<PointT> ());
00168 
00169       // Load point cloud data
00170       if (pcl::io::loadPCDFile (argv [pFileIndicesPCD [cloud]], *input) == -1)
00171       {
00172         ROS_ERROR ("Couldn't read file %s", argv [pFileIndicesPCD [cloud]]);
00173         return (-1);
00174       }
00175       ROS_INFO ("Loaded %5d data points from %s with the following fields: %s", (int) (input->points.size ()), argv [pFileIndicesPCD [cloud]], pcl::getFieldsList (*input).c_str ());
00176 
00177       if ( (int) file.find ("projected") != -1 )
00178       {
00179         // Add input to projected cloud
00180         *cloud_of_projected += *input;
00181 
00182         // Add input to projected vector
00183         vector_of_projected.push_back (input);
00184       }
00185       else if ( (int) file.find ("hull") != -1 )
00186       {
00187         // Add input to hull cloud
00188         *cloud_of_hull += *input;
00189 
00190         // Add input to hull vector
00191         vector_of_hull.push_back (input);
00192       }
00193     }
00194   }
00195 
00196   // ---------------------------------------------------- //
00197   // ------------------ Visualizations ------------------ //
00198   // ---------------------------------------------------- //
00199 
00200 /*
00201   // Add the input cloud
00202   viewer.addPointCloud (*cloud_of_projected, "PROJECTED");
00203 
00204   // Color the cloud in white
00205   viewer.setPointCloudRenderingProperties (pcl_visualization::PCL_VISUALIZER_COLOR, 0.0, 0.0, 0.0, "PROJECTED");
00206 
00207   // Set the size of points for cloud
00208   viewer.setPointCloudRenderingProperties (pcl_visualization::PCL_VISUALIZER_POINT_SIZE, size_of_projected, "PROJECTED");
00209 
00210   // Wait or not wait
00211   if ( step )
00212   {
00213   // And wait until Q key is pressed
00214   viewer.spin ();
00215   }
00216 
00217 */
00218 
00220 
00221   for (int cloud = 0; cloud < (int) vector_of_projected.size (); cloud++)
00222   {
00223     // Create id for visualization
00224     std::stringstream id_of_projected;
00225     id_of_projected << "PROJECTED_" << ros::Time::now();
00226 
00227     // Visualize handle
00228     viewer.addPointCloud (*vector_of_projected.at (cloud), id_of_projected.str());
00229 
00230     // Set the size of points for cloud
00231     viewer.setPointCloudRenderingProperties (pcl_visualization::PCL_VISUALIZER_POINT_SIZE, size_of_projected, id_of_projected.str());     
00232 
00233   }
00234 
00235   // Wait or not wait
00236   if ( step )
00237   {
00238     // And wait until Q key is pressed
00239     viewer.spin ();
00240   }
00241 
00242 //*/
00243 
00245 
00246   // Add the input cloud
00247   viewer.addPointCloud (*cloud_of_hull, "HULL");
00248 
00249   // Color the cloud in white
00250   viewer.setPointCloudRenderingProperties (pcl_visualization::PCL_VISUALIZER_COLOR, 0.0, 0.0, 0.0, "HULL");
00251 
00252   // Set the size of points for cloud
00253   viewer.setPointCloudRenderingProperties (pcl_visualization::PCL_VISUALIZER_POINT_SIZE, size_of_hull, "HULL");
00254 
00255   // Wait or not wait
00256   if ( step )
00257   {
00258     // And wait until Q key is pressed
00259     viewer.spin ();
00260   }
00261 
00262 //*/
00263 
00264 /*
00265 
00266   for (int cloud = 0; cloud < (int) vector_of_hull.size (); cloud++)
00267   {
00268     // Create id for visualization
00269     std::stringstream id_of_hull;
00270     id_of_hull << "HULL_" << ros::Time::now();
00271 
00272     // Visualize handle
00273     viewer.addPointCloud (*vector_of_hull.at (cloud), id_of_hull.str());
00274 
00275     // Set the size of points for cloud
00276     viewer.setPointCloudRenderingProperties (pcl_visualization::PCL_VISUALIZER_POINT_SIZE, size_of_hull, id_of_hull.str());     
00277 
00278   }
00279 
00280   // Wait or not wait
00281   if ( step )
00282   {
00283     // And wait until Q key is pressed
00284     viewer.spin ();
00285   }
00286 
00287 */
00288 
00289   if ( verbose )
00290   {
00291     // Displaying the overall time
00292     ROS_WARN ("Finished in %5.3g [s] !", tt.toc ());
00293   }
00294 
00295   return (0);
00296 }
 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