mesh_sampling.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2010-2011, Willow Garage, Inc.
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of Willow Garage, Inc. nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  */
00037 
00038 #include <pcl/visualization/pcl_visualizer.h>
00039 #include <pcl/io/pcd_io.h>
00040 #include <pcl/common/transforms.h>
00041 #include <vtkPLYReader.h>
00042 #include <vtkOBJReader.h>
00043 #include <vtkTriangle.h>
00044 #include <vtkTriangleFilter.h>
00045 #include <vtkPolyDataMapper.h>
00046 #include <pcl/filters/voxel_grid.h>
00047 #include <pcl/console/print.h>
00048 #include <pcl/console/parse.h>
00049 
00050 inline double
00051 uniform_deviate (int seed)
00052 {
00053   double ran = seed * (1.0 / (RAND_MAX + 1.0));
00054   return ran;
00055 }
00056 
00057 inline void
00058 randomPointTriangle (float a1, float a2, float a3, float b1, float b2, float b3, float c1, float c2, float c3,
00059                      Eigen::Vector4f& p)
00060 {
00061   float r1 = static_cast<float> (uniform_deviate (rand ()));
00062   float r2 = static_cast<float> (uniform_deviate (rand ()));
00063   float r1sqr = sqrtf (r1);
00064   float OneMinR1Sqr = (1 - r1sqr);
00065   float OneMinR2 = (1 - r2);
00066   a1 *= OneMinR1Sqr;
00067   a2 *= OneMinR1Sqr;
00068   a3 *= OneMinR1Sqr;
00069   b1 *= OneMinR2;
00070   b2 *= OneMinR2;
00071   b3 *= OneMinR2;
00072   c1 = r1sqr * (r2 * c1 + b1) + a1;
00073   c2 = r1sqr * (r2 * c2 + b2) + a2;
00074   c3 = r1sqr * (r2 * c3 + b3) + a3;
00075   p[0] = c1;
00076   p[1] = c2;
00077   p[2] = c3;
00078   p[3] = 0;
00079 }
00080 
00081 inline void
00082 randPSurface (vtkPolyData * polydata, std::vector<double> * cumulativeAreas, double totalArea, Eigen::Vector4f& p)
00083 {
00084   float r = static_cast<float> (uniform_deviate (rand ()) * totalArea);
00085 
00086   std::vector<double>::iterator low = std::lower_bound (cumulativeAreas->begin (), cumulativeAreas->end (), r);
00087   vtkIdType el = vtkIdType (low - cumulativeAreas->begin ());
00088 
00089   double A[3], B[3], C[3];
00090   vtkIdType npts = 0;
00091   vtkIdType *ptIds = NULL;
00092   polydata->GetCellPoints (el, npts, ptIds);
00093   polydata->GetPoint (ptIds[0], A);
00094   polydata->GetPoint (ptIds[1], B);
00095   polydata->GetPoint (ptIds[2], C);
00096   randomPointTriangle (float (A[0]), float (A[1]), float (A[2]), 
00097                        float (B[0]), float (B[1]), float (B[2]), 
00098                        float (C[0]), float (C[1]), float (C[2]), p);
00099 }
00100 
00101 void
00102 uniform_sampling (vtkSmartPointer<vtkPolyData> polydata, size_t n_samples, pcl::PointCloud<pcl::PointXYZ> & cloud_out)
00103 {
00104   polydata->BuildCells ();
00105   vtkSmartPointer<vtkCellArray> cells = polydata->GetPolys ();
00106 
00107   double p1[3], p2[3], p3[3], totalArea = 0;
00108   std::vector<double> cumulativeAreas (cells->GetNumberOfCells (), 0);
00109   size_t i = 0;
00110   vtkIdType npts = 0, *ptIds = NULL;
00111   for (cells->InitTraversal (); cells->GetNextCell (npts, ptIds); i++)
00112   {
00113     polydata->GetPoint (ptIds[0], p1);
00114     polydata->GetPoint (ptIds[1], p2);
00115     polydata->GetPoint (ptIds[2], p3);
00116     totalArea += vtkTriangle::TriangleArea (p1, p2, p3);
00117     cumulativeAreas[i] = totalArea;
00118   }
00119 
00120   cloud_out.points.resize (n_samples);
00121   cloud_out.width = static_cast<uint32_t> (n_samples);
00122   cloud_out.height = 1;
00123 
00124   for (i = 0; i < n_samples; i++)
00125   {
00126     Eigen::Vector4f p;
00127     randPSurface (polydata, &cumulativeAreas, totalArea, p);
00128     cloud_out.points[i].x = p[0];
00129     cloud_out.points[i].y = p[1];
00130     cloud_out.points[i].z = p[2];
00131   }
00132 }
00133 
00134 using namespace pcl;
00135 using namespace pcl::io;
00136 using namespace pcl::console;
00137 
00138 int default_number_samples = 100000;
00139 float default_leaf_size = 0.01f;
00140 
00141 void
00142 printHelp (int, char **argv)
00143 {
00144   print_error ("Syntax is: %s input.{ply,obj} output.pcd <options>\n", argv[0]);
00145   print_info ("  where options are:\n");
00146   print_info ("                     -n_samples X      = number of samples (default: ");
00147   print_value ("%d", default_number_samples);
00148   print_info (")\n");
00149   print_info (
00150               "                     -leaf_size X  = the XYZ leaf size for the VoxelGrid -- for data reduction (default: ");
00151   print_value ("%f", default_leaf_size);
00152   print_info (" m)\n");
00153 }
00154 
00155 /* ---[ */
00156 int
00157 main (int argc, char **argv)
00158 {
00159   print_info ("Convert a CAD model to a point cloud using uniform sampling. For more information, use: %s -h\n",
00160               argv[0]);
00161 
00162   if (argc < 3)
00163   {
00164     printHelp (argc, argv);
00165     return (-1);
00166   }
00167 
00168   // Parse command line arguments
00169   int SAMPLE_POINTS_ = default_number_samples;
00170   parse_argument (argc, argv, "-n_samples", SAMPLE_POINTS_);
00171   float leaf_size = default_leaf_size;
00172   parse_argument (argc, argv, "-leaf_size", leaf_size);
00173 
00174   // Parse the command line arguments for .ply and PCD files
00175   std::vector<int> pcd_file_indices = parse_file_extension_argument (argc, argv, ".pcd");
00176   if (pcd_file_indices.size () != 1)
00177   {
00178     print_error ("Need a single output PCD file to continue.\n");
00179     return (-1);
00180   }
00181   std::vector<int> ply_file_indices = parse_file_extension_argument (argc, argv, ".ply");
00182   std::vector<int> obj_file_indices = parse_file_extension_argument (argc, argv, ".obj");
00183   if (ply_file_indices.size () != 1 && obj_file_indices.size () != 1)
00184   {
00185     print_error ("Need a single input PLY/OBJ file to continue.\n");
00186     return (-1);
00187   }
00188 
00189   vtkSmartPointer<vtkPolyData> polydata1;
00190   if (ply_file_indices.size () == 1)
00191   {
00192     vtkSmartPointer<vtkPLYReader> readerQuery = vtkSmartPointer<vtkPLYReader>::New ();
00193     readerQuery->SetFileName (argv[ply_file_indices[0]]);
00194   }
00195   else if (obj_file_indices.size () == 1)
00196   {
00197     vtkSmartPointer<vtkOBJReader> readerQuery = vtkSmartPointer<vtkOBJReader>::New ();
00198     readerQuery->SetFileName (argv[obj_file_indices[0]]);
00199     polydata1 = readerQuery->GetOutput ();
00200     polydata1->Update ();
00201   }
00202 
00203   //make sure that the polygons are triangles!
00204   vtkSmartPointer<vtkTriangleFilter> triangleFilter = vtkSmartPointer<vtkTriangleFilter>::New ();
00205   triangleFilter->SetInput (polydata1);
00206   triangleFilter->Update ();
00207 
00208   vtkSmartPointer<vtkPolyDataMapper> triangleMapper = vtkSmartPointer<vtkPolyDataMapper>::New ();
00209   triangleMapper->SetInputConnection (triangleFilter->GetOutputPort ());
00210   triangleMapper->Update();
00211   polydata1 = triangleMapper->GetInput();
00212   polydata1->Update ();
00213 
00214   bool INTER_VIS = false;
00215   bool VIS = true;
00216 
00217   if (INTER_VIS)
00218   {
00219     visualization::PCLVisualizer vis;
00220     vis.addModelFromPolyData (polydata1, "mesh1", 0);
00221     vis.setRepresentationToSurfaceForAllActors ();
00222     vis.spin();
00223   }
00224 
00225   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_1 (new pcl::PointCloud<pcl::PointXYZ>);
00226   uniform_sampling (polydata1, SAMPLE_POINTS_, *cloud_1);
00227 
00228   if (INTER_VIS)
00229   {
00230     visualization::PCLVisualizer vis_sampled;
00231     vis_sampled.addPointCloud (cloud_1);
00232     vis_sampled.spin ();
00233   }
00234 
00235   // Voxelgrid
00236   VoxelGrid<PointXYZ> grid_;
00237   grid_.setInputCloud (cloud_1);
00238   grid_.setLeafSize (leaf_size, leaf_size, leaf_size);
00239   grid_.filter (*cloud_1);
00240 
00241   if (VIS)
00242   {
00243     visualization::PCLVisualizer vis3 ("VOXELIZED SAMPLES CLOUD");
00244     vis3.addPointCloud (cloud_1);
00245     vis3.spin ();
00246   }
00247 
00248   savePCDFileASCII (argv[pcd_file_indices[0]], *cloud_1);
00249 }


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:15:39