rsd.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: rsd.hpp 5026 2012-03-12 02:51:44Z rusu $
00035  *
00036  */
00037 
00038 #ifndef PCL_FEATURES_IMPL_RSD_H_
00039 #define PCL_FEATURES_IMPL_RSD_H_
00040 
00041 #include <cfloat>
00042 #include <pcl/features/rsd.h>
00043 
00045 template <typename PointInT, typename PointNT, typename PointOutT> Eigen::MatrixXf
00046 pcl::computeRSD (boost::shared_ptr<const pcl::PointCloud<PointInT> > &surface, boost::shared_ptr<const pcl::PointCloud<PointNT> > &normals,
00047                  const std::vector<int> &indices, double max_dist,
00048                  int nr_subdiv, double plane_radius, PointOutT &radii, bool compute_histogram)
00049 {
00050   // Check if the full histogram has to be saved or not
00051   Eigen::MatrixXf histogram;
00052   if (compute_histogram)
00053     histogram = Eigen::MatrixXf::Zero (nr_subdiv, nr_subdiv);
00054 
00055   // Check if enough points are provided or not
00056   if (indices.size () < 2)
00057   {
00058     radii.r_max = 0;
00059     radii.r_min = 0;
00060     return histogram;
00061   }
00062   
00063   // Initialize minimum and maximum angle values in each distance bin
00064   std::vector<std::vector<double> > min_max_angle_by_dist (nr_subdiv);
00065   min_max_angle_by_dist[0].resize (2);
00066   min_max_angle_by_dist[0][0] = min_max_angle_by_dist[0][1] = 0.0;
00067   for (int di=1; di<nr_subdiv; di++)
00068   {
00069     min_max_angle_by_dist[di].resize (2);
00070     min_max_angle_by_dist[di][0] = +DBL_MAX;
00071     min_max_angle_by_dist[di][1] = -DBL_MAX;
00072   }
00073 
00074   // Compute distance by normal angle distribution for points
00075   std::vector<int>::const_iterator i, begin (indices.begin()), end (indices.end());
00076   for(i = begin+1; i != end; ++i)
00077   {
00078     // compute angle between the two lines going through normals (disregard orientation!)
00079     double cosine = normals->points[*i].normal[0] * normals->points[*begin].normal[0] +
00080                     normals->points[*i].normal[1] * normals->points[*begin].normal[1] +
00081                     normals->points[*i].normal[2] * normals->points[*begin].normal[2];
00082     if (cosine > 1) cosine = 1;
00083     if (cosine < -1) cosine = -1;
00084     double angle  = acos (cosine);
00085     if (angle > M_PI/2) angle = M_PI - angle; 
00086 
00087     // Compute point to point distance
00088     double dist = sqrt ((surface->points[*i].x - surface->points[*begin].x) * (surface->points[*i].x - surface->points[*begin].x) +
00089                         (surface->points[*i].y - surface->points[*begin].y) * (surface->points[*i].y - surface->points[*begin].y) +
00090                         (surface->points[*i].z - surface->points[*begin].z) * (surface->points[*i].z - surface->points[*begin].z));
00091 
00092     if (dist > max_dist)
00093       continue; 
00094 
00095     // compute bins and increase
00096     int bin_d = (int) floor (nr_subdiv * dist / max_dist);
00097     if (compute_histogram)
00098     {
00099       int bin_a = std::min (nr_subdiv-1, (int) floor (nr_subdiv * angle / (M_PI/2)));
00100       histogram(bin_a, bin_d)++;
00101     }
00102 
00103     // update min-max values for distance bins
00104     if (min_max_angle_by_dist[bin_d][0] > angle) min_max_angle_by_dist[bin_d][0] = angle;
00105     if (min_max_angle_by_dist[bin_d][1] < angle) min_max_angle_by_dist[bin_d][1] = angle;
00106   }
00107 
00108   // Estimate radius from min and max lines
00109   double Amint_Amin = 0, Amint_d = 0;
00110   double Amaxt_Amax = 0, Amaxt_d = 0;
00111   for (int di=0; di<nr_subdiv; di++)
00112   {
00113     // combute the members of A'*A*r = A'*D
00114     if (min_max_angle_by_dist[di][1] >= 0)
00115     {
00116       double p_min = min_max_angle_by_dist[di][0];
00117       double p_max = min_max_angle_by_dist[di][1];
00118       double f = (di+0.5)*max_dist/nr_subdiv;
00119       Amint_Amin += p_min * p_min;
00120       Amint_d += p_min * f;
00121       Amaxt_Amax += p_max * p_max;
00122       Amaxt_d += p_max * f;
00123     }
00124   }
00125   float min_radius = Amint_Amin == 0 ? plane_radius : std::min (Amint_d/Amint_Amin, plane_radius);
00126   float max_radius = Amaxt_Amax == 0 ? plane_radius : std::min (Amaxt_d/Amaxt_Amax, plane_radius);
00127 
00128   // Small correction of the systematic error of the estimation (based on analysis with nr_subdiv_ = 5)
00129   min_radius *= 1.1;
00130   max_radius *= 0.9;
00131   if (min_radius < max_radius)
00132   {
00133     radii.r_min = min_radius;
00134     radii.r_max = max_radius;
00135   }
00136   else
00137   {
00138     radii.r_max = min_radius;
00139     radii.r_min = max_radius;
00140   }
00141   
00142   return histogram;
00143 }
00144 
00146 template <typename PointNT, typename PointOutT> Eigen::MatrixXf
00147 pcl::computeRSD (boost::shared_ptr<const pcl::PointCloud<PointNT> > &normals,
00148                  const std::vector<int> &indices, const std::vector<float> &sqr_dists, double max_dist,
00149                  int nr_subdiv, double plane_radius, PointOutT &radii, bool compute_histogram)
00150 {
00151   // Check if the full histogram has to be saved or not
00152   Eigen::MatrixXf histogram;
00153   if (compute_histogram)
00154     histogram = Eigen::MatrixXf::Zero (nr_subdiv, nr_subdiv);
00155   
00156   // Check if enough points are provided or not
00157   if (indices.size () < 2)
00158   {
00159     radii.r_max = 0;
00160     radii.r_min = 0;
00161     return histogram;
00162   }
00163   
00164   // Initialize minimum and maximum angle values in each distance bin
00165   std::vector<std::vector<double> > min_max_angle_by_dist (nr_subdiv);
00166   min_max_angle_by_dist[0].resize (2);
00167   min_max_angle_by_dist[0][0] = min_max_angle_by_dist[0][1] = 0.0;
00168   for (int di=1; di<nr_subdiv; di++)
00169   {
00170     min_max_angle_by_dist[di].resize (2);
00171     min_max_angle_by_dist[di][0] = +DBL_MAX;
00172     min_max_angle_by_dist[di][1] = -DBL_MAX;
00173   }
00174   
00175   // Compute distance by normal angle distribution for points
00176   std::vector<int>::const_iterator i, begin (indices.begin()), end (indices.end());
00177   for(i = begin+1; i != end; ++i)
00178   {
00179     // compute angle between the two lines going through normals (disregard orientation!)
00180     double cosine = normals->points[*i].normal[0] * normals->points[*begin].normal[0] +
00181                     normals->points[*i].normal[1] * normals->points[*begin].normal[1] +
00182                     normals->points[*i].normal[2] * normals->points[*begin].normal[2];
00183     if (cosine > 1) cosine = 1;
00184     if (cosine < -1) cosine = -1;
00185     double angle  = acos (cosine);
00186     if (angle > M_PI/2) angle = M_PI - angle; 
00187 
00188     // Compute point to point distance
00189     double dist = sqrt (sqr_dists[i-begin]);
00190 
00191     if (dist > max_dist)
00192       continue; 
00193 
00194     // compute bins and increase
00195     int bin_d = (int) floor (nr_subdiv * dist / max_dist);
00196     if (compute_histogram)
00197     {
00198       int bin_a = std::min (nr_subdiv-1, (int) floor (nr_subdiv * angle / (M_PI/2)));
00199       histogram(bin_a, bin_d)++;
00200     }
00201 
00202     // update min-max values for distance bins
00203     if (min_max_angle_by_dist[bin_d][0] > angle) min_max_angle_by_dist[bin_d][0] = angle;
00204     if (min_max_angle_by_dist[bin_d][1] < angle) min_max_angle_by_dist[bin_d][1] = angle;
00205   }
00206 
00207   // Estimate radius from min and max lines
00208   double Amint_Amin = 0, Amint_d = 0;
00209   double Amaxt_Amax = 0, Amaxt_d = 0;
00210   for (int di=0; di<nr_subdiv; di++)
00211   {
00212     // combute the members of A'*A*r = A'*D
00213     if (min_max_angle_by_dist[di][1] >= 0)
00214     {
00215       double p_min = min_max_angle_by_dist[di][0];
00216       double p_max = min_max_angle_by_dist[di][1];
00217       double f = (di+0.5)*max_dist/nr_subdiv;
00218       Amint_Amin += p_min * p_min;
00219       Amint_d += p_min * f;
00220       Amaxt_Amax += p_max * p_max;
00221       Amaxt_d += p_max * f;
00222     }
00223   }
00224   float min_radius = Amint_Amin == 0 ? plane_radius : std::min (Amint_d/Amint_Amin, plane_radius);
00225   float max_radius = Amaxt_Amax == 0 ? plane_radius : std::min (Amaxt_d/Amaxt_Amax, plane_radius);
00226 
00227   // Small correction of the systematic error of the estimation (based on analysis with nr_subdiv_ = 5)
00228   min_radius *= 1.1;
00229   max_radius *= 0.9;
00230   if (min_radius < max_radius)
00231   {
00232     radii.r_min = min_radius;
00233     radii.r_max = max_radius;
00234   }
00235   else
00236   {
00237     radii.r_max = min_radius;
00238     radii.r_min = max_radius;
00239   }
00240   
00241   return histogram;
00242 }
00243 
00245 template <typename PointInT, typename PointNT, typename PointOutT> void
00246 pcl::RSDEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
00247 {
00248   // Check if search_radius_ was set
00249   if (search_radius_ < 0)
00250   {
00251     PCL_ERROR ("[pcl::%s::computeFeature] A search radius needs to be set!\n", getClassName ().c_str ());
00252     output.width = output.height = 0;
00253     output.points.clear ();
00254     return;
00255   }
00256 
00257   // List of indices and corresponding squared distances for a neighborhood
00258   // \note resize is irrelevant for a radiusSearch ().
00259   std::vector<int> nn_indices;
00260   std::vector<float> nn_sqr_dists;
00261 
00262   // Check if the full histogram has to be saved or not
00263   if (save_histograms_)
00264   {
00265     // Reserve space for the output histogram dataset
00266     histograms_.reset (new std::vector<Eigen::MatrixXf, Eigen::aligned_allocator<Eigen::MatrixXf> >);
00267     histograms_->reserve (output.points.size ());
00268     
00269     // Iterating over the entire index vector
00270     for (size_t idx = 0; idx < indices_->size (); ++idx)
00271     {
00272       // Compute and store r_min and r_max in the output cloud
00273       this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_sqr_dists);
00274       //histograms_->push_back (computeRSD (surface_, normals_, nn_indices, search_radius_, nr_subdiv_, plane_radius_, output.points[idx], true));
00275       histograms_->push_back (computeRSD (normals_, nn_indices, nn_sqr_dists, search_radius_, nr_subdiv_, plane_radius_, output.points[idx], true));
00276     }
00277   }
00278   else
00279   {
00280     // Iterating over the entire index vector
00281     for (size_t idx = 0; idx < indices_->size (); ++idx)
00282     {
00283       // Compute and store r_min and r_max in the output cloud
00284       this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_sqr_dists);
00285       //computeRSD (surface_, normals_, nn_indices, search_radius_, nr_subdiv_, plane_radius_, output.points[idx], false);
00286       computeRSD (normals_, nn_indices, nn_sqr_dists, search_radius_, nr_subdiv_, plane_radius_, output.points[idx], false);
00287     }
00288   }
00289 }
00290 
00291 #define PCL_INSTANTIATE_RSDEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::RSDEstimation<T,NT,OutT>;
00292 
00293 #endif    // PCL_FEATURES_IMPL_VFH_H_ 


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:17:42