Point Cloud Library (PCL)  1.3.1
intensity_spin.hpp
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: intensity_spin.hpp 1370 2011-06-19 01:06:01Z jspricke $
00035  *
00036  */
00037 
00038 #ifndef PCL_FEATURES_IMPL_INTENSITY_SPIN_H_
00039 #define PCL_FEATURES_IMPL_INTENSITY_SPIN_H_
00040 
00041 #include "pcl/features/intensity_spin.h"
00042 
00044 template <typename PointInT, typename PointOutT> void
00045 pcl::IntensitySpinEstimation<PointInT, PointOutT>::computeIntensitySpinImage (
00046       const PointCloudIn &cloud, float radius, float sigma, 
00047       int k,
00048       const std::vector<int> &indices, 
00049       const std::vector<float> &squared_distances, Eigen::MatrixXf &intensity_spin_image)
00050 {
00051   // Determine the number of bins to use based on the size of intensity_spin_image
00052   int nr_distance_bins = intensity_spin_image.cols ();
00053   int nr_intensity_bins = intensity_spin_image.rows ();
00054 
00055   // Find the min and max intensity values in the given neighborhood
00056   float min_intensity = std::numeric_limits<float>::max ();
00057   float max_intensity = std::numeric_limits<float>::min ();
00058   for (int idx = 0; idx < k; ++idx)
00059   {
00060     min_intensity = (std::min) (min_intensity, cloud.points[indices[idx]].intensity);
00061     max_intensity = (std::max) (max_intensity, cloud.points[indices[idx]].intensity);
00062   }
00063 
00064   // Compute the intensity spin image
00065   intensity_spin_image.setZero ();
00066   for (int idx = 0; idx < k; ++idx)
00067   {
00068     // Normalize distance and intensity values to: 0.0 <= d,i < nr_distance_bins,nr_intensity_bins
00069     const float eps = std::numeric_limits<float>::epsilon ();
00070     float d = nr_distance_bins * sqrt (squared_distances[idx]) / (radius + eps);
00071     float i = nr_intensity_bins * 
00072       (cloud.points[indices[idx]].intensity - min_intensity) / (max_intensity - min_intensity + eps);
00073 
00074     if (sigma == 0)
00075     {
00076       // If sigma is zero, update the histogram with no smoothing kernel
00077       int d_idx = (int) d;
00078       int i_idx = (int) i;
00079       intensity_spin_image (i_idx, d_idx) += 1;
00080     }
00081     else
00082     {
00083       // Compute the bin indices that need to be updated (+/- 3 standard deviations)
00084       int d_idx_min = (std::max)((int) floor (d - 3*sigma), 0);
00085       int d_idx_max = (std::min)((int) ceil  (d + 3*sigma), nr_distance_bins - 1);
00086       int i_idx_min = (std::max)((int) floor (i - 3*sigma), 0);
00087       int i_idx_max = (std::min)((int) ceil  (i + 3*sigma), nr_intensity_bins - 1);
00088     
00089       // Update the appropriate bins of the histogram 
00090       for (int i_idx = i_idx_min; i_idx <= i_idx_max; ++i_idx)  
00091       {
00092         for (int d_idx = d_idx_min; d_idx <= d_idx_max; ++d_idx)
00093         {
00094           // Compute a "soft" update weight based on the distance between the point and the bin
00095           float w = exp (-pow (d - d_idx, 2) / (2.0*sigma_*sigma_) 
00096                          -pow (i - i_idx, 2) / (2.0*sigma_*sigma_));
00097           intensity_spin_image (i_idx, d_idx) += w;
00098         }
00099       }
00100     }
00101   }
00102 }
00103 
00105 template <typename PointInT, typename PointOutT> void
00106 pcl::IntensitySpinEstimation<PointInT, PointOutT>::computeFeature (PointCloudOut &output)
00107 {
00108   // Make sure a search radius is set
00109   if (search_radius_ == 0.0)
00110   {
00111     PCL_ERROR ("[pcl::%s::computeFeature] The search radius must be set before computing the feature!\n",
00112                getClassName ().c_str ());
00113     output.width = output.height = 0;
00114     output.points.clear ();
00115     return;
00116   }
00117 
00118   // Make sure the spin image has valid dimensions
00119   if (nr_intensity_bins_ <= 0)
00120   {
00121     PCL_ERROR ("[pcl::%s::computeFeature] The number of intensity bins must be greater than zero!\n",
00122                getClassName ().c_str ());
00123     output.width = output.height = 0;
00124     output.points.clear ();
00125     return;
00126   }
00127   if (nr_distance_bins_ <= 0)
00128   {
00129     PCL_ERROR ("[pcl::%s::computeFeature] The number of distance bins must be greater than zero!\n",
00130                getClassName ().c_str ());
00131     output.width = output.height = 0;
00132     output.points.clear ();
00133     return;
00134   }
00135 
00136   Eigen::MatrixXf intensity_spin_image (nr_intensity_bins_, nr_distance_bins_);
00137   // Allocate enough space to hold the radiusSearch results
00138   std::vector<int> nn_indices (surface_->points.size ());
00139   std::vector<float> nn_dist_sqr (surface_->points.size ());
00140  
00141   // Iterating over the entire index vector
00142   for (size_t idx = 0; idx < indices_->size (); ++idx)
00143   {
00144     // Find neighbors within the search radius
00145     // TODO: do we want to use searchForNeigbors instead?
00146     int k = tree_->radiusSearch ((*indices_)[idx], search_radius_, nn_indices, nn_dist_sqr);
00147 
00148     // Compute the intensity spin image
00149     computeIntensitySpinImage (*surface_, search_radius_, sigma_, k, nn_indices, nn_dist_sqr, intensity_spin_image);
00150 
00151     // Copy into the resultant cloud
00152     for (int bin = 0; bin < intensity_spin_image.size (); ++bin)
00153       output.points[idx].histogram[bin] = intensity_spin_image (bin);
00154   }
00155 }
00156 
00157 #define PCL_INSTANTIATE_IntensitySpinEstimation(T,NT) template class PCL_EXPORTS pcl::IntensitySpinEstimation<T,NT>;
00158 
00159 #endif    // PCL_FEATURES_IMPL_INTENSITY_SPIN_H_ 
00160 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines