virtual_scanner.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2010, 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 Willow Garage, Inc. 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: virtual_scanner.cpp 36181 2010-04-27 16:08:14Z rusu $
00035  *
00036  */
00037 
00046 #include <string>
00047 
00048 // ROS messages
00049 #include <ros/ros.h>
00050 #include <boost/algorithm/string.hpp>
00051 #include <boost/filesystem.hpp>
00052 // PCL dependencies
00053 #include <pcl/ros/register_point_struct.h>
00054 #include <pcl/io/pcd_io.h>
00055 #include <pcl/filters/voxel_grid.h>
00056 #include <pcl/point_types.h>
00057 
00058 // ITPP dependencies (will go away after CVPR when we merge with trunk)
00059 #include <itpp/itbase.h>
00060 #include <itpp/itstat.h>
00061 #include <itpp/signal/sigfun.h>
00062 
00063 // VTK dependencies (will go away after CVPR when we merge with trunk)
00064 #include <vtkMath.h>
00065 #include <vtkGeneralTransform.h>
00066 #include <vtkPlatonicSolidSource.h>
00067 #include <vtkLoopSubdivisionFilter.h>
00068 #include <vtkPLYReader.h>
00069 #include <vtkSmartPointer.h>
00070 #include <vtkCellLocator.h>
00071 #include <vtkPolyData.h>
00072 
00073 //terminal_tools includes
00074 #include <terminal_tools/print.h>
00075 #include <terminal_tools/parse.h>
00076 
00077 //local includes
00078 #include "pcl_vtk_tools/misc.h"
00079 
00080 #include <vector>
00081 #define EPS 0.00001
00082 
00083 struct ScanParameters
00084 {
00085   int nr_scans;             // number of steps for sweep movement
00086   int nr_points_in_scans;   // number of laser beam measurements per scan
00087   double vert_res;          // vertical resolution (res. of sweep) in degrees
00088   double hor_res;           // horizontal  resolution (of laser beam) in degrees
00089   double max_dist;          // maximum distance in units.
00090 };
00091 
00093 
00097 vtkPolyData*
00098   loadPLYAsDataSet (const char* file_name)
00099 {
00100   vtkPLYReader* reader = vtkPLYReader::New ();
00101   reader->SetFileName (file_name);
00102   reader->Update ();
00103   return (reader->GetOutput ());
00104 }
00105 
00106 // struct PointXYZViewpoint;
00107 
00108 // POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZViewpoint,
00109 //                                    (float, x, x)
00110 //                                    (float, y, y)
00111 //                                    (float, z, z)
00112 //                                    (float, vx, vx)
00113 //                                    (float, vy, vy)
00114 //                                    (float, vz, vz)
00115 // );
00116 
00117 /* ---[ */
00118 int
00119   main (int argc, char** argv)
00120 {
00121   if (argc < 3)
00122     {
00123     ROS_INFO("Usage %s -single_view <0|1> -view_point <x,y,z> -target_point <x,y,z> <model.ply | model.vtk>", argv[0]);
00124     return -1;
00125     }
00126   std::string filename;
00127   // Parse the command line arguments for .vtk or .ply files
00128   std::vector<int> p_file_indices_vtk = terminal_tools::parse_file_extension_argument (argc, argv, ".vtk");
00129   std::vector<int> p_file_indices_ply = terminal_tools::parse_file_extension_argument (argc, argv, ".ply");
00130   bool object_coordinates = true;
00131   terminal_tools::parse_argument (argc, argv, "-object_coordinates", object_coordinates);
00132   bool single_view = false;
00133   terminal_tools::parse_argument (argc, argv, "-single_view", single_view);
00134   double vx = 0, vy = 0, vz = 0;
00135   terminal_tools::parse_3x_arguments (argc, argv, "-view_point", vx, vy, vz);
00136   double tx = 0, ty = 0, tz = 0;
00137   terminal_tools::parse_3x_arguments (argc, argv, "-target_point", tx, ty, tz);
00138   vtkSmartPointer<vtkPolyData> data;
00139   // Loading VTK file
00140   if (p_file_indices_vtk.size() != 0)
00141     {
00142      data = load_poly_data_as_data_set(argv[p_file_indices_vtk.at (0)]);
00143      std::stringstream filename_stream;
00144      filename_stream << argv[p_file_indices_vtk.at (0)];
00145      filename = filename_stream.str();
00146      ROS_INFO ("Loading vtk model with %d vertices/points.", (int)data->GetNumberOfPoints ());
00147     }
00148   //Loading PLY file
00149   else if (p_file_indices_ply.size() != 0)
00150     {
00151       std::stringstream filename_stream;
00152       filename_stream << argv[p_file_indices_ply.at (0)];
00153       filename = filename_stream.str();
00154       data = loadPLYAsDataSet (filename.c_str());
00155       ROS_INFO ("Loading ply model with %d vertices/points.", (int)data->GetNumberOfPoints ());
00156     }
00157   else
00158     {
00159       ROS_ERROR("Unrecognized file format");
00160       return -1;
00161     }
00162   // Default scan parameters
00163   ScanParameters sp;
00164   sp.nr_scans           = 900;
00165   sp.nr_points_in_scans = 900;
00166   sp.max_dist           = 30000;   // maximum distance (in mm)
00167   sp.vert_res           = 0.25;
00168   sp.hor_res            = 0.25;
00169 
00170   int noise_model = 0;              // set the default noise level to none
00171   double noise_std = 0.5;           // 0.5 standard deviations by default
00172 
00173   int subdiv_level = 1;
00174   double scan_dist = 3;
00175   std::string fname, base;
00176   char seq[256];
00177 
00178   // Compute start/stop for vertical and horizontal
00179   double vert_start = - ((double)(sp.nr_scans-1) / 2.0) * sp.vert_res;
00180   double vert_end   = + ((sp.nr_scans-1) * sp.vert_res) + vert_start;
00181   double hor_start  = - ((double)(sp.nr_points_in_scans-1) / 2.0) * sp.hor_res;
00182   double hor_end    = + ((sp.nr_points_in_scans-1) * sp.hor_res) + hor_start;
00183 
00184   // Prepare the point cloud data
00185   pcl::PointCloud<pcl::PointWithViewpoint> cloud;
00186 
00187   // Prepare the leaves for downsampling
00188   pcl::VoxelGrid<pcl::PointWithViewpoint> grid;
00189   grid.setLeafSize (2.5, 2.5, 2.5);    // @note: this value should be given in mm!
00190 
00191   // Reset and set a random seed for the Global Random Number Generator
00192   itpp::RNG_randomize ();
00193   // Create random noise distributions with mean <0> and standard deviation <std>
00194   itpp::Normal_RNG    n_rng   (0.0, noise_std*noise_std);
00195   itpp::Laplace_RNG   lap_rng (0.0, noise_std*noise_std);
00196 
00197   std::vector<std::string> st;
00198   // Virtual camera parameters
00199   double eye[3]     = {0.0, 0.0, 0.0};
00200   double viewray[3] = {0.0, 0.0, 0.0};
00201   double up[3]      = {0.0, 0.0, 0.0};
00202   double right[3]  = {0.0, 0.0, 0.0};
00203   double x_axis[3] = {1.0, 0.0, 0.0};
00204   double z_axis[3] = {0.0, 0.0, 1.0};
00205   double bounds[6];
00206   double temp_beam[3], beam[3], p[3];
00207   double p_coords[3], x[3], t;
00208   int subId;
00209  
00210   // Create a Icosahedron at center in origin and radius of 1
00211   vtkSmartPointer<vtkPlatonicSolidSource> icosa = vtkSmartPointer<vtkPlatonicSolidSource>::New ();
00212   icosa->SetSolidTypeToIcosahedron ();
00213 
00214   // Tesselize the source icosahedron (subdivide each triangular face
00215   // of the icosahedron into smaller triangles)
00216   vtkSmartPointer<vtkLoopSubdivisionFilter> subdivide = vtkSmartPointer<vtkLoopSubdivisionFilter>::New ();
00217   subdivide->SetNumberOfSubdivisions (subdiv_level);
00218   subdivide->SetInputConnection (icosa->GetOutputPort ());
00219 
00220   // Get camera positions
00221   vtkPolyData *sphere = subdivide->GetOutput ();
00222   sphere->Update ();
00223   if(!single_view)
00224     ROS_INFO ("Created %d camera position points.", (int)sphere->GetNumberOfPoints ());
00225 
00226   // Build a spatial locator for our dataset
00227   vtkSmartPointer<vtkCellLocator> tree = vtkSmartPointer<vtkCellLocator>::New ();
00228   tree->SetDataSet (data);
00229   tree->CacheCellBoundsOn ();
00230   tree->SetTolerance (0.0);
00231   tree->SetNumberOfCellsPerBucket (1);
00232   tree->AutomaticOn ();
00233   tree->BuildLocator ();
00234   tree->Update ();
00235 
00236   // Get the min-max bounds of data
00237   data->GetBounds (bounds);
00238 
00239   // if single view is required iterate over loop only once
00240   int number_of_points = sphere->GetNumberOfPoints ();
00241   if (single_view)
00242     number_of_points = 1;
00243 
00244   int sid = -1;
00245   for (int i = 0; i < number_of_points; i++)
00246   {
00247     sphere->GetPoint (i, eye);
00248     if (fabs(eye[0]) < EPS) eye[0] = 0;
00249     if (fabs(eye[1]) < EPS) eye[1] = 0;
00250     if (fabs(eye[2]) < EPS) eye[2] = 0;
00251 
00252     viewray[0] = -eye[0];
00253     viewray[1] = -eye[1];
00254     viewray[2] = -eye[2];
00255     eye[0] *= scan_dist;
00256     eye[1] *= scan_dist;
00257     eye[2] *= scan_dist;
00258     //Change here if only single view point is required
00259     if (single_view)
00260     {
00261       eye[0] = vx;//0.0;
00262       eye[1] = vy;//-0.26;
00263       eye[2] = vz;//-0.86;
00264       viewray[0] = tx - vx;
00265       viewray[1] = ty - vy;
00266       viewray[2] = tz - vz;
00267       double len = sqrt (viewray[0]*viewray[0] + viewray[1]*viewray[1] + viewray[2]*viewray[2]);
00268       if (len == 0)
00269       {
00270         ROS_ERROR ("The single_view option is enabled but the view_point and the target_point are the same!");
00271         break;
00272       }
00273       viewray[0] /= len;
00274       viewray[1] /= len;
00275       viewray[2] /= len;
00276     }
00277 
00278     if ((viewray[0] == 0) && (viewray[1] == 0))
00279       vtkMath::Cross (viewray, x_axis, right);
00280     else
00281       vtkMath::Cross (viewray, z_axis, right);
00282     if (fabs(right[0]) < EPS) right[0] = 0;
00283     if (fabs(right[1]) < EPS) right[1] = 0;
00284     if (fabs(right[2]) < EPS) right[2] = 0;
00285 
00286     vtkMath::Cross (viewray, right, up);
00287     if (fabs(up[0]) < EPS) up[0] = 0;
00288     if (fabs(up[1]) < EPS) up[1] = 0;
00289     if (fabs(up[2]) < EPS) up[2] = 0;
00290 
00291     if (!object_coordinates)
00292     {
00293       // Normalization
00294       double right_len = sqrt (right[0]*right[0] + right[1]*right[1] + right[2]*right[2]);
00295       right[0] /= right_len;
00296       right[1] /= right_len;
00297       right[2] /= right_len;
00298       double up_len = sqrt (up[0]*up[0] + up[1]*up[1] + up[2]*up[2]);
00299       up[0] /= up_len;
00300       up[1] /= up_len;
00301       up[2] /= up_len;
00302     
00303       // Output resulting vectors
00304       cerr << "Viewray Right Up:" << endl;
00305       cerr << viewray[0] << " " << viewray[1] << " " << viewray[2] << " " << endl;
00306       cerr << right[0] << " " << right[1] << " " << right[2] << " " << endl;
00307       cerr << up[0] << " " << up[1] << " " << up[2] << " " << endl;
00308     }
00309 
00310     // Create a transformation
00311     vtkGeneralTransform* tr1 = vtkGeneralTransform::New ();
00312     vtkGeneralTransform* tr2 = vtkGeneralTransform::New ();
00313 
00314     // right = viewray x up
00315     vtkMath::Cross (viewray, up, right);
00316 
00317     // Sweep vertically
00318     for (double vert = vert_start; vert <= vert_end; vert += sp.vert_res)
00319     {
00320       sid++;
00321 
00322       tr1->Identity ();
00323       tr1->RotateWXYZ (vert, right);
00324       tr1->InternalTransformPoint (viewray, temp_beam);
00325 
00326       // Sweep horizontally
00327       int pid = -1;
00328       for (double hor = hor_start; hor <= hor_end; hor += sp.hor_res)
00329       {
00330         pid ++;
00331       
00332         // Create a beam vector with (lat,long) angles (vert, hor) with the viewray
00333         tr2->Identity ();
00334         tr2->RotateWXYZ (hor, up);
00335         tr2->InternalTransformPoint (temp_beam, beam);
00336         vtkMath::Normalize (beam);
00337 
00338         // Find point at max range: p = eye + beam * max_dist
00339         for (int d = 0; d < 3; d++)
00340           p[d] = eye[d] + beam[d] * sp.max_dist;
00341 
00342         // Put p_coords into laser scan at packetid = vert, scan id = hor
00343         vtkIdType cellId;
00344         if (tree->IntersectWithLine (eye, p, 0, t, x, p_coords, subId, cellId))
00345         {
00346           pcl::PointWithViewpoint pt;
00347           if (object_coordinates)
00348           {
00349             pt.x = x[0]; pt.y = x[1]; pt.z = x[2];
00350             pt.vp_x = eye[0]; pt.vp_y = eye[1]; pt.vp_z = eye[2];
00351           }
00352           else
00353           {
00354             // z axis is the viewray
00355             // y axis is up
00356             // x axis is -right (negative because z*y=-x but viewray*up=right)
00357             pt.x = -right[0]*x[1] + up[0]*x[2] + viewray[0]*x[0] + eye[0];
00358             pt.y = -right[1]*x[1] + up[1]*x[2] + viewray[1]*x[0] + eye[1];
00359             pt.z = -right[2]*x[1] + up[2]*x[2] + viewray[2]*x[0] + eye[2];
00360             pt.vp_x = pt.vp_y = pt.vp_z = 0.0;
00361           }
00362           cloud.points.push_back(pt);
00363         }
00364       } // Horizontal
00365     } // Vertical
00366 
00367     // Noisify each point in the dataset
00368     // \note: we might decide to noisify along the ray later
00369     for (size_t cp = 0; cp < cloud.points.size (); ++cp)
00370     {
00371       // Add noise ?
00372       switch (noise_model)
00373       {
00374         // Gaussian
00375         case 1: { cloud.points[cp].x += n_rng (); cloud.points[cp].y += n_rng (); cloud.points[cp].z += n_rng (); break; }
00376         // Laplace
00377         case 2: { cloud.points[cp].x += lap_rng (); cloud.points[cp].y += lap_rng (); cloud.points[cp].z += lap_rng (); break; }
00378       }
00379     }
00380 
00381     // Downsample and remove silly po int duplicates
00382     pcl::PointCloud<pcl::PointWithViewpoint> cloud_downsampled;
00383     grid.setInputCloud (boost::make_shared<pcl::PointCloud<pcl::PointWithViewpoint> > (cloud));
00384     //grid.filter (cloud_downsampled);
00385 
00386     // Saves the point cloud data to disk
00387     sprintf (seq, "%d", (int)i);
00388     boost::trim (filename);
00389     boost::split (st, filename, boost::is_any_of ("/"), boost::token_compress_on);
00390 
00391     std::stringstream ss;
00392     std::string output_dir = st.at (st.size () - 1);
00393     boost::filesystem::path outpath (output_dir);
00394     if (!boost::filesystem::exists (outpath))
00395     {
00396       if (!boost::filesystem::create_directories (outpath))
00397       {
00398         ROS_ERROR ("Error creating directory %s.", output_dir.c_str ());
00399         return (-1);
00400       }
00401       ROS_INFO ("Creating directory %s", output_dir.c_str ());
00402     }
00403 
00404     fname = st.at (st.size () - 1) + seq + ".pcd";
00405     ROS_INFO ("Writing %d points to %s", (int)cloud.points.size (), fname.c_str ());
00406     pcl::io::savePCDFile (fname.c_str (), cloud);
00407   } // sphere
00408   return (0);
00409 }
00410 /* ]--- */
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


pcl_vtk_tools
Author(s): Nico Blodow, Zoltan-Csaba Marton, Dejan Pangercic
autogenerated on Thu May 23 2013 17:19:40