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 1370 2011-06-19 01:06:01Z jspricke $
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> inline void
00046 pcl::computeRSD (const pcl::PointCloud<PointInT> &surface, const pcl::PointCloud<PointNT> &normals,
00047      const std::vector<int> &indices, double max_dist,
00048      int nr_subdiv, double plane_radius, PointOutT &radii)
00049 {
00050   // Initialize minimum and maximum angle values in each distance bin
00051   std::vector<std::vector<double> > min_max_angle_by_dist (nr_subdiv);
00052   min_max_angle_by_dist[0].resize (2);
00053   min_max_angle_by_dist[0][0] = min_max_angle_by_dist[0][1] = 0.0;
00054   for (int di=1; di<nr_subdiv; di++)
00055   {
00056     min_max_angle_by_dist[di].resize (2);
00057     min_max_angle_by_dist[di][0] = +DBL_MAX;
00058     min_max_angle_by_dist[di][1] = -DBL_MAX;
00059   }
00060 
00061   // Compute distance by normal angle distribution for points
00062   std::vector<int>::const_iterator i, begin (indices.begin()), end (indices.end());
00063   for(i = begin+1; i != end; ++i)
00064   {
00065     // compute angle between the two lines going through normals (disregard orientation!)
00066     double cosine = normals.points[*i].normal[0] * normals.points[*begin].normal[0] +
00067                     normals.points[*i].normal[1] * normals.points[*begin].normal[1] +
00068                     normals.points[*i].normal[2] * normals.points[*begin].normal[2];
00069     if (cosine > 1) cosine = 1;
00070     if (cosine < -1) cosine = -1;
00071     double angle  = acos (cosine);
00072     if (angle > M_PI/2) angle = M_PI - angle; 
00073 
00074     // Compute point to point distance
00075     double dist = sqrt ((surface.points[*i].x - surface.points[*begin].x) * (surface.points[*i].x - surface.points[*begin].x) +
00076                         (surface.points[*i].y - surface.points[*begin].y) * (surface.points[*i].y - surface.points[*begin].y) +
00077                         (surface.points[*i].z - surface.points[*begin].z) * (surface.points[*i].z - surface.points[*begin].z));
00078 
00079     if (dist > max_dist)
00080       continue; 
00081 
00082     // compute bins and increase
00083     int bin_d = (int) floor (nr_subdiv * dist / max_dist);
00084 
00085     // update min-max values for distance bins
00086     if (min_max_angle_by_dist[bin_d][0] > angle) min_max_angle_by_dist[bin_d][0] = angle;
00087     if (min_max_angle_by_dist[bin_d][1] < angle) min_max_angle_by_dist[bin_d][1] = angle;
00088   }
00089 
00090   // Estimate radius from min and max lines
00091   double Amint_Amin = 0, Amint_d = 0;
00092   double Amaxt_Amax = 0, Amaxt_d = 0;
00093   for (int di=0; di<nr_subdiv; di++)
00094   {
00095     // combute the members of A'*A*r = A'*D
00096     if (min_max_angle_by_dist[di][1] >= 0)
00097     {
00098       double p_min = min_max_angle_by_dist[di][0];
00099       double p_max = min_max_angle_by_dist[di][1];
00100       //cerr << p_min << " " << p_max << endl;
00101       double f = (di+0.5)*max_dist/nr_subdiv;
00102       //cerr << f << endl;
00103       Amint_Amin += p_min * p_min;
00104       Amint_d += p_min * f;
00105       Amaxt_Amax += p_max * p_max;
00106       Amaxt_d += p_max * f;
00107     }
00108   }
00109   radii.r_max = Amint_Amin == 0 ? plane_radius : std::min (Amint_d/Amint_Amin, plane_radius);
00110   radii.r_min = Amaxt_Amax == 0 ? plane_radius : std::min (Amaxt_d/Amaxt_Amax, plane_radius);
00111 }
00112 
00114 template <typename PointInT, typename PointNT, typename PointOutT> void
00115 pcl::RSDEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
00116 {
00117   // Check if input was set
00118   if (!normals_)
00119   {
00120     PCL_ERROR ("[pcl::%s::computeFeature] No input dataset containing normals was given!\n", getClassName ().c_str ());
00121     output.width = output.height = 0;
00122     output.points.clear ();
00123     return;
00124   }
00125   if (normals_->points.size () != surface_->points.size ())
00126   {
00127     PCL_ERROR ("[pcl::%s::computeFeature] The number of points in the input dataset differs from the number of points in the dataset containing the normals!\n", getClassName ().c_str ());
00128     output.width = output.height = 0;
00129     output.points.clear ();
00130     return;
00131   }
00132 
00133   // Check if search_radius_ was set
00134   if (search_radius_ < 0)
00135   {
00136     PCL_ERROR ("[pcl::%s::computeFeature] A search radius needs to be set!\n", getClassName ().c_str ());
00137     output.width = output.height = 0;
00138     output.points.clear ();
00139     return;
00140   }
00141 
00142   // Allocate enough space to hold the results
00143   // \note resize is irrelevant for a radiusSearch ().
00144   std::vector<int> nn_indices;
00145   std::vector<float> nn_sqr_dists;
00146 
00147   // Iterating over the entire index vector
00148   for (size_t idx = 0; idx < indices_->size (); ++idx)
00149   {
00150     // Compute and store r_min and r_max in the output cloud
00151     this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_sqr_dists);
00152     computeRSD (*surface_, *normals_, nn_indices, search_radius_, nr_subdiv_, plane_radius_, output.points[idx]);
00153   }
00154 }
00155 
00156 #define PCL_INSTANTIATE_RSDEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::RSDEstimation<T,NT,OutT>;
00157 
00158 #endif    // PCL_FEATURES_IMPL_VFH_H_