gfpfh.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Point Cloud Library (PCL) - www.pointclouds.org
00005  *  Copyright (c) 2009, Willow Garage, Inc.
00006  *  Copyright (c) 2012-, Open Perception, Inc.
00007  *
00008  *  All rights reserved.
00009  *
00010  *  Redistribution and use in source and binary forms, with or without
00011  *  modification, are permitted provided that the following conditions
00012  *  are met:
00013  *
00014  *   * Redistributions of source code must retain the above copyright
00015  *     notice, this list of conditions and the following disclaimer.
00016  *   * Redistributions in binary form must reproduce the above
00017  *     copyright notice, this list of conditions and the following
00018  *     disclaimer in the documentation and/or other materials provided
00019  *     with the distribution.
00020  *   * Neither the name of the copyright holder(s) nor the names of its
00021  *     contributors may be used to endorse or promote products derived
00022  *     from this software without specific prior written permission.
00023  *
00024  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00025  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00026  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00027  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00028  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00029  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00030  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00031  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00032  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00033  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00034  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00035  *  POSSIBILITY OF SUCH DAMAGE.
00036  *
00037  * $Id: gfpfh.hpp 2218 2011-08-25 20:27:15Z rusu $
00038  *
00039  */
00040 
00041 #ifndef PCL_FEATURES_IMPL_GFPFH_H_
00042 #define PCL_FEATURES_IMPL_GFPFH_H_
00043 
00044 #include <pcl/features/gfpfh.h>
00045 #include <pcl/octree/octree.h>
00046 #include <pcl/octree/octree_search.h>
00047 #include <pcl/common/eigen.h>
00048 
00049 #include <algorithm>
00050 #include <fstream>
00051 
00053 template<typename PointInT, typename PointNT, typename PointOutT> void
00054 pcl::GFPFHEstimation<PointInT, PointNT, PointOutT>::compute (PointCloudOut &output)
00055 {
00056   if (!Feature<PointInT, PointOutT>::initCompute ())
00057   {
00058     output.width = output.height = 0;
00059     output.points.clear ();
00060     return;
00061   }
00062   // Copy the header
00063   output.header = input_->header;
00064 
00065   // Resize the output dataset
00066   // Important! We should only allocate precisely how many elements we will need, otherwise
00067   // we risk at pre-allocating too much memory which could lead to bad_alloc 
00068   // (see http://dev.pointclouds.org/issues/657)
00069   output.width = output.height = 1;
00070   output.is_dense = input_->is_dense;
00071   output.points.resize (1);
00072 
00073   // Perform the actual feature computation
00074   computeFeature (output);
00075 
00076   Feature<PointInT, PointOutT>::deinitCompute ();
00077 }
00078 
00080 template <typename PointInT, typename PointNT, typename PointOutT> void
00081 pcl::GFPFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
00082 {
00083   pcl::octree::OctreePointCloudSearch<PointInT> octree (octree_leaf_size_);
00084   octree.setInputCloud (input_);
00085   octree.addPointsFromInputCloud ();
00086 
00087   typename pcl::PointCloud<PointInT>::VectorType occupied_cells;
00088   octree.getOccupiedVoxelCenters (occupied_cells);
00089 
00090   // Determine the voxels crosses along the line segments
00091   // formed by every pair of occupied cells.
00092   std::vector< std::vector<int> > line_histograms;
00093   for (size_t i = 0; i < occupied_cells.size (); ++i)
00094   {
00095     Eigen::Vector3f origin = occupied_cells[i].getVector3fMap ();
00096 
00097     for (size_t j = i+1; j < occupied_cells.size (); ++j)
00098     {
00099       typename pcl::PointCloud<PointInT>::VectorType intersected_cells;
00100       Eigen::Vector3f end = occupied_cells[j].getVector3fMap ();
00101       octree.getApproxIntersectedVoxelCentersBySegment (origin, end, intersected_cells, 0.5f);
00102 
00103       // Intersected cells are ordered from closest to furthest w.r.t. the origin.
00104       std::vector<int> histogram;
00105       for (size_t k = 0; k < intersected_cells.size (); ++k)
00106       {
00107         std::vector<int> indices;
00108         octree.voxelSearch (intersected_cells[k], indices);
00109         int label = emptyLabel ();
00110         if (indices.size () != 0)
00111         {
00112           label = getDominantLabel (indices);
00113         }
00114         histogram.push_back (label);
00115       }
00116 
00117       line_histograms.push_back(histogram);
00118     }
00119   }
00120 
00121   std::vector< std::vector<int> > transition_histograms;
00122   computeTransitionHistograms (line_histograms, transition_histograms);
00123 
00124   std::vector<float> distances;
00125   computeDistancesToMean (transition_histograms, distances);
00126 
00127   std::vector<float> gfpfh_histogram;
00128   computeDistanceHistogram (distances, gfpfh_histogram);
00129 
00130   output.clear ();
00131   output.width = 1;
00132   output.height = 1;
00133   output.points.resize (1);
00134   std::copy (gfpfh_histogram.begin (), gfpfh_histogram.end (), output.points[0].histogram);
00135 }
00136 
00138 template <typename PointInT, typename PointNT, typename PointOutT> void
00139 pcl::GFPFHEstimation<PointInT, PointNT, PointOutT>::computeTransitionHistograms (const std::vector< std::vector<int> >& label_histograms,
00140                                                                                  std::vector< std::vector<int> >& transition_histograms)
00141 {
00142   transition_histograms.resize (label_histograms.size ());
00143 
00144   for (size_t i = 0; i < label_histograms.size (); ++i)
00145   {
00146     transition_histograms[i].resize ((getNumberOfClasses () + 2) * (getNumberOfClasses () + 1) / 2, 0);
00147 
00148     std::vector< std::vector <int> > transitions (getNumberOfClasses () + 1);
00149     for (size_t k = 0; k < transitions.size (); ++k)
00150     {
00151       transitions[k].resize (getNumberOfClasses () + 1, 0);
00152     }
00153 
00154     for (size_t k = 1; k < label_histograms[i].size (); ++k)
00155     {
00156       uint32_t first_class = label_histograms[i][k-1];
00157       uint32_t second_class = label_histograms[i][k];
00158       // Order has no influence.
00159       if (second_class < first_class)
00160         std::swap (first_class, second_class);
00161 
00162       transitions[first_class][second_class] += 1;
00163     }
00164 
00165     // Build a one-dimension histogram out of it.
00166     int flat_index = 0;
00167     for (int m = 0; m < static_cast<int> (transitions.size ()); ++m)
00168       for (int n = m; n < static_cast<int> (transitions[m].size ()); ++n)
00169       {
00170         transition_histograms[i][flat_index] = transitions[m][n];
00171         ++flat_index;
00172       }
00173 
00174     assert (flat_index == static_cast<int> (transition_histograms[i].size ()));
00175   }
00176 }
00177 
00179 template <typename PointInT, typename PointNT, typename PointOutT> void
00180 pcl::GFPFHEstimation<PointInT, PointNT, PointOutT>::computeDistancesToMean (const std::vector< std::vector<int> >& transition_histograms,
00181                                                                             std::vector<float>& distances)
00182 {
00183   distances.resize (transition_histograms.size ());
00184 
00185   std::vector<float> mean_histogram;
00186   computeMeanHistogram (transition_histograms, mean_histogram);
00187 
00188   for (size_t i = 0; i < transition_histograms.size (); ++i)
00189   {
00190     float d = computeHIKDistance (transition_histograms[i], mean_histogram);
00191     distances[i] = d;
00192   }
00193 }
00194 
00196 template <typename PointInT, typename PointNT, typename PointOutT> void
00197 pcl::GFPFHEstimation<PointInT, PointNT, PointOutT>::computeDistanceHistogram (const std::vector<float>& distances,
00198                                                                               std::vector<float>& histogram)
00199 {
00200   std::vector<float>::const_iterator min_it = std::min_element (distances.begin (), distances.end ());
00201   assert (min_it != distances.end ());
00202   const float min_value = *min_it;
00203 
00204   std::vector<float>::const_iterator max_it = std::max_element (distances.begin (), distances.end ());
00205   assert (max_it != distances.end());
00206   const float max_value = *max_it;
00207 
00208   histogram.resize (descriptorSize (), 0);
00209 
00210   const float range = max_value - min_value;
00211   const int max_bin = descriptorSize () - 1;
00212   for (size_t i = 0; i < distances.size (); ++i)
00213   {
00214     const float raw_bin = static_cast<float> (descriptorSize ()) * (distances[i] - min_value) / range;
00215     int bin = std::min (max_bin, static_cast<int> (floor (raw_bin)));
00216     histogram[bin] += 1;
00217   }
00218 }
00219 
00221 template <typename PointInT, typename PointNT, typename PointOutT> void
00222 pcl::GFPFHEstimation<PointInT, PointNT, PointOutT>::computeMeanHistogram (const std::vector< std::vector<int> >& histograms,
00223                                                                           std::vector<float>& mean_histogram)
00224 {
00225   assert (histograms.size () > 0);
00226 
00227   mean_histogram.resize (histograms[0].size (), 0);
00228   for (size_t i = 0; i < histograms.size (); ++i)
00229     for (size_t j = 0; j < histograms[i].size (); ++j)
00230       mean_histogram[j] += static_cast<float> (histograms[i][j]);
00231 
00232   for (size_t i = 0; i < mean_histogram.size (); ++i)
00233     mean_histogram[i] /= static_cast<float> (histograms.size ());
00234 }
00235 
00237 template <typename PointInT, typename PointNT, typename PointOutT> float
00238 pcl::GFPFHEstimation<PointInT, PointNT, PointOutT>::computeHIKDistance (const std::vector<int>& histogram,
00239                                                                         const std::vector<float>& mean_histogram)
00240 {
00241   assert (histogram.size () == mean_histogram.size ());
00242 
00243   float norm = 0.f;
00244   for (size_t i = 0; i < histogram.size (); ++i)
00245     norm += std::min (static_cast<float> (histogram[i]), mean_histogram[i]);
00246 
00247   norm /= static_cast<float> (histogram.size ());
00248   return (norm);
00249 }
00250 
00252 template <typename PointInT, typename PointNT, typename PointOutT> boost::uint32_t
00253 pcl::GFPFHEstimation<PointInT, PointNT, PointOutT>::getDominantLabel (const std::vector<int>& indices)
00254 {
00255   std::vector<uint32_t> counts (getNumberOfClasses () + 1, 0);
00256   for (size_t i = 0; i < indices.size (); ++i)
00257   {
00258     uint32_t label = labels_->points[indices[i]].label;
00259     counts[label] += 1;
00260   }
00261 
00262   std::vector<uint32_t>::const_iterator max_it;
00263   max_it = std::max_element (counts.begin (), counts.end ());
00264   if (max_it == counts.end ())
00265     return (emptyLabel ());
00266 
00267   return (static_cast<uint32_t> (max_it - counts.begin ()));
00268 }
00269 
00270 #define PCL_INSTANTIATE_GFPFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::GFPFHEstimation<T,NT,OutT>;
00271 
00272 #endif    // PCL_FEATURES_IMPL_GFPFH_H_


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:24:25