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
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
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
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;
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
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
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
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
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
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
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
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
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
00243 p.addPointCloud (cloud_xyz_demean, cloud_name, viewport);
00244
00245
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);
00251
00252
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
00264 p.setShapeRenderingProperties (pcl::visualization::PCL_VISUALIZER_FONT_SIZE, 18, ss.str (), viewport);
00265
00266
00267 p.addText (cloud_name, 20, 10, cloud_name, viewport);
00268 }
00269
00270 p.addCoordinateSystem (0.1, 0);
00271
00272 p.spin ();
00273 return (0);
00274 }