Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP
Browse files

Merge branch 'master' of github.com:b-anand/PR2-Feeder

  • Loading branch information...
commit d1cfb6c3fa61214a84d242666109f4a44e231507 2 parents 320d6b2 + 35d36a6
Shreesh shreeshga authored
2  food_detector/CMakeLists.txt
View
@@ -14,7 +14,7 @@ project(food_detector)
rosbuild_init()
#set(PCL_DIR "/opt/ros/electric/stacks/perception_pcl/pcl/share/pcl-1.1/")
-find_package(PCL 1.2 REQUIRED)
+#find_package(PCL 1.2 REQUIRED)
#find_package(FLANN 1.7.1 REQUIRED)
#set the default path for built executables to the "bin" directory
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
19 food_detector/scripts/brename.sh
View
@@ -0,0 +1,19 @@
+#!/bin/bash
+
+prefix="$1"
+shift
+
+base_n="$1"
+shift
+
+step="$1"
+shift
+
+n=$base_n
+
+for file in "$@" ; do
+ formatted_n=$(printf "%03d" $n)
+ # re-use original file extension whilke we're at it.
+ mv "$file" "${prefix}_${formatted_n}.${file##*.}"
+ let n=n+$step
+done
456 food_detector/src/vfh_food_classifier.cpp
View
@@ -1,7 +1,7 @@
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/common/common.h>
-//#include <pcl/common/transforms.h>
+#include <pcl/common/centroid.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/console/parse.h>
#include <pcl/console/print.h>
@@ -9,7 +9,24 @@
#include <iostream>
#include <flann/flann.h>
#include <flann/io/hdf5.h>
+#include <flann/util/matrix.h>
#include <boost/filesystem.hpp>
+#include <pcl-1.5/pcl/console/print.h>
+#include <pcl/surface/convex_hull.h>
+#include <pcl/segmentation/extract_polygonal_prism_data.h>
+#include <pcl/ModelCoefficients.h>
+#include <pcl/point_types.h>
+#include <pcl/io/pcd_io.h>
+#include <pcl/filters/extract_indices.h>
+#include <pcl/filters/voxel_grid.h>
+#include <pcl/features/normal_3d.h>
+#include <pcl-1.5/pcl/kdtree/kdtree.h>
+#include <pcl/sample_consensus/method_types.h>
+#include <pcl/sample_consensus/model_types.h>
+#include <pcl/segmentation/sac_segmentation.h>
+#include <pcl/segmentation/extract_clusters.h>
+#include <pcl/filters/filter.h>
+#include <pcl/features/vfh.h>
typedef std::pair<std::string, std::vector<float> > vfh_model;
@@ -19,88 +36,343 @@ typedef pcl::PointCloud<pcl::VFHSignature308> cloud_vfh;
typedef pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_pos_ptr;
typedef pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_prgb_ptr;
typedef pcl::PointCloud<pcl::VFHSignature308>::Ptr cloud_vfh_ptr;
+namespace fs = boost::filesystem;
+typedef std::multimap<std::time_t, fs::path> result_set_t;
namespace vfh_food_classifier {
- class VFHFoodClassifier {
+
+ class VFHFoodClassifier {
private:
- static int k = 6;
- static double thresh= DBL_MAX; // No threshold, disabled by default
+ int k;
+ double thresh; // No threshold, disabled by default
vfh_model histogram;
std::vector<vfh_model> models;
flann::Matrix<int> k_indices;
flann::Matrix<float> k_distances;
flann::Matrix<float> data;
std::vector<int> pcd_indices;
+ std::string test_file;
+
public:
- bool loadHistFile(const boost::filesystem::path &path, vfh_model &vfh);
- VFHFoodClassifier()
- {
+ result_set_t result_set;
+ boost::filesystem::path outPath;
+
+ bool loadHistFile(const boost::filesystem::path &path, vfh_model &vfh);
+ VFHFoodClassifier() {
+ k = 6;
+ thresh = DBL_MAX;
}
~VFHFoodClassifier() {
}
- cloud_pos_ptr getClosestMatch(cloud_prgb_ptr);
+ bool loadPCDFiles(const boost::filesystem::path &dirPath,const boost::filesystem::path &outPath) {
+ fs::directory_iterator end_iter;
+ pcl::PCDReader r;
+
+
+ this->outPath = outPath;
+ if ( fs::exists(dirPath) && fs::is_directory(dirPath))
+ {
+ for( fs::directory_iterator dir_iter(dirPath) ; dir_iter != end_iter ; ++dir_iter)
+ {
+ if (fs::is_regular_file(dir_iter->status()) )
+ {
+ result_set.insert(result_set_t::value_type(fs::last_write_time(*dir_iter)
+ , *dir_iter));
+ }
+ }
+ }
+ return true;
+ }
-
+ void getMinX(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud,double &max_x,double &min_x,double &max_y,double &min_y) {
+ std::vector<double> xarray;
+ std::vector<double> yarray;
+ for (int i =0 ;i < cloud->points.size();i++) {
+ xarray.push_back(cloud->points[i].x);
+ yarray.push_back(cloud->points[i].y);
+ }
+ std::sort(xarray.begin(),xarray.end());
+ std::sort(yarray.begin(),yarray.end());
+ min_x = xarray.front();
+ max_x = xarray.back();
+ min_y = yarray.front();
+ max_y = yarray.back();
+ }
+
+
+ void extractClusters(sensor_msgs::PointCloud2 &cloud_in,std::string &filePrefix) {
+ sensor_msgs::PointCloud2::Ptr cloud (new sensor_msgs::PointCloud2(cloud_in));
+// sensor_msgs::PointCloud2::Ptr cloud_f (new sensor_msgs::PointCloud2);
+ sensor_msgs::PointCloud2::Ptr cloud_filtered_blob (new sensor_msgs::PointCloud2);
+ double plane_min_x,plane_max_x;
+ double cluster_min_x,cluster_max_x;
+ double plane_min_y,plane_max_y;
+ double cluster_min_y,cluster_max_y;
+// double plane_min_z,plane_max_z;
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>);
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZRGB>);
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_non_planars (new pcl::PointCloud<pcl::PointXYZRGB>);
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_objects (new pcl::PointCloud<pcl::PointXYZRGB>);
+
+ std::cout <<"============================"<<std::endl;
+ std::cout <<filePrefix.c_str()<< " PointCloud before filtering has: " << cloud_in.data.size () << " data points." << std::endl; //*
+
+ pcl::fromROSMsg(cloud_in,*cloud_filtered);
+ // Remove nans
+ std::vector<int> indices;
+ pcl::removeNaNFromPointCloud(*cloud_filtered,*cloud_filtered,indices);
+ // Create the filtering object: downsample the dataset using a leaf size of 1cm
+// pcl::VoxelGrid<pcl::PointXYZRGB>vg;
+// vg.setInputCloud (cloud_filtered);
+// vg.setLeafSize (0.01f, 0.01f, 0.01f);
+// vg.filter (*cloud_filtered);
+// vg.setFilterFieldName("z");
+// vg.setFilterLimits(0.001,1.5);
+// vg.setFilterLimitsNegative(false);
+// pcl::fromROSMsg(*cloud,*cloud_filtered);
+
+ std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size () << " data points." << std::endl; //*
+
+ // Create the segmentation object for the planar model and set all the parameters
+ pcl::SACSegmentation<pcl::PointXYZRGB> seg;
+ pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
+ pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
+ pcl::PCDWriter writer;
+ seg.setOptimizeCoefficients (true);
+ seg.setModelType (pcl::SACMODEL_PLANE);
+ seg.setMethodType (pcl::SAC_RANSAC);
+ seg.setMaxIterations (1000);
+ seg.setDistanceThreshold (0.01);
+ seg.setEpsAngle(0.09);
+
+ int i=0, nr_points = (int) cloud_filtered->points.size ();
+ while (cloud_filtered->points.size () > 0.3 * nr_points)
+ {
+ // Segment the largest planar component from the remaining cloud
+ seg.setInputCloud (cloud_filtered);
+ seg.segment (*inliers, *coefficients);
+ if (inliers->indices.size () == 0)
+ {
+ std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
+ break;
+ }
+
+ // Extract the planar inliers from the input cloud
+ pcl::ExtractIndices<pcl::PointXYZRGB> extract;
+ extract.setInputCloud (cloud_filtered);
+ extract.setIndices (inliers);
+ extract.setNegative (false);
+
+ // Write the planar inliers
+ extract.filter (*cloud_plane);
+ std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;
+
+ std::cout << plane_max_x<<std::endl;
+// getMinX(cloud_plane,plane_max_x,plane_min_x,plane_max_y,plane_min_y);
+ std::cout<<plane_max_x<<std::endl;
+ // Remove the planar inliers, extract the rest
+ extract.setNegative (true);
+ extract.filter (*cloud_non_planars);
+
+// // Create a Convex Hull representation of the projected inliers
+// pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_hull
+// (new pcl::PointCloud<pcl::PointXYZRGB>);
+// pcl::ConvexHull<pcl::PointXYZRGB> chull;
+// chull.setInputCloud (cloud_plane);
+// chull.reconstruct (*cloud_hull);
+//
+// // segment those points that are in the polygonal prism
+// pcl::PointIndices::Ptr prism_inliers (new pcl::PointIndices);
+// pcl::ExtractPolygonalPrismData<pcl::PointXYZRGB> ex;
+// ex.setInputCloud (cloud_non_planars);
+// ex.setInputPlanarHull (cloud_hull);
+//// ex.setHeightLimits(0.001,0.07);
+// ex.segment(*prism_inliers);
+// // Extract the prism inliers from the input cloud
+// pcl::ExtractIndices<pcl::PointXYZRGB> prism_extract;
+// prism_extract.setInputCloud (cloud_non_planars);
+// prism_extract.setIndices (prism_inliers);
+// prism_extract.setNegative (false);
+// prism_extract.filter (*cloud_objects);
+
+ cloud_filtered = cloud_non_planars;
+
+ }
+
+ // Creating the KdTree object for the search method of the extraction
+// pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
+// tree->setInputCloud (cloud_filtered);
+
+ // Creating the KdTree object for the search method of the extraction
+ pcl::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::KdTreeFLANN<pcl::PointXYZRGB>);
+ tree->setInputCloud (cloud_filtered);
+
+ std::vector<pcl::PointIndices> cluster_indices;
+ pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
+ ec.setClusterTolerance (0.02); // 2cm
+ ec.setMinClusterSize (100);
+ ec.setMaxClusterSize (10000);
+ ec.setSearchMethod (tree);
+ ec.setInputCloud (cloud_filtered);
+ ec.extract (cluster_indices);
+
+ int j = 0;
+ for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
+ {
+ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZRGB>);
+ for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
+ cloud_cluster->points.push_back (cloud_filtered->points[*pit]); //*
+ cloud_cluster->width = cloud_cluster->points.size ();
+ cloud_cluster->height = 1;
+ cloud_cluster->is_dense = true;
+
+// getMinX(cloud_cluster,cluster_max_x,cluster_min_x,cluster_max_y,cluster_min_y);
+// if(cluster_max_x > plane_max_x || cluster_min_x < plane_min_x )
+// continue;
+ std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;
+ std::stringstream ss;
+ ss << this->outPath.string().c_str() << filePrefix.c_str()<<"_cluster_" << j << ".pcd";
+ writer.write<pcl::PointXYZRGB> (ss.str (), *cloud_cluster, false); //*
+ j++;
+ }
+
+
+ }
+
+
+
+ void generateVFH(const boost::filesystem::path &dirPath) {
+ fs::directory_iterator end_iter;
+
+ cloud_prgb_ptr cloud_in (new cloud_prgb);
+ cloud_prgb_ptr cloud (new cloud_prgb);
+ pcl::PCDReader r;
+ if ( fs::exists(dirPath) && fs::is_directory(dirPath))
+ {
+ for( fs::directory_iterator dir_iter(dirPath) ; dir_iter != end_iter ; ++dir_iter)
+ {
+ if (fs::is_regular_file(dir_iter->status()) )
+ {
+ sensor_msgs::PointCloud2 cloud_blob;
+ int version;
+ Eigen::Vector4f origin;
+ Eigen::Quaternionf orientation;
+ pcl::PCDReader r;
+ bool type; int idx;
+ boost::filesystem::path filename = *dir_iter;
+ r.read(filename.string(), cloud_blob, origin, orientation, version);
+ pcl::fromROSMsg(cloud_blob,*cloud_in);
+
+ std::cout << "Loaded "<< cloud_in->width * cloud_in->height<< " data points\n";
+
+ // Remove nans
+ std::vector<int> indices;
+ pcl::removeNaNFromPointCloud(*cloud_in,*cloud,indices);
+ // Create the normal estimation class, and pass the input dataset to it
+ pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> ne;
+ ne.setInputCloud (cloud);
+
+ // Create an empty kdtree representation, and pass it to the normal estimation object.
+ // Its content will be filled inside the object, based on the given input dataset (as no other search surface is given).
+ pcl::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::KdTreeFLANN<pcl::PointXYZRGB>);
+ tree->setInputCloud(cloud);
+ ne.setSearchMethod (tree);
+
+ // Output datasets
+ pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
+
+ // Use all neighbors in a sphere of radius scaleFactor cm
+ ne.setRadiusSearch (0.03);
+
+ // Compute the normals
+ ne.compute (*normals);
+ // Create the VFH estimation class, and pass the input dataset+normals to it
+ //pcl::VFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::VFHSignature308> vfh;
+ pcl::VFHEstimation<pcl::PointXYZRGB, pcl::Normal, pcl::VFHSignature308> vfh;
+ vfh.setInputCloud (cloud);
+ vfh.setInputNormals (normals);
+ // Create an empty kdtree representation, and pass it to the FPFH estimation object.
+ // Its content will be filled inside the object, based on the given input dataset (as no other search surface is given).
+ // pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr tree (new pcl::KdTreeFLANN<pcl::PointXYZ> ());
+ vfh.setSearchMethod (tree);
+
+ // Output datasets
+ cloud_vfh_ptr vfhs (new cloud_vfh ());
+
+ // Compute the features
+ vfh.compute (*vfhs);
+
+ //save to file
+ std::vector<std::string> strs;
+ boost::split(strs, filename.string(),boost::is_any_of("."));
+ cout<< strs[0];
+ std::stringstream name;
+ name << strs[0]<< "_vfh" << ".pcd";
+ pcl::io::savePCDFile(name.str(),*vfhs,false);
+ }
+ }
+ }
+
+
+ }
+
/** \brief Loads an n-D histogram file as a VFH signature
- * \param path the input file name
- * \param vfh the resultant VFH model
- */
- bool
- loadHist (const boost::filesystem::path &path, vfh_model &vfh)
- {
- int vfh_idx;
- // Load the file as a PCD
- try
- {
- sensor_msgs::PointCloud2 cloud;
- int version;
- Eigen::Vector4f origin;
- Eigen::Quaternionf orientation;
- pcl::PCDReader r;
- int type; int idx;
- r.readHeader (path.string (), cloud, origin, orientation, version, type, idx);
-
- vfh_idx = pcl::getFieldIndex (cloud, "vfh");
- if (vfh_idx == -1)
- return (false);
- if ((int)cloud.width * cloud.height != 1)
- return (false);
- }
- catch (pcl::InvalidConversionException e)
- {
- return (false);
- }
-
- // Treat the VFH signature as a single Point Cloud
- pcl::PointCloud <pcl::VFHSignature308> point;
- pcl::io::loadPCDFile (path.string (), point);
- vfh.second.resize (308);
-
- std::vector <sensor_msgs::PointField> fields;
- getFieldIndex (point, "vfh", fields);
-
- for (size_t i = 0; i < fields[vfh_idx].count; ++i)
- {
- vfh.second[i] = point.points[0].histogram[i];
- }
- vfh.first = path.string ();
- return (true);
+ * \param path the input file name
+ * \param vfh the resultant VFH model
+ */
+ bool loadHist(const boost::filesystem::path &path, vfh_model &vfh) {
+ int vfh_idx;
+ // Load the file as a PCD
+ try
+ {
+ sensor_msgs::PointCloud2 cloud;
+ int version;
+ Eigen::Vector4f origin;
+ Eigen::Quaternionf orientation;
+ pcl::PCDReader r;
+ bool type; int idx;
+ r.readHeader(path.string (), cloud, origin, orientation, version, type, idx);
+
+ vfh_idx = pcl::getFieldIndex (cloud, "vfh");
+ if (vfh_idx == -1)
+ return (false);
+ if ((int)cloud.width * cloud.height != 1)
+ return (false);
+ }
+ catch (pcl::InvalidConversionException e)
+ {
+ return (false);
+ }
+
+ // Treat the VFH signature as a single Point Cloud
+ pcl::PointCloud <pcl::VFHSignature308> point;
+ pcl::io::loadPCDFile(path.string(), point);
+ vfh.second.resize(308);
+
+ std::vector <sensor_msgs::PointField> fields;
+ pcl::getFieldIndex(point, "vfh", fields);
+
+ for (size_t i = 0; i < fields[vfh_idx].count; ++i) {
+ vfh.second[i] = point.points[0].histogram[i];
+ }
+ vfh.first = path.string();
+ test_file = path.string();
+ return (true);
}
-
-
+
bool loadHistFile(int argc, char **argv) {
// Load the file as a PCD
- const boost::filesystem::path &path, vfh_model &vfh
-// std::string extension (".pcd");
-// transform (extension.begin (), extension.end (), extension.begin (), (int(*)(int))tolower);
+ // std::string extension (".pcd");
+ // transform (extension.begin (), extension.end (), extension.begin (), (int(*)(int))tolower);
// Load the test histogram
- pcd_indices = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
- if (!loadHist (argv[pcd_indices.at (0)], histogram))
- {
- pcl::console::print_error ("Cannot load test file %s\n", argv[pcd_indices.at (0)]);
+ pcd_indices = pcl::console::parse_file_extension_argument(argc, argv,
+ ".pcd");
+ if (!loadHist(argv[pcd_indices.at (0)], histogram)) {
+ pcl::console::print_error("Cannot load test file %s\n",
+ argv[pcd_indices.at (0)]);
return (-1);
}
return (true);
@@ -119,12 +391,12 @@ namespace vfh_food_classifier {
flann::Matrix<float> &distances) {
// Query point
flann::Matrix<float> p = flann::Matrix<float>(new float[model.second.size ()], 1, model.second.size ());
- memcpy (&p.ptr ()[0], &model.second[0], p.cols * p.rows * sizeof (float));
+ memcpy (&p.ptr()[0], &model.second[0], p.cols * p.rows * sizeof (float));
indices = flann::Matrix<int>(new int[k], 1, k);
distances = flann::Matrix<float>(new float[k], 1, k);
index.knnSearch (p, indices, distances, k, flann::SearchParams (512));
- delete[] p.ptr ();
+ delete[] p.ptr();
}
/** \brief Load the list of file model names from an ASCII file
@@ -189,7 +461,7 @@ namespace vfh_food_classifier {
}
// Output the results on screen
- pcl::console::print_highlight ("The closest %d neighbors for %s are:\n", k, argv[pcd_indices[0]]);
+ pcl::console::print_highlight ("The closest %d neighbors for %s are:\n", k, test_file.c_str());
for (int i = 0; i < k; ++i)
pcl::console::print_info (" %d - %s (%d) with a distance of: %f\n",
i, models.at (k_indices[0][i]).first.c_str (), k_indices[0][i], k_distances[0][i]);
@@ -201,7 +473,8 @@ namespace vfh_food_classifier {
display_results()
{
// Load the results
- pcl::visualization::PCLVisualizer p (argc, argv, "VFH Cluster Classifier");
+ //pcl::visualization::PCLVisualizer p (argc, argv, "VFH Cluster Classifier");
+ pcl::visualization::PCLVisualizer p ("VFH Cluster Classifier");
int y_s = (int)floor (sqrt ((double)k));
int x_s = y_s + (int)ceil ((k / (double)y_s) - y_s);
double x_step = (double)(1 / (double)x_s);
@@ -252,9 +525,9 @@ namespace vfh_food_classifier {
// Demean the cloud
Eigen::Vector4f centroid;
- pcl::compute3DCentroid (cloud_xyz, centroid);
+ pcl::compute3DCentroid(cloud_xyz, centroid);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_xyz_demean (new pcl::PointCloud<pcl::PointXYZ>);
- pcl::demeanPointCloud<pcl::PointXYZ> (cloud_xyz, centroid, *cloud_xyz_demean);
+ pcl::demeanPointCloud(cloud_xyz, centroid, *cloud_xyz_demean);
// Add to renderer*
p.addPointCloud (cloud_xyz_demean, cloud_name, viewport);
@@ -284,16 +557,45 @@ namespace vfh_food_classifier {
}
// Add coordianate systems to all viewports
p.addCoordinateSystem (0.1, 0);
+ p.spin();
}
};
+}
+
+
+void usage() {
+ pcl::console::print_error("usage:object_classifier input_dir output_dir");
+}
+int main(int argc, char** argv) {
+ if(argc < 2) {
+ usage();
+ exit(0);
+ }
+ pcl::PCDReader r;
+ sensor_msgs::PointCloud2 cloud;
+ Eigen::Vector4f origin;
+ Eigen::Quaternionf orientation;
+ int version;
+
+ vfh_food_classifier::VFHFoodClassifier* vfh = new vfh_food_classifier::VFHFoodClassifier();
+ if(vfh->loadPCDFiles(argv[1],argv[2])){
+ for (std::multimap<std::time_t, fs::path>::iterator it = vfh->result_set.begin();
+ it != vfh->result_set.end() ; ++it) {
+
+ boost::filesystem::path fileName = (*it).second;
+ // boost::filesystem::path outPath = vfh->outPath;
+ r.read(fileName.string(),cloud,origin,orientation,version);
+ pcl::console::print_info ("Size of PCD File %s data %d\n", fileName.c_str(),cloud.data.size());
+ std::string f = fileName.string();
+ std::string str = f.substr(0,f.rfind('.'));
+
+ vfh->extractClusters(cloud, str);
+ }
+ } else {
+ pcl::console::print_error("Error while reading Data");
+ }
+
+ //vfh->generateVFH(argv[1]);
+ return (0);
+}
- int
- main (int argc, char** argv)
- {
- VFHFoodClassifier* vfh = new VFHFoodClassifier();
- vfh->loadHistFile(argc,argv);
- vfh->nearest_search();
- vfh->display_results();
- p.spin ();
- return (0);
- }
Please sign in to comment.
Something went wrong with that request. Please try again.