nearest_neighbors.cpp
Go to the documentation of this file.
00001 #include <pcl/point_types.h>
00002 #include <pcl/point_cloud.h>
00003 #include <pcl/common/common.h>
00004 #include <pcl/common/transforms.h>
00005 #include <pcl/visualization/pcl_visualizer.h>
00006 #include <pcl/console/parse.h>
00007 #include <pcl/console/print.h>
00008 #include <pcl/io/pcd_io.h>
00009 #include <iostream>
00010 #include <flann/flann.h>
00011 #include <flann/io/hdf5.h>
00012 #include <boost/filesystem.hpp>
00013 
00014 typedef std::pair<std::string, std::vector<float> > vfh_model;
00015 
00020 bool
00021 loadHist (const boost::filesystem::path &path, vfh_model &vfh)
00022 {
00023   int vfh_idx;
00024   // Load the file as a PCD
00025   try
00026   {
00027     sensor_msgs::PointCloud2 cloud;
00028     int version;
00029     Eigen::Vector4f origin;
00030     Eigen::Quaternionf orientation;
00031     pcl::PCDReader r;
00032     int type; unsigned int idx;
00033     r.readHeader (path.string (), cloud, origin, orientation, version, type, idx);
00034 
00035     vfh_idx = pcl::getFieldIndex (cloud, "vfh");
00036     if (vfh_idx == -1)
00037       return (false);
00038     if ((int)cloud.width * cloud.height != 1)
00039       return (false);
00040   }
00041   catch (pcl::InvalidConversionException e)
00042   {
00043     return (false);
00044   }
00045 
00046   // Treat the VFH signature as a single Point Cloud
00047   pcl::PointCloud <pcl::VFHSignature308> point;
00048   pcl::io::loadPCDFile (path.string (), point);
00049   vfh.second.resize (308);
00050 
00051   std::vector <sensor_msgs::PointField> fields;
00052   getFieldIndex (point, "vfh", fields);
00053 
00054   for (size_t i = 0; i < fields[vfh_idx].count; ++i)
00055   {
00056     vfh.second[i] = point.points[0].histogram[i];
00057   }
00058   vfh.first = path.string ();
00059   return (true);
00060 }
00061 
00062 
00070 inline void
00071 nearestKSearch (flann::Index<flann::ChiSquareDistance<float> > &index, const vfh_model &model, 
00072                 int k, flann::Matrix<int> &indices, flann::Matrix<float> &distances)
00073 {
00074   // Query point
00075   flann::Matrix<float> p = flann::Matrix<float>(new float[model.second.size ()], 1, model.second.size ());
00076   memcpy (&p.ptr ()[0], &model.second[0], p.cols * p.rows * sizeof (float));
00077 
00078   indices = flann::Matrix<int>(new int[k], 1, k);
00079   distances = flann::Matrix<float>(new float[k], 1, k);
00080   index.knnSearch (p, indices, distances, k, flann::SearchParams (512));
00081   delete[] p.ptr ();
00082 }
00083 
00088 bool
00089 loadFileList (std::vector<vfh_model> &models, const std::string &filename)
00090 {
00091   ifstream fs;
00092   fs.open (filename.c_str ());
00093   if (!fs.is_open () || fs.fail ())
00094     return (false);
00095 
00096   std::string line;
00097   while (!fs.eof ())
00098   {
00099     getline (fs, line);
00100     if (line.empty ())
00101       continue;
00102     vfh_model m;
00103     m.first = line;
00104     models.push_back (m);
00105   }
00106   fs.close ();
00107   return (true);
00108 }
00109 
00110 int
00111 main (int argc, char** argv)
00112 {
00113   int k = 6;
00114 
00115   double thresh = DBL_MAX;     // No threshold, disabled by default
00116 
00117   if (argc < 2)
00118   {
00119     pcl::console::print_error 
00120       ("Need at least three parameters! Syntax is: %s <query_vfh_model.pcd> [options] {kdtree.idx} {training_data.h5} {training_data.list}\n", argv[0]);
00121     pcl::console::print_info ("    where [options] are:  -k      = number of nearest neighbors to search for in the tree (default: "); 
00122     pcl::console::print_value ("%d", k); pcl::console::print_info (")\n");
00123     pcl::console::print_info ("                          -thresh = maximum distance threshold for a model to be considered VALID (default: "); 
00124     pcl::console::print_value ("%f", thresh); pcl::console::print_info (")\n\n");
00125     return (-1);
00126   }
00127 
00128   std::string extension (".pcd");
00129   transform (extension.begin (), extension.end (), extension.begin (), (int(*)(int))tolower);
00130 
00131   // Load the test histogram
00132   std::vector<int> pcd_indices = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
00133   vfh_model histogram;
00134   if (!loadHist (argv[pcd_indices.at (0)], histogram))
00135   {
00136     pcl::console::print_error ("Cannot load test file %s\n", argv[pcd_indices.at (0)]);
00137     return (-1);
00138   }
00139 
00140   pcl::console::parse_argument (argc, argv, "-thresh", thresh);
00141   // Search for the k closest matches
00142   pcl::console::parse_argument (argc, argv, "-k", k);
00143   pcl::console::print_highlight ("Using "); pcl::console::print_value ("%d", k); pcl::console::print_info (" nearest neighbors.\n");
00144 
00145   std::string kdtree_idx_file_name = "kdtree.idx";
00146   std::string training_data_h5_file_name = "training_data.h5";
00147   std::string training_data_list_file_name = "training_data.list";
00148 
00149   std::vector<vfh_model> models;
00150   flann::Matrix<int> k_indices;
00151   flann::Matrix<float> k_distances;
00152   flann::Matrix<float> data;
00153   // Check if the data has already been saved to disk
00154   if (!boost::filesystem::exists ("training_data.h5") || !boost::filesystem::exists ("training_data.list"))
00155   {
00156     pcl::console::print_error ("Could not find training data models files %s and %s!\n", 
00157         training_data_h5_file_name.c_str (), training_data_list_file_name.c_str ());
00158     return (-1);
00159   }
00160   else
00161   {
00162     loadFileList (models, training_data_list_file_name);
00163     flann::load_from_file (data, training_data_h5_file_name, "training_data");
00164     pcl::console::print_highlight ("Training data found. Loaded %d VFH models from %s/%s.\n", 
00165         (int)data.rows, training_data_h5_file_name.c_str (), training_data_list_file_name.c_str ());
00166   }
00167 
00168   // Check if the tree index has already been saved to disk
00169   if (!boost::filesystem::exists (kdtree_idx_file_name))
00170   {
00171     pcl::console::print_error ("Could not find kd-tree index in file %s!", kdtree_idx_file_name.c_str ());
00172     return (-1);
00173   }
00174   else
00175   {
00176     flann::Index<flann::ChiSquareDistance<float> > index (data, flann::SavedIndexParams ("kdtree.idx"));
00177     index.buildIndex ();
00178     nearestKSearch (index, histogram, k, k_indices, k_distances);
00179   }
00180 
00181   // Output the results on screen
00182   pcl::console::print_highlight ("The closest %d neighbors for %s are:\n", k, argv[pcd_indices[0]]);
00183   for (int i = 0; i < k; ++i)
00184     pcl::console::print_info ("    %d - %s (%d) with a distance of: %f\n", 
00185         i, models.at (k_indices[0][i]).first.c_str (), k_indices[0][i], k_distances[0][i]);
00186 
00187   // Load the results
00188   pcl::visualization::PCLVisualizer p (argc, argv, "VFH Cluster Classifier");
00189   int y_s = (int)floor (sqrt ((double)k));
00190   int x_s = y_s + (int)ceil ((k / (double)y_s) - y_s);
00191   double x_step = (double)(1 / (double)x_s);
00192   double y_step = (double)(1 / (double)y_s);
00193   pcl::console::print_highlight ("Preparing to load "); 
00194   pcl::console::print_value ("%d", k); 
00195   pcl::console::print_info (" files ("); 
00196   pcl::console::print_value ("%d", x_s);    
00197   pcl::console::print_info ("x"); 
00198   pcl::console::print_value ("%d", y_s); 
00199   pcl::console::print_info (" / ");
00200   pcl::console::print_value ("%f", x_step); 
00201   pcl::console::print_info ("x"); 
00202   pcl::console::print_value ("%f", y_step); 
00203   pcl::console::print_info (")\n");
00204 
00205   int viewport = 0, l = 0, m = 0;
00206   for (int i = 0; i < k; ++i)
00207   {
00208     std::string cloud_name = models.at (k_indices[0][i]).first;
00209     boost::replace_last (cloud_name, "_vfh", "");
00210 
00211     p.createViewPort (l * x_step, m * y_step, (l + 1) * x_step, (m + 1) * y_step, viewport);
00212     l++;
00213     if (l >= x_s)
00214     {
00215       l = 0;
00216       m++;
00217     }
00218 
00219     sensor_msgs::PointCloud2 cloud;
00220     pcl::console::print_highlight (stderr, "Loading "); pcl::console::print_value (stderr, "%s ", cloud_name.c_str ());
00221     if (pcl::io::loadPCDFile (cloud_name, cloud) == -1)
00222       break;
00223 
00224     // Convert from blob to PointCloud
00225     pcl::PointCloud<pcl::PointXYZ> cloud_xyz;
00226     pcl::fromROSMsg (cloud, cloud_xyz);
00227 
00228     if (cloud_xyz.points.size () == 0)
00229       break;
00230 
00231     pcl::console::print_info ("[done, "); 
00232     pcl::console::print_value ("%d", (int)cloud_xyz.points.size ()); 
00233     pcl::console::print_info (" points]\n");
00234     pcl::console::print_info ("Available dimensions: "); 
00235     pcl::console::print_value ("%s\n", pcl::getFieldsList (cloud).c_str ());
00236 
00237     // Demean the cloud
00238     Eigen::Vector4f centroid;
00239     pcl::compute3DCentroid (cloud_xyz, centroid);
00240     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz_demean (new pcl::PointCloud<pcl::PointXYZ>);
00241     pcl::demeanPointCloud<pcl::PointXYZ> (cloud_xyz, centroid, *cloud_xyz_demean);
00242     // Add to renderer*
00243     p.addPointCloud (cloud_xyz_demean, cloud_name, viewport);
00244     
00245     // Check if the model found is within our inlier tolerance
00246     std::stringstream ss;
00247     ss << k_distances[0][i];
00248     if (k_distances[0][i] > thresh)
00249     {
00250       p.addText (ss.str (), 20, 30, 1, 0, 0, ss.str (), viewport);  // display the text with red
00251 
00252       // Create a red line
00253       pcl::PointXYZ min_p, max_p;
00254       pcl::getMinMax3D (*cloud_xyz_demean, min_p, max_p);
00255       std::stringstream line_name;
00256       line_name << "line_" << i;
00257       p.addLine (min_p, max_p, 1, 0, 0, line_name.str (), viewport);
00258       p.setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 5, line_name.str (), viewport);
00259     }
00260     else
00261       p.addText (ss.str (), 20, 30, 0, 1, 0, ss.str (), viewport);
00262 
00263     // Increase the font size for the score*
00264     p.setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_FONT_SIZE, 18, ss.str (), viewport);
00265 
00266     // Add the cluster name
00267     p.addText (cloud_name, 20, 10, cloud_name, viewport);
00268   }
00269   // Add coordianate systems to all viewports
00270   p.addCoordinateSystem (0.1, 0);
00271 
00272   p.spin ();
00273   return (0);
00274 }


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