Point Cloud Library (PCL)  1.3.1
rift.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: rift.hpp 1370 2011-06-19 01:06:01Z jspricke $
00035  *
00036  */
00037 
00038 #ifndef PCL_FEATURES_IMPL_RIFT_H_
00039 #define PCL_FEATURES_IMPL_RIFT_H_
00040 
00041 #include "pcl/features/rift.h"
00042 
00044 template <typename PointInT, typename GradientT, typename PointOutT> void
00045 pcl::RIFTEstimation<PointInT, GradientT, PointOutT>::computeRIFT (
00046       const PointCloudIn &cloud, const PointCloudGradient &gradient, 
00047       int p_idx, float radius, const std::vector<int> &indices, 
00048       const std::vector<float> &sqr_distances, Eigen::MatrixXf &rift_descriptor)
00049 {
00050   if (indices.empty ())
00051   {
00052     PCL_ERROR ("[pcl::RIFTEstimation] Null indices points passed!\n");
00053     return;
00054   }
00055 
00056   // Determine the number of bins to use based on the size of rift_descriptor
00057   int nr_distance_bins = rift_descriptor.cols ();
00058   int nr_gradient_bins = rift_descriptor.rows ();
00059 
00060   // Get the center point
00061   pcl::Vector3fMapConst p0 = cloud.points[p_idx].getVector3fMap ();
00062 
00063   // Compute the RIFT descriptor
00064   rift_descriptor.setZero ();
00065   for (size_t idx = 0; idx < indices.size (); ++idx)
00066   {
00067     // Compute the gradient magnitude and orientation (relative to the center point)
00069     pcl::Vector3fMapConst point = cloud.points[indices[idx]].getVector3fMap ();
00070     Eigen::Map<const Eigen::Vector3f> gradient_vector (& (gradient.points[indices[idx]].gradient[0]));
00071 
00072     float gradient_magnitude = gradient_vector.norm ();
00073     float gradient_angle_from_center = acos (gradient_vector.dot ((point - p0).normalized ()) / gradient_magnitude);
00074     if (!pcl_isfinite (gradient_angle_from_center))
00075     {
00076       gradient_angle_from_center = 0.0;
00077     }
00078 
00079     // Normalize distance and angle values to: 0.0 <= d,g < nr_distances_bins,nr_gradient_bins
00080     const float eps = std::numeric_limits<float>::epsilon ();
00081     float d = nr_distance_bins * sqrt (sqr_distances[idx]) / (radius + eps);
00082     float g = nr_gradient_bins * gradient_angle_from_center / (M_PI + eps);
00083 
00084     // Compute the bin indices that need to be updated
00085     int d_idx_min = (std::max)((int) ceil (d - 1), 0);
00086     int d_idx_max = (std::min)((int) floor (d + 1), nr_distance_bins - 1);
00087     int g_idx_min = (int) ceil (g - 1);
00088     int g_idx_max = (int) floor (g + 1);
00089 
00090     // Update the appropriate bins of the histogram 
00091     for (int g_idx = g_idx_min; g_idx <= g_idx_max; ++g_idx)  
00092     {
00093       // Because gradient orientation is cyclical, out-of-bounds values must wrap around
00094       int g_idx_wrapped = ((g_idx + nr_gradient_bins) % nr_gradient_bins); 
00095 
00096       for (int d_idx = d_idx_min; d_idx <= d_idx_max; ++d_idx)
00097       {
00098         // To avoid boundary effects, use linear interpolation when updating each bin 
00099         float w = (1 - fabs (d - d_idx)) * (1 - fabs (g - g_idx));
00100 
00101         rift_descriptor (g_idx_wrapped * nr_distance_bins + d_idx) += w * gradient_magnitude;
00102       }
00103     }
00104   }
00105 
00106   // Normalize the RIFT descriptor to unit magnitude
00107   rift_descriptor.normalize ();
00108 }
00109 
00110 
00112 template <typename PointInT, typename GradientT, typename PointOutT> void
00113 pcl::RIFTEstimation<PointInT, GradientT, PointOutT>::computeFeature (PointCloudOut &output)
00114 {
00115   // Make sure a search radius is set
00116   if (search_radius_ == 0.0)
00117   {
00118     PCL_ERROR ("[pcl::%s::computeFeature] The search radius must be set before computing the feature!\n",
00119                getClassName ().c_str ());
00120     output.width = output.height = 0;
00121     output.points.clear ();
00122     return;
00123   }
00124 
00125   // Make sure the RIFT descriptor has valid dimensions
00126   if (nr_gradient_bins_ <= 0)
00127   {
00128     PCL_ERROR ("[pcl::%s::computeFeature] The number of gradient bins must be greater than zero!\n",
00129                getClassName ().c_str ());
00130     output.width = output.height = 0;
00131     output.points.clear ();
00132     return;
00133   }
00134   if (nr_distance_bins_ <= 0)
00135   {
00136     PCL_ERROR ("[pcl::%s::computeFeature] The number of distance bins must be greater than zero!\n",
00137                getClassName ().c_str ());
00138     output.width = output.height = 0;
00139     output.points.clear ();
00140     return;
00141   }
00142 
00143   // Check for valid input gradient
00144   if (!gradient_)
00145   {
00146     PCL_ERROR ("[pcl::%s::computeFeature] No input gradient was given!\n", getClassName ().c_str ());
00147     output.width = output.height = 0;
00148     output.points.clear ();
00149     return;
00150   }
00151   if (gradient_->points.size () != surface_->points.size ())
00152   {
00153     PCL_ERROR ("[pcl::%s::computeFeature] The number of points in the input dataset differs from the number of points in the gradient!\n", getClassName ().c_str ());
00154     output.width = output.height = 0;
00155     output.points.clear ();
00156     return;
00157   }
00158 
00159   Eigen::MatrixXf rift_descriptor (nr_gradient_bins_, nr_distance_bins_);
00160   std::vector<int> nn_indices;
00161   std::vector<float> nn_dist_sqr;
00162  
00163   // Iterating over the entire index vector
00164   for (size_t idx = 0; idx < indices_->size (); ++idx)
00165   {
00166     // Find neighbors within the search radius
00167     tree_->radiusSearch ((*indices_)[idx], search_radius_, nn_indices, nn_dist_sqr);
00168 
00169     // Compute the RIFT descriptor
00170     computeRIFT (*surface_, *gradient_, (*indices_)[idx], search_radius_, nn_indices, nn_dist_sqr, rift_descriptor);
00171 
00172     // Copy into the resultant cloud
00173     for (int bin = 0; bin < rift_descriptor.size (); ++bin)
00174       output.points[idx].histogram[bin] = rift_descriptor (bin);
00175   }
00176 }
00177 
00178 #define PCL_INSTANTIATE_RIFTEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::RIFTEstimation<T,NT,OutT>;
00179 
00180 #endif    // PCL_FEATURES_IMPL_RIFT_H_ 
00181 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines