vfh_nn_classifier.h
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: vfh_nn_classifier.h 6218 2012-07-06 21:46:51Z aichim $
00035  *
00036  */
00037 
00038 #ifndef VFHCLASSIFICATION_H_
00039 #define VFHCLASSIFICATION_H_
00040 
00041 #include <fstream>
00042 #include <pcl/point_types.h>
00043 #include <pcl/io/pcd_io.h>
00044 #include <pcl/features/vfh.h>
00045 #include <pcl/features/normal_3d.h>
00046 #include <pcl/apps/nn_classification.h>
00047 #include <sensor_msgs/PointCloud2.h>
00048 
00049 namespace pcl
00050 {
00056   template <typename PointT> pcl::PointCloud<pcl::VFHSignature308>::Ptr
00057   computeVFH (typename PointCloud<PointT>::ConstPtr cloud, double radius)
00058   {
00059     using namespace pcl;
00060 
00061     // Create an empty kdtree representation, and pass it to the objects.
00062     // Its content will be filled inside the object, based on the given input dataset (as no other search surface is given).
00063     typename pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT> ());
00064 
00065     // Create the normal estimation class, and pass the input dataset to it
00066     NormalEstimation<PointT, Normal> ne;
00067     ne.setInputCloud (cloud);
00068     ne.setSearchMethod (tree);
00069 
00070     // Use all neighbors in a sphere of given radius to compute the normals
00071     PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
00072     ne.setRadiusSearch (radius);
00073     ne.compute (*normals);
00074 
00075     // Create the VFH estimation class, and pass the input dataset+normals to it
00076     VFHEstimation<PointT, Normal, VFHSignature308> vfh;
00077     vfh.setInputCloud (cloud);
00078     vfh.setInputNormals (normals);
00079     vfh.setSearchMethod (tree);
00080 
00081     // Output datasets
00082     PointCloud<VFHSignature308>::Ptr vfhs (new PointCloud<VFHSignature308>);
00083 
00084     // Compute the features and return
00085     vfh.compute (*vfhs);
00086     return vfhs;
00087   }
00088 
00093   class VFHClassifierNN
00094   {
00095     public:
00096 
00097       typedef pcl::PointCloud<pcl::VFHSignature308> FeatureCloud;
00098       typedef pcl::PointCloud<pcl::VFHSignature308>::Ptr FeatureCloudPtr;
00099       typedef pcl::PointCloud<pcl::VFHSignature308>::ConstPtr FeatureCloudConstPtr;
00100       typedef NNClassification<pcl::VFHSignature308>::Result Result;
00101       typedef NNClassification<pcl::VFHSignature308>::ResultPtr ResultPtr;
00102 
00103     private:
00104 
00106       FeatureCloudPtr training_features_;
00108       std::vector<std::string> labels_;
00110       NNClassification<pcl::VFHSignature308> classifier_;
00111 
00112     public:
00113 
00114       VFHClassifierNN () : training_features_ (), labels_ (), classifier_ ()
00115       {
00116         reset ();
00117       }
00118 
00119       void reset ()
00120       {
00121         training_features_.reset (new FeatureCloud);
00122         labels_.clear ();
00123         classifier_ = NNClassification<pcl::VFHSignature308> ();
00124       }
00125 
00127       void finalizeTraining ()
00128       {
00129         finalizeTree ();
00130         finalizeLabels ();
00131       }
00132 
00134       void finalizeTree ()
00135       {
00136         classifier_.setTrainingFeatures(training_features_);
00137       }
00138 
00140       void finalizeLabels ()
00141       {
00142         classifier_.setTrainingLabels(labels_);
00143       }
00144 
00150       bool saveTrainingFeatures(std::string file_name, std::string labels_file_name)
00151       {
00152         if (labels_.size () == training_features_->points.size ())
00153         {
00154           if (pcl::io::savePCDFile (file_name.c_str (), *training_features_) != 0)
00155             return false;
00156           std::ofstream f (labels_file_name.c_str ());
00157           BOOST_FOREACH (std::string s, labels_)
00158             f << s << "\n";
00159           return true;
00160         }
00161         return false;
00162       }
00163 
00170       bool addTrainingFeatures (const FeatureCloudPtr training_features, const std::vector<std::string> &labels)
00171       {
00172         if (labels.size () == training_features->points.size ())
00173         {
00174           labels_.insert (labels_.end (), labels.begin (), labels.end ());
00175           training_features_->points.insert (training_features_->points.end (), training_features->points.begin (), training_features->points.end ());
00176           training_features_->header = training_features->header;
00177           training_features_->height = 1;
00178           training_features_->width  = static_cast<uint32_t> (training_features_->points.size ());
00179           training_features_->is_dense &= training_features->is_dense;
00180           training_features_->sensor_origin_ = training_features->sensor_origin_;
00181           training_features_->sensor_orientation_ = training_features->sensor_orientation_;
00182           return true;
00183         }
00184         return false;
00185       }
00186 
00193       bool loadTrainingFeatures(std::string file_name, std::string labels_file_name)
00194       {
00195         FeatureCloudPtr cloud (new FeatureCloud);
00196         if (pcl::io::loadPCDFile (file_name.c_str (), *cloud) != 0)
00197           return false;
00198         std::vector<std::string> labels;
00199         std::ifstream f (labels_file_name.c_str ());
00200         std::string label;
00201         while (getline (f, label))
00202           if (label.size () > 0)
00203             labels.push_back(label);
00204         return addTrainingFeatures (cloud, labels);
00205       }
00206 
00214       bool loadTrainingData (std::string file_name, std::string label)
00215       {
00216         sensor_msgs::PointCloud2 cloud_blob;
00217         if (pcl::io::loadPCDFile (file_name.c_str (), cloud_blob) != 0)
00218           return false;
00219         return addTrainingData (cloud_blob, label);
00220       }
00221 
00228       bool addTrainingData (const sensor_msgs::PointCloud2 &training_data, std::string &label)
00229       {
00230         // Create label list containing the single label
00231         std::vector<std::string> labels;
00232         labels.push_back (label);
00233 
00234         // Compute the feature from the cloud and add it as a training example
00235         FeatureCloudPtr vfhs = computeFeature (training_data);
00236         return addTrainingFeatures(vfhs, labels);
00237       }
00238 
00245       ResultPtr classify (const sensor_msgs::PointCloud2 &testing_data, double radius = 300, double min_score = 0.002)
00246       {
00247         // compute the VFH feature for this point cloud
00248         FeatureCloudPtr vfhs = computeFeature (testing_data);
00249         // compute gaussian parameter producing the desired minimum score (around 50 for the default values)
00250         float gaussian_param = - static_cast<float> (radius / log (min_score));
00251         // TODO accept result to be filled in by reference
00252         return classifier_.classify(vfhs->points.at (0), radius, gaussian_param);
00253       }
00254 
00260       FeatureCloudPtr computeFeature (const sensor_msgs::PointCloud2 &points, double radius = 0.03)
00261       {
00262         pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ> ());
00263         pcl::fromROSMsg (points, *cloud);
00264         return pcl::computeVFH<pcl::PointXYZ> (cloud, radius);
00265       }
00266   };
00267 }
00268 
00269 #endif /* VFHCLASSIFICATION_H_ */


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:19:00