Point Cloud Library (PCL)  1.3.1
fpfh.hpp
Go to the documentation of this file.
00001 /*
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2009, 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: fpfh.hpp 2713 2011-10-11 21:06:28Z rusu $
00035 *
00036 */
00037 
00038 #ifndef PCL_FEATURES_IMPL_FPFH_H_
00039 #define PCL_FEATURES_IMPL_FPFH_H_
00040 
00041 #include "pcl/features/fpfh.h"
00042 #include <pcl/features/pfh.h>
00043 
00045 template <typename PointInT, typename PointNT, typename PointOutT> bool
00046 pcl::FPFHEstimation<PointInT, PointNT, PointOutT>::computePairFeatures (
00047     const pcl::PointCloud<PointInT> &cloud, const pcl::PointCloud<PointNT> &normals,
00048     int p_idx, int q_idx, float &f1, float &f2, float &f3, float &f4)
00049 {
00050     pcl::computePairFeatures (cloud.points[p_idx].getVector4fMap (), normals.points[p_idx].getNormalVector4fMap (),
00051         cloud.points[q_idx].getVector4fMap (), normals.points[q_idx].getNormalVector4fMap (),
00052         f1, f2, f3, f4);
00053     return (true);
00054 }
00055 
00057 template <typename PointInT, typename PointNT, typename PointOutT> 
00058 void pcl::FPFHEstimation<PointInT, PointNT, PointOutT>::
00059 computePointSPFHSignature (const pcl::PointCloud<PointInT> &cloud, const pcl::PointCloud<PointNT> &normals,
00060                            int p_idx, int row, const std::vector<int> &indices,
00061                            Eigen::MatrixXf &hist_f1, Eigen::MatrixXf &hist_f2, Eigen::MatrixXf &hist_f3)
00062 {
00063   Eigen::Vector4f pfh_tuple;
00064   // Get the number of bins from the histograms size
00065   int nr_bins_f1 = hist_f1.cols ();
00066   int nr_bins_f2 = hist_f2.cols ();
00067   int nr_bins_f3 = hist_f3.cols ();
00068 
00069   // Factorization constant
00070   float hist_incr = 100.0 / (float)(indices.size () - 1);
00071 
00072   // Iterate over all the points in the neighborhood
00073   for (size_t idx = 0; idx < indices.size (); ++idx)
00074   {
00075     // Avoid unnecessary returns
00076     if (p_idx == indices[idx])
00077         continue;
00078 
00079     // Compute the pair P to NNi
00080     if (!computePairFeatures (cloud, normals, p_idx, indices[idx], pfh_tuple[0], pfh_tuple[1], pfh_tuple[2], pfh_tuple[3]))
00081         continue;
00082 
00083     // Normalize the f1, f2, f3 features and push them in the histogram
00084     int h_index = floor (nr_bins_f1 * ((pfh_tuple[0] + M_PI) * d_pi_));
00085     if (h_index < 0)           h_index = 0;
00086     if (h_index >= nr_bins_f1) h_index = nr_bins_f1 - 1;
00087     hist_f1 (row, h_index) += hist_incr;
00088 
00089     h_index = floor (nr_bins_f2 * ((pfh_tuple[1] + 1.0) * 0.5));
00090     if (h_index < 0)           h_index = 0;
00091     if (h_index >= nr_bins_f2) h_index = nr_bins_f2 - 1;
00092     hist_f2 (row, h_index) += hist_incr;
00093 
00094     h_index = floor (nr_bins_f3 * ((pfh_tuple[2] + 1.0) * 0.5));
00095     if (h_index < 0)           h_index = 0;
00096     if (h_index >= nr_bins_f3) h_index = nr_bins_f3 - 1;
00097     hist_f3 (row, h_index) += hist_incr;
00098   }
00099 }
00100 
00102 template <typename PointInT, typename PointNT, typename PointOutT> void
00103 pcl::FPFHEstimation<PointInT, PointNT, PointOutT>::weightPointSPFHSignature (
00104     const Eigen::MatrixXf &hist_f1, const Eigen::MatrixXf &hist_f2, const Eigen::MatrixXf &hist_f3,
00105     const std::vector<int> &indices, const std::vector<float> &dists, Eigen::VectorXf &fpfh_histogram)
00106 {
00107     assert (indices.size () == dists.size ());
00108     double sum_f1 = 0.0, sum_f2 = 0.0, sum_f3 = 0.0;
00109     float weight = 0.0, val_f1, val_f2, val_f3;
00110 
00111     // Get the number of bins from the histograms size
00112     int nr_bins_f1 = hist_f1.cols ();
00113     int nr_bins_f2 = hist_f2.cols ();
00114     int nr_bins_f3 = hist_f3.cols ();
00115     int nr_bins_f12 = nr_bins_f1 + nr_bins_f2;
00116 
00117     // Use the entire patch
00118     for (size_t idx = 0, data_size = indices.size (); idx < data_size; ++idx)
00119     {
00120         // Minus the query point itself
00121         if (dists[idx] == 0)
00122             continue;
00123 
00124         // Standard weighting function used
00125         weight = 1.0 / dists[idx];
00126 
00127         // Weight the SPFH of the query point with the SPFH of its neighbors
00128         for (int f1_i = 0; f1_i < nr_bins_f1; ++f1_i)
00129         {
00130             val_f1 = hist_f1 (indices[idx], f1_i) * weight;
00131             sum_f1 += val_f1;
00132             fpfh_histogram[f1_i] += val_f1;
00133         }
00134 
00135         for (int f2_i = 0; f2_i < nr_bins_f2; ++f2_i)
00136         {
00137             val_f2 = hist_f2 (indices[idx], f2_i) * weight;
00138             sum_f2 += val_f2;
00139             fpfh_histogram[f2_i + nr_bins_f1] += val_f2;
00140         }
00141 
00142         for (int f3_i = 0; f3_i < nr_bins_f3; ++f3_i)
00143         {
00144             val_f3 = hist_f3 (indices[idx], f3_i) * weight;
00145             sum_f3 += val_f3;
00146             fpfh_histogram[f3_i + nr_bins_f12] += val_f3;
00147         }
00148     }
00149 
00150     if (sum_f1 != 0)
00151         sum_f1 = 100.0 / sum_f1;           // histogram values sum up to 100
00152     if (sum_f2 != 0)
00153         sum_f2 = 100.0 / sum_f2;           // histogram values sum up to 100
00154     if (sum_f3 != 0)
00155         sum_f3 = 100.0 / sum_f3;           // histogram values sum up to 100
00156 
00157     // Adjust final FPFH values
00158     for (int f1_i = 0; f1_i < nr_bins_f1; ++f1_i)
00159         fpfh_histogram[f1_i] *= sum_f1;
00160     for (int f2_i = 0; f2_i < nr_bins_f2; ++f2_i)
00161         fpfh_histogram[f2_i + nr_bins_f1] *= sum_f2;
00162     for (int f3_i = 0; f3_i < nr_bins_f3; ++f3_i)
00163         fpfh_histogram[f3_i + nr_bins_f12] *= sum_f3;
00164 }
00165 
00167 template <typename PointInT, typename PointNT, typename PointOutT> void
00168 pcl::FPFHEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
00169 {
00170     // Allocate enough space to hold the NN search results
00171     // \note This resize is irrelevant for a radiusSearch ().
00172     std::vector<int> nn_indices (k_);
00173     std::vector<float> nn_dists (k_);
00174 
00175     std::set<int> spfh_indices;
00176     std::vector<int> spfh_hist_lookup (surface_->points.size ());
00177 
00178     // Build a list of (unique) indices for which we will need to compute SPFH signatures
00179     // (We need an SPFH signature for every point that is a neighbor of any point in input_[indices_])
00180     if (surface_ != input_ ||
00181         indices_->size () != surface_->points.size ())
00182     { 
00183         for (size_t idx = 0; idx < indices_->size (); ++idx)
00184         {
00185             int p_idx = (*indices_)[idx];
00186             this->searchForNeighbors (p_idx, search_parameter_, nn_indices, nn_dists);
00187 
00188             spfh_indices.insert (nn_indices.begin (), nn_indices.end ());
00189         }
00190     }
00191     else
00192     {
00193         // Special case: When a feature must be computed at every point, there is no need for a neighborhood search
00194         for (size_t idx = 0; idx < indices_->size (); ++idx)
00195             spfh_indices.insert ((int) idx);
00196     }
00197 
00198     // Initialize the arrays that will store the SPFH signatures
00199     size_t data_size = spfh_indices.size ();
00200     hist_f1_.setZero (data_size, nr_bins_f1_);
00201     hist_f2_.setZero (data_size, nr_bins_f2_);
00202     hist_f3_.setZero (data_size, nr_bins_f3_);
00203 
00204     // Compute SPFH signatures for every point that needs them
00205     std::set<int>::iterator spfh_indices_itr = spfh_indices.begin ();
00206     for (size_t i = 0; i < spfh_indices.size (); ++i)
00207     {
00208         // Get the next point index
00209         int p_idx = *spfh_indices_itr;
00210         ++spfh_indices_itr;
00211 
00212         // Find the neighborhood around p_idx
00213         this->searchForNeighbors (*surface_, p_idx, search_parameter_, nn_indices, nn_dists);
00214 
00215         // Estimate the SPFH signature around p_idx
00216         computePointSPFHSignature (*surface_, *normals_, p_idx, i, nn_indices, hist_f1_, hist_f2_, hist_f3_);
00217 
00218         // Populate a lookup table for converting a point index to its corresponding row in the spfh_hist_* matrices
00219         spfh_hist_lookup[p_idx] = i;
00220     }
00221 
00222     // Intialize the array that will store the FPFH signature
00223     int nr_bins = nr_bins_f1_ + nr_bins_f2_ + nr_bins_f3_;
00224     fpfh_histogram_.setZero (nr_bins);
00225 
00226     // Iterate over the entire index vector
00227     for (size_t idx = 0; idx < indices_->size (); ++idx)
00228     {
00229         // Find the indices of point idx's neighbors... 
00230         this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists);
00231 
00232         // ... and remap the nn_indices values so that they represent row indices in the spfh_hist_* matrices 
00233         // instead of indices into surface_->points
00234         for (size_t i = 0; i < nn_indices.size (); ++i)
00235             nn_indices[i] = spfh_hist_lookup[nn_indices[i]];
00236 
00237         // Compute the FPFH signature (i.e. compute a weighted combination of local SPFH signatures) ...
00238         weightPointSPFHSignature (hist_f1_, hist_f2_, hist_f3_, nn_indices, nn_dists, fpfh_histogram_);
00239 
00240         // ...and copy it into the output cloud
00241         for (int d = 0; d < fpfh_histogram_.size (); ++d)
00242             output.points[idx].histogram[d] = fpfh_histogram_[d];
00243 
00244         fpfh_histogram_.setZero ();
00245     }
00246 
00247 }
00248 
00249 #define PCL_INSTANTIATE_FPFHEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::FPFHEstimation<T,NT,OutT>;
00250 
00251 #endif    // PCL_FEATURES_IMPL_FPFH_H_ 
00252 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines