scan.cpp
Go to the documentation of this file.
00001 /*
00002  * scan.cpp
00003  *
00004  *  Created on: Apr 17, 2012
00005  *      Author: vsu
00006  */
00007 
00008 #include <pcl17/console/print.h>
00009 #include <iostream>
00010 #include <boost/filesystem.hpp>
00011 #include <boost/random.hpp>
00012 #include <pcl17/point_types.h>
00013 #include <pcl17/point_cloud.h>
00014 #include <pcl17/console/parse.h>
00015 #include <pcl17/io/vtk_lib_io.h>
00016 #include <pcl17/io/pcd_io.h>
00017 #include <pcl17/common/transforms.h>
00018 #include <pcl17/visualization/pcl_visualizer.h>
00019 #include <pcl17/filters/voxel_grid.h>
00020 
00021 #include <pcl17/sample_consensus/ransac.h>
00022 #include <pcl17/search/kdtree.h>
00023 #include <pcl17/surface/mls.h>
00024 
00025 #include <vtkTriangle.h>
00026 #include <vtkTriangleFilter.h>
00027 #include <vtkPolyDataMapper.h>
00028 
00029 void addNoise(pcl17::PointCloud<pcl17::PointXYZ>::Ptr cloud, double noise_std)
00030 {
00031 
00032   boost::mt19937 rng(static_cast<unsigned int> (std::time(0)));
00033   boost::normal_distribution<float> normal_distrib(0.0f, noise_std * noise_std);
00034   boost::variate_generator<boost::mt19937&, boost::normal_distribution<float> > gaussian_rng(rng, normal_distrib);
00035 
00036   for (size_t cp = 0; cp < cloud->points.size(); cp++)
00037   {
00038     cloud->points[cp].z += gaussian_rng();
00039   }
00040 
00041 }
00042 
00043 void moveToNewCenterAndAlign(pcl17::PointCloud<pcl17::PointXYZ>::Ptr cloud,
00044                              pcl17::PointCloud<pcl17::PointXYZ>::Ptr cloud_transformed, double new_center[3],
00045                              double tilt_angle)
00046 {
00047 
00048   Eigen::Affine3f view_transform;
00049   view_transform.setIdentity();
00050   Eigen::Translation<float, 3> translation(new_center[1], new_center[2], -new_center[0]);
00051   Eigen::AngleAxis<float> tilt_rotation(tilt_angle * M_PI / 180 + M_PI, Eigen::Vector3f(1, 0, 0));
00052   view_transform *= tilt_rotation;
00053   view_transform *= translation;
00054 
00055 
00056   Eigen::Affine3f change_coords;
00057   change_coords.matrix() << 0, 0, -1, 0, -1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 1;
00058 
00059   view_transform.matrix() = change_coords.matrix() * view_transform.matrix();
00060 
00061   pcl17::transformPointCloud(*cloud, *cloud_transformed, view_transform);
00062 
00063   cloud_transformed->sensor_origin_ = view_transform *  Eigen::Vector4f(0, 0, 0, 1);
00064   cloud_transformed->sensor_orientation_ = view_transform.rotate(Eigen::AngleAxisf(M_PI, Eigen::Vector3f(0, 0, 1))).rotation();
00065 
00066 }
00067 
00068 // Code from mesh_sampling.cpp
00069 inline double uniform_deviate(int seed)
00070 {
00071   double ran = seed * (1.0 / (RAND_MAX + 1.0));
00072   return ran;
00073 }
00074 
00075 inline void randomPointTriangle(float a1, float a2, float a3, float b1, float b2, float b3, float c1, float c2,
00076                                 float c3, Eigen::Vector4f& p)
00077 {
00078   float r1 = static_cast<float> (uniform_deviate(rand()));
00079   float r2 = static_cast<float> (uniform_deviate(rand()));
00080   float r1sqr = sqrtf(r1);
00081   float OneMinR1Sqr = (1 - r1sqr);
00082   float OneMinR2 = (1 - r2);
00083   a1 *= OneMinR1Sqr;
00084   a2 *= OneMinR1Sqr;
00085   a3 *= OneMinR1Sqr;
00086   b1 *= OneMinR2;
00087   b2 *= OneMinR2;
00088   b3 *= OneMinR2;
00089   c1 = r1sqr * (r2 * c1 + b1) + a1;
00090   c2 = r1sqr * (r2 * c2 + b2) + a2;
00091   c3 = r1sqr * (r2 * c3 + b3) + a3;
00092   p[0] = c1;
00093   p[1] = c2;
00094   p[2] = c3;
00095   p[3] = 0;
00096 }
00097 
00098 inline void randPSurface(vtkPolyData * polydata, std::vector<double> * cumulativeAreas, double totalArea,
00099                          Eigen::Vector4f& p)
00100 {
00101   float r = static_cast<float> (uniform_deviate(rand()) * totalArea);
00102 
00103   std::vector<double>::iterator low = std::lower_bound(cumulativeAreas->begin(), cumulativeAreas->end(), r);
00104   vtkIdType el = vtkIdType(low - cumulativeAreas->begin());
00105 
00106   double A[3], B[3], C[3];
00107   vtkIdType npts = 0;
00108   vtkIdType *ptIds = NULL;
00109   polydata->GetCellPoints(el, npts, ptIds);
00110   polydata->GetPoint(ptIds[0], A);
00111   polydata->GetPoint(ptIds[1], B);
00112   polydata->GetPoint(ptIds[2], C);
00113   randomPointTriangle(float(A[0]), float(A[1]), float(A[2]), float(B[0]), float(B[1]), float(B[2]), float(C[0]),
00114                       float(C[1]), float(C[2]), p);
00115 }
00116 
00117 void uniform_sampling(vtkSmartPointer<vtkPolyData> polydata, size_t n_samples,
00118                       pcl17::PointCloud<pcl17::PointXYZ> & cloud_out)
00119 {
00120   polydata->BuildCells();
00121   vtkSmartPointer<vtkCellArray> cells = polydata->GetPolys();
00122 
00123   std::cerr << "Number of cells " << cells->GetNumberOfCells() << std::endl;
00124 
00125   double p1[3], p2[3], p3[3], totalArea = 0;
00126   std::vector<double> cumulativeAreas(cells->GetNumberOfCells(), 0);
00127   size_t i = 0;
00128   vtkIdType npts = 0, *ptIds = NULL;
00129   for (cells->InitTraversal(); cells->GetNextCell(npts, ptIds); i++)
00130   {
00131     polydata->GetPoint(ptIds[0], p1);
00132     polydata->GetPoint(ptIds[1], p2);
00133     polydata->GetPoint(ptIds[2], p3);
00134     totalArea += vtkTriangle::TriangleArea(p1, p2, p3);
00135     cumulativeAreas[i] = totalArea;
00136   }
00137 
00138   cloud_out.points.resize(n_samples);
00139   cloud_out.width = static_cast<uint32_t> (n_samples);
00140   cloud_out.height = 1;
00141 
00142   for (i = 0; i < n_samples; i++)
00143   {
00144     Eigen::Vector4f p;
00145     randPSurface(polydata, &cumulativeAreas, totalArea, p);
00146     cloud_out.points[i].x = p[0];
00147     cloud_out.points[i].y = p[1];
00148     cloud_out.points[i].z = p[2];
00149   }
00150 }
00151 // End of code from mesh_sampling.cpp
00152 
00153 void createFullModelPointcloud(vtkSmartPointer<vtkPolyData> polydata, size_t n_samples,
00154                                pcl17::PointCloud<pcl17::PointXYZ> & cloud_out)
00155 {
00156 
00157   vtkSmartPointer<vtkTriangleFilter> triangleFilter = vtkSmartPointer<vtkTriangleFilter>::New();
00158   triangleFilter->SetInput(polydata);
00159   triangleFilter->Update();
00160 
00161   vtkSmartPointer<vtkPolyDataMapper> triangleMapper = vtkSmartPointer<vtkPolyDataMapper>::New();
00162   triangleMapper->SetInputConnection(triangleFilter->GetOutputPort());
00163   triangleMapper->Update();
00164   polydata = triangleMapper->GetInput();
00165   polydata->Update();
00166 
00167   uniform_sampling(polydata, n_samples, cloud_out);
00168 
00169 }
00170 
00171 int main(int argc, char** argv)
00172 {
00173 
00174   if (argc < 5)
00175   {
00176     PCL17_INFO ("Usage %s -input_dir /dir/with/models -output_dir /output/dir [options]\n", argv[0]);
00177     PCL17_INFO (" * where options are:\n"
00178         "         -height <X>            : simulate scans with sensor mounted on height X\n"
00179         "         -noise_std <X>         : std of gaussian noise added to pointcloud. Default value 0.0001.\n"
00180         "         -distance <X>          : simulate scans with object located on a distance X. Can be used multiple times. Default value 4.\n"
00181         "         -tilt <X>              : tilt sensor for X degrees. X == 0 - sensor looks strait. X < 0 Sensor looks down. X > 0 Sensor looks up . Can be used multiple times. Default value -15.\n"
00182         "         -shift <X>             : shift object from the straight line. Can be used multiple times. Default value 0.\n"
00183         "         -num_views <X>         : how many times rotate the object in for every distance, tilt and shift. Default value 6.\n"
00184 
00185         "");
00186     return -1;
00187   }
00188 
00189   std::string input_dir, output_dir;
00190   double height = 1.5;
00191   double num_views = 6;
00192   double noise_std = 0.0001;
00193   std::vector<double> distances;
00194   std::vector<double> tilt;
00195   std::vector<double> shift;
00196   int full_model_n_points = 30000;
00197 
00198   pcl17::console::parse_argument(argc, argv, "-input_dir", input_dir);
00199   pcl17::console::parse_argument(argc, argv, "-output_dir", output_dir);
00200   pcl17::console::parse_argument(argc, argv, "-num_views", num_views);
00201   pcl17::console::parse_argument(argc, argv, "-height", height);
00202   pcl17::console::parse_argument(argc, argv, "-noise_std", noise_std);
00203   pcl17::console::parse_multiple_arguments(argc, argv, "-distance", distances);
00204   pcl17::console::parse_multiple_arguments(argc, argv, "-tilt", tilt);
00205   pcl17::console::parse_multiple_arguments(argc, argv, "-shift", shift);
00206 
00207   PCL17_INFO("distances size: %d\n", distances.size());
00208   for (size_t i = 0; i < distances.size(); i++)
00209   {
00210     PCL17_INFO("distance: %f\n", distances[i]);
00211   }
00212 
00213   // Set default values if user didn't provide any
00214   if (distances.size() == 0)
00215     distances.push_back(4);
00216   if (tilt.size() == 0)
00217     tilt.push_back(-15);
00218   if (shift.size() == 0)
00219     shift.push_back(0);
00220 
00221   // Check if input path exists
00222   boost::filesystem::path input_path(input_dir);
00223   if (!boost::filesystem::exists(input_path))
00224   {
00225     PCL17_ERROR("Input directory doesnt exists.");
00226     return -1;
00227   }
00228 
00229   // Check if input path is a directory
00230   if (!boost::filesystem::is_directory(input_path))
00231   {
00232     PCL17_ERROR("%s is not directory.", input_path.c_str());
00233     return -1;
00234   }
00235 
00236   // Check if output directory exists
00237   boost::filesystem::path output_path(output_dir);
00238   if (!boost::filesystem::exists(output_path) || !boost::filesystem::is_directory(output_path))
00239   {
00240     if (!boost::filesystem::create_directories(output_path))
00241     {
00242       PCL17_ERROR ("Error creating directory %s.\n", output_path.c_str ());
00243       return -1;
00244     }
00245   }
00246 
00247   // Find all .vtk files in the input directory
00248   std::vector<std::string> files_to_process;
00249   PCL17_INFO("Processing following files:\n");
00250   boost::filesystem::directory_iterator end_iter;
00251   for (boost::filesystem::directory_iterator iter(input_path); iter != end_iter; iter++)
00252   {
00253     boost::filesystem::path class_dir_path(*iter);
00254     for (boost::filesystem::directory_iterator iter2(class_dir_path); iter2 != end_iter; iter2++)
00255     {
00256       boost::filesystem::path file(*iter2);
00257       if (file.extension() == ".vtk")
00258       {
00259         files_to_process.push_back(file.c_str());
00260         PCL17_INFO("\t%s\n", file.c_str());
00261       }
00262     }
00263 
00264   }
00265 
00266   // Check if there are any .vtk files to process
00267   if (files_to_process.size() == 0)
00268   {
00269     PCL17_ERROR("Directory %s has no .vtk files.", input_path.c_str());
00270     return -1;
00271   }
00272 
00273   // Iterate over all files
00274   for (size_t i = 0; i < files_to_process.size(); i++)
00275   {
00276     vtkSmartPointer<vtkPolyData> model;
00277     vtkSmartPointer<vtkPolyDataReader> reader = vtkPolyDataReader::New();
00278     vtkSmartPointer<vtkTransform> transform = vtkTransform::New();
00279     pcl17::PointCloud<pcl17::PointXYZ>::Ptr cloud(new pcl17::PointCloud<pcl17::PointXYZ>());
00280 
00281     // Compute output directory for this model
00282     std::vector<std::string> st;
00283     boost::split(st, files_to_process.at(i), boost::is_any_of("/"), boost::token_compress_on);
00284     std::string class_dirname = st.at(st.size() - 2);
00285     std::string dirname = st.at(st.size() - 1);
00286     dirname = dirname.substr(0, dirname.size() - 4);
00287     dirname = output_dir + class_dirname + "/" + dirname;
00288 
00289     // Check if output directory for this model exists. If not create
00290     boost::filesystem::path dirpath(dirname);
00291     if (!boost::filesystem::exists(dirpath))
00292     {
00293       if (!boost::filesystem::create_directories(dirpath))
00294       {
00295         PCL17_ERROR ("Error creating directory %s.\n", dirpath.c_str ());
00296         return -1;
00297       }
00298     }
00299 
00300     // Load model from file
00301     reader->SetFileName(files_to_process.at(i).c_str());
00302     reader->Update();
00303     model = reader->GetOutput();
00304     PCL17_INFO("Number of points %d\n",model->GetNumberOfPoints());
00305 
00306     // Coumpute bounds and center of the model
00307     double bounds[6];
00308     model->GetBounds(bounds);
00309     double min_z_value = bounds[4];
00310 
00311     double center[3];
00312     model->GetCenter(center);
00313 
00314     createFullModelPointcloud(model, full_model_n_points, *cloud);
00315     pcl17::io::savePCDFile(dirname + "/full.pcd", *cloud);
00316 
00317     // Initialize PCLVisualizer. Add model to scene.
00318     pcl17::visualization::PCLVisualizer viz;
00319     viz.initCameraParameters();
00320     viz.updateCamera();
00321     viz.setCameraPosition(0, 0, 0, 1, 0, 0, 0, 0, 1);
00322     viz.addModelFromPolyData(model, transform);
00323     viz.setRepresentationToSurfaceForAllActors();
00324 
00325     // Iterate over all shifts
00326     for (size_t shift_index = 0; shift_index < shift.size(); shift_index++)
00327     {
00328 
00329       // Iterate over all tilts
00330       for (size_t tilt_index = 0; tilt_index < tilt.size(); tilt_index++)
00331       {
00332 
00333         // Iterate over all distances
00334         for (size_t distance_index = 0; distance_index < distances.size(); distance_index++)
00335         {
00336 
00337           // Iterate over all angles
00338           double angle = 0;
00339           double angle_step = 360.0 / num_views;
00340           for (int angle_index = 0; angle_index < num_views; angle_index++)
00341           {
00342 
00343             // Set transformation with distance, shift, tilt and angle parameters.
00344             transform->Identity();
00345             transform->RotateY(tilt[tilt_index]);
00346             transform->Translate(distances[distance_index], shift[shift_index], -(height + min_z_value));
00347             transform->RotateZ(angle);
00348 
00349                 /*              
00350 
00351             // Render pointcloud
00352             viz.renderView(640, 480, cloud);
00353 
00354             //Add noise
00355             addNoise(cloud, noise_std);
00356 
00357             // Compute new coordinates of the model center
00358             double new_center[3];
00359             transform->TransformPoint(center, new_center);
00360 
00361             // Shift origin of the poincloud to the model center and align with initial coordinate system.
00362             pcl17::PointCloud<pcl17::PointXYZ>::Ptr cloud_transformed(new pcl17::PointCloud<pcl17::PointXYZ>());
00363             moveToNewCenterAndAlign(cloud, cloud_transformed, new_center, tilt[tilt_index]);
00364 
00365             // Compute file name for this pointcloud and save it
00366             std::stringstream ss;
00367             ss << dirname << "/rotation" << angle << "_distance" << distances[distance_index] << "_tilt"
00368                 << tilt[tilt_index] << "_shift" << shift[shift_index] << ".pcd";
00369             PCL17_INFO("Writing %d points to file %s\n", cloud->points.size(), ss.str().c_str());
00370             pcl17::io::savePCDFile(ss.str(), *cloud_transformed);
00371 
00372                 */
00373 
00374             // increment angle by step
00375             angle += angle_step;
00376         
00377         
00378 
00379           }
00380 
00381         }
00382       }
00383     }
00384 
00385   }
00386 
00387   return 0;
00388 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


furniture_classification
Author(s): Vladyslav Usenko
autogenerated on Sun Oct 6 2013 12:12:34