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


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:32:10