train_linemod_template.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the copyright holder(s) nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  * $Id$
00035  *
00036  */
00037 
00038 #include <math.h>
00039 
00040 #include <pcl/point_cloud.h>
00041 #include <pcl/point_types.h>
00042 #include <pcl/io/pcd_io.h>
00043 #include <pcl/console/print.h>
00044 #include <pcl/console/parse.h>
00045 #include <pcl/console/time.h>
00046 
00047 #include <pcl/PointIndices.h>
00048 #include <pcl/ModelCoefficients.h>
00049 #include <pcl/sample_consensus/method_types.h>
00050 #include <pcl/sample_consensus/model_types.h>
00051 #include <pcl/segmentation/sac_segmentation.h>
00052 
00053 #include <pcl/recognition/linemod.h>
00054 #include <pcl/recognition/color_gradient_modality.h>
00055 #include <pcl/recognition/surface_normal_modality.h>
00056 
00057 using namespace pcl;
00058 using namespace pcl::io;
00059 using namespace pcl::console;
00060 
00061 typedef pcl::PointCloud<pcl::PointXYZRGBA> PointCloudXYZRGBA;
00062 
00063 void
00064 printHelp (int, char **argv)
00065 {
00066   print_error ("Syntax is: %s input1.pcd input2.pcd input3.pcd (etc.)\n", argv[0]);
00067   print_info ("  where options are:\n");
00068   print_info ("           -min_depth z_min   = the depth of the near clipping plane\n"); 
00069   print_info ("           -max_depth z_max   = the depth of the far clipping plane\n"); 
00070   print_info ("           -max_height y_max  = the height of the vertical clipping plane\n");
00071   print_info ("Two new template files will be created for each input file.  They will append ");
00072   print_info ("the following suffixes to the original filename:\n");
00073   print_info ("   _template.pcd (A PCD containing segmented points)\n");
00074   print_info ("   _template.sqmmt (A file storing LINEMOD's 'Sparse Quantized Multi-Modal Template' representation)\n");
00075 
00076 }
00077 
00078 void printElapsedTimeAndNumberOfPoints (double t, int w, int h=1)
00079 {
00080   print_info ("[done, "); print_value ("%g", t); print_info (" ms : "); 
00081   print_value ("%d", w*h); print_info (" points]\n");
00082 }
00083 
00084 bool
00085 loadCloud (const std::string & filename, PointCloudXYZRGBA & cloud)
00086 {
00087   TicToc tt;
00088   print_highlight ("Loading "); print_value ("%s ", filename.c_str ());
00089 
00090   tt.tic ();
00091   if (loadPCDFile (filename, cloud) < 0)
00092     return (false);
00093 
00094   printElapsedTimeAndNumberOfPoints (tt.toc (), cloud.width, cloud.height);
00095 
00096   print_info ("Available dimensions: "); print_value ("%s\n", pcl::getFieldsList (cloud).c_str ());
00097 
00098   return (true);
00099 }
00100 
00101 
00102 std::vector<bool>
00103 maskForegroundPoints (const PointCloudXYZRGBA::ConstPtr & input,
00104                       float min_depth, float max_depth, float max_height)
00105 {
00106   std::vector<bool> foreground_mask (input->size (), false);
00107   
00108   // Mask off points outside the specified near and far depth thresholds
00109   pcl::IndicesPtr indices (new std::vector<int>);
00110   for (size_t i = 0; i < input->size (); ++i)
00111   {
00112     const float z = input->points[i].z;
00113     if (min_depth < z && z < max_depth)
00114     {
00115       foreground_mask[i] = true;
00116       indices->push_back (static_cast<int> (i));
00117     }
00118   }
00119 
00120   // Find the dominant plane between the specified near/far thresholds
00121   const float distance_threshold = 0.02f;
00122   const int max_iterations = 500;
00123   pcl::SACSegmentation<pcl::PointXYZRGBA> seg;
00124   seg.setOptimizeCoefficients (true);
00125   seg.setModelType (pcl::SACMODEL_PLANE);
00126   seg.setMethodType (pcl::SAC_RANSAC);
00127   seg.setDistanceThreshold (distance_threshold);
00128   seg.setMaxIterations (max_iterations);
00129   seg.setInputCloud (input);
00130   seg.setIndices (indices);
00131   pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
00132   pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
00133   seg.segment (*inliers, *coefficients);  
00134   
00135   // Mask off the plane inliers
00136   for (size_t i = 0; i < inliers->indices.size (); ++i)
00137     foreground_mask[inliers->indices[i]] = false;
00138 
00139   // Mask off any foreground points that are too high above the detected plane
00140   const std::vector<float> & c = coefficients->values;
00141   for (size_t i = 0; i < input->size (); ++i)
00142   {
00143     if (foreground_mask[i])
00144     {
00145       const pcl::PointXYZRGBA & p = input->points[i];
00146       float d = fabsf (c[0]*p.x + c[1]*p.y + c[2]*p.z + c[3]);
00147       foreground_mask[i] = (d < max_height);
00148     }
00149   }
00150 
00151   return (foreground_mask);
00152 }
00153 
00154 void
00155 trainTemplate (const PointCloudXYZRGBA::ConstPtr & input, const std::vector<bool> &foreground_mask, 
00156                pcl::LINEMOD & linemod)
00157 {
00158   pcl::ColorGradientModality<pcl::PointXYZRGBA> color_grad_mod;
00159   color_grad_mod.setInputCloud (input);
00160   color_grad_mod.processInputData ();
00161   
00162   pcl::SurfaceNormalModality<pcl::PointXYZRGBA> surface_norm_mod;
00163   surface_norm_mod.setInputCloud (input);
00164   surface_norm_mod.processInputData ();
00165 
00166   std::vector<pcl::QuantizableModality*> modalities (2);
00167   modalities[0] = &color_grad_mod;
00168   modalities[1] = &surface_norm_mod;
00169 
00170   size_t min_x (input->width), min_y (input->height), max_x (0), max_y (0);
00171   pcl::MaskMap mask_map (input->width, input->height);
00172   for (size_t j = 0; j < input->height; ++j)
00173   {
00174     for (size_t i = 0; i < input->width; ++i)
00175     {
00176       mask_map (i,j) = foreground_mask[j*input->width+i];
00177       if (foreground_mask[j*input->width+i])
00178       {
00179         min_x = std::min (min_x, i);
00180         max_x = std::max (max_x, i);
00181         min_y = std::min (min_y, j);
00182         max_y = std::max (max_y, j);
00183       }
00184     }
00185   }
00186 
00187   std::vector<pcl::MaskMap*> masks (2);
00188   masks[0] = &mask_map;
00189   masks[1] = &mask_map;
00190 
00191   pcl::RegionXY region;
00192   region.x = static_cast<int> (min_x);
00193   region.y = static_cast<int> (min_y);
00194   region.width = static_cast<int> (max_x - min_x + 1);
00195   region.height = static_cast<int> (max_y - min_y + 1);
00196 
00197   printf ("%d %d %d %d\n", region.x, region.y, region.width, region.height);
00198 
00199   linemod.createAndAddTemplate (modalities, masks, region);
00200 }
00201 
00202 void
00203 compute (const PointCloudXYZRGBA::ConstPtr & input, float min_depth, float max_depth, float max_height,
00204          const std::string & template_pcd_filename, const std::string & template_sqmmt_filename)
00205 {
00206   // Segment the foreground object
00207   std::vector<bool> foreground_mask = maskForegroundPoints (input, min_depth, max_depth, max_height);
00208 
00209   // Save the masked template cloud (masking with NaNs to preserve its organized structure)
00210   PointCloudXYZRGBA template_cloud (*input);
00211   for (size_t i = 0; i < foreground_mask.size (); ++i)
00212   {
00213     if (!foreground_mask[i])
00214     {
00215       pcl::PointXYZRGBA & p = template_cloud.points[i];
00216       p.x = p.y = p.z = std::numeric_limits<float>::quiet_NaN ();
00217     }
00218   }
00219   pcl::io::savePCDFile (template_pcd_filename, template_cloud);
00220 
00221   // Create a LINEMOD template
00222   pcl::LINEMOD linemod;
00223   trainTemplate (input, foreground_mask, linemod);
00224 
00225   // Save the LINEMOD template
00226   std::ofstream file_stream;
00227   file_stream.open (template_sqmmt_filename.c_str (), std::ofstream::out | std::ofstream::binary);
00228   linemod.getTemplate (0).serialize (file_stream);
00229   file_stream.close ();
00230 }
00231 
00232 /* ---[ */
00233 int
00234 main (int argc, char** argv)
00235 {
00236   print_info ("Train one or more linemod templates. For more information, use: %s -h\n", argv[0]);
00237 
00238   // If no arguments are given, print the help text
00239   if (argc == 1)
00240   {
00241     printHelp (argc, argv);
00242     return (-1);
00243   }
00244 
00245   // Parse the command line arguments for .pcd files
00246   std::vector<int> p_file_indices;
00247   p_file_indices = parse_file_extension_argument (argc, argv, ".pcd");
00248   if (p_file_indices.empty ())
00249   {
00250     print_error ("Need at least one input PCD file.\n");
00251     return (-1);
00252   }
00253 
00254   // Parse the min_depth, max_depth, and max_height parameters
00255   float min_depth = 0;
00256   parse_argument (argc, argv, "-min_depth", min_depth);
00257 
00258   float max_depth = std::numeric_limits<float>::max ();
00259   parse_argument (argc, argv, "-max_depth", max_depth);
00260 
00261   float max_height = std::numeric_limits<float>::max ();
00262   parse_argument (argc, argv, "-max_height", max_height);
00263 
00264   // Segment and create templates for each input file
00265   for (size_t i_file = 0; i_file < p_file_indices.size (); ++i_file)
00266   {
00267     // Load input file
00268     const std::string input_filename = argv[p_file_indices[i_file]];
00269     PointCloudXYZRGBA::Ptr cloud (new PointCloudXYZRGBA);
00270     if (!loadCloud (input_filename, *cloud)) 
00271       return (-1);
00272 
00273     // Construct output filenames
00274     std::string sqmmt_filename = input_filename;
00275     sqmmt_filename.replace(sqmmt_filename.length () - 4, 13, "_template.sqmmt");
00276 
00277     std::string pcd_filename = input_filename;
00278     pcd_filename.replace(pcd_filename.length () - 4, 13, "_template.pcd");
00279 
00280     std::cout << sqmmt_filename << std::endl;
00281     std::cout << pcd_filename << std::endl;
00282 
00283     // Train the LINE-MOD template and output it to the specified file
00284     compute (cloud, min_depth, max_depth, max_height, pcd_filename, sqmmt_filename);
00285   }
00286 
00287 }
00288 


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:36:44