Point Cloud Library (PCL)  1.3.1
spin_image.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   *
00035   */
00036 
00037 #ifndef PCL_FEATURES_IMPL_SPIN_IMAGE_H_
00038 #define PCL_FEATURES_IMPL_SPIN_IMAGE_H_
00039 
00040 #include <limits>
00041 
00042 #include "pcl/point_cloud.h"
00043 #include "pcl/point_types.h"
00044 #include "pcl/exceptions.h"
00045 #include "pcl/kdtree/kdtree_flann.h"
00046 #include "pcl/features/spin_image.h"
00047 
00048 
00049 using namespace pcl;
00050 
00051 template <typename PointInT, typename PointNT, typename PointOutT>
00052 const double SpinImageEstimation<PointInT, PointNT, PointOutT>::PI = 4.0 * std::atan2(1.0, 1.0);
00053 
00054 
00056 template <typename PointInT, typename PointNT, typename PointOutT>
00057 SpinImageEstimation<PointInT, PointNT, PointOutT>::SpinImageEstimation (
00058   unsigned int image_width, double support_angle_cos, unsigned int min_pts_neighb):
00059     is_angular_(false), use_custom_axis_(false), use_custom_axes_cloud_(false), 
00060     is_radial_(false),
00061     image_width_(image_width), support_angle_cos_(support_angle_cos), min_pts_neighb_(min_pts_neighb)
00062 {
00063   assert (support_angle_cos_ <= 1.0 && support_angle_cos_ >= 0.0); // may be permit negative cosine?
00064 
00065   feature_name_ = "SpinImageEstimation";
00066 }
00067 
00068 
00070 template <typename PointInT, typename PointNT, typename PointOutT> Eigen::ArrayXXd 
00071 SpinImageEstimation<PointInT, PointNT, PointOutT>::computeSiForPoint (int index) const
00072 {
00073   assert (image_width_ > 0);
00074   assert (support_angle_cos_ <= 1.0 && support_angle_cos_ >= 0.0); // may be permit negative cosine?
00075 
00076   const Eigen::Vector3f origin_point (input_->points[index].getVector3fMap ());
00077 
00078   Eigen::Vector3f origin_normal;
00079   origin_normal = 
00080     input_normals_ ? 
00081       input_normals_->points[index].getNormalVector3fMap () :
00082       Eigen::Vector3f (); // just a placeholder; should never be used!
00083 
00084   const Eigen::Vector3f rotation_axis = use_custom_axis_ ? 
00085     rotation_axis_.getNormalVector3fMap () : 
00086     use_custom_axes_cloud_ ?
00087       rotation_axes_cloud_->points[index].getNormalVector3fMap () :
00088       origin_normal;  
00089 
00090   Eigen::ArrayXXd m_matrix (Eigen::ArrayXXd::Zero (image_width_+1, 2*image_width_+1));
00091   Eigen::ArrayXXd m_averAngles (Eigen::ArrayXXd::Zero (image_width_+1, 2*image_width_+1));
00092 
00093   // OK, we are interested in the points of the cylinder of height 2*r and
00094   // base radius r, where r = m_dBinSize * in_iImageWidth
00095   // it can be embedded to the sphere of radius sqrt(2) * m_dBinSize * in_iImageWidth
00096   // suppose that points are uniformly distributed, so we lose ~40%
00097   // according to the volumes ratio
00098   double bin_size = 0.0;
00099   if (is_radial_)
00100   {
00101     bin_size = search_radius_ / image_width_;  
00102   }
00103   else
00104   {
00105     bin_size = search_radius_ / image_width_ / sqrt(2.0);
00106   }
00107 
00108   std::vector<int> nn_indices;
00109   std::vector<float> nn_sqr_dists;
00110   const int neighb_cnt = searchForNeighbors (index, search_radius_, nn_indices, nn_sqr_dists);
00111   if (neighb_cnt < (int)min_pts_neighb_)
00112   {
00113     throw PCLException (
00114       "Too few points for spin image, use setMinPointCountInNeighbourhood() to decrease the threshold or use larger feature radius",
00115       "spin_image.hpp", "computeSiForPoint");
00116   }
00117 
00118   // for all neighbor points
00119   for (int i_neigh = 0; i_neigh < neighb_cnt ; i_neigh++)
00120   {
00121     // first, skip the points with distant normals
00122     double cos_between_normals = -2.0; // should be initialized if used
00123     if (support_angle_cos_ > 0.0 || is_angular_) // not bogus
00124     {
00125       cos_between_normals = origin_normal.dot (
00126         normals_->points[nn_indices[i_neigh]].getNormalVector3fMap ());
00127       if (fabs(cos_between_normals) > (1.0 + 10*std::numeric_limits<float>::epsilon())) // should be okay for numeric stability
00128       {      
00129         PCL_ERROR ("[pcl::%s::computeSiForPoint] Normal for the point %d and/or the point %d are not normalized, dot ptoduct is %f.\n", 
00130           getClassName ().c_str (), nn_indices[i_neigh], index, cos_between_normals);
00131         throw PCLException ("Some normals are not normalized",
00132           "spin_image.hpp", "computeSiForPoint");
00133       }
00134       cos_between_normals = std::max (-1.0, std::min (1.0, cos_between_normals));
00135 
00136       if (fabs (cos_between_normals) < support_angle_cos_ )    // allow counter-directed normals
00137       {
00138         continue;
00139       }
00140 
00141       if (cos_between_normals < 0.0)
00142       {
00143         cos_between_normals = -cos_between_normals; // the normal is not used explicitly from now
00144       }
00145     }
00146     
00147     // now compute the coordinate in cylindric coordinate system associated with the origin point
00148     const Eigen::Vector3f direction (
00149       surface_->points[nn_indices[i_neigh]].getVector3fMap () - origin_point);
00150     const double direction_norm = direction.norm ();
00151     if (fabs(direction_norm) < 10*std::numeric_limits<double>::epsilon ())  
00152       continue;  // ignore the point itself; it does not contribute really
00153     assert (direction_norm > 0.0);
00154 
00155     // the angle between the normal vector and the direction to the point
00156     double cos_dir_axis = direction.dot(rotation_axis) / direction_norm;
00157     if (fabs(cos_dir_axis) > (1.0 + 10*std::numeric_limits<float>::epsilon())) // should be okay for numeric stability
00158     {      
00159       PCL_ERROR ("[pcl::%s::computeSiForPoint] Rotation axis for the point %d are not normalized, dot ptoduct is %f.\n", 
00160         getClassName ().c_str (), index, cos_dir_axis);
00161       throw PCLException ("Some rotation axis is not normalized",
00162         "spin_image.hpp", "computeSiForPoint");
00163     }
00164     cos_dir_axis = std::max (-1.0, std::min (1.0, cos_dir_axis));
00165 
00166     // compute coordinates w.r.t. the reference frame
00167     double beta = std::numeric_limits<double>::signaling_NaN ();
00168     double alpha = std::numeric_limits<double>::signaling_NaN ();
00169     if (is_radial_) // radial spin image structure
00170     {
00171       beta = asin (cos_dir_axis);  // yes, arc sine! to get the angle against tangent, not normal!
00172       alpha = direction_norm;
00173     }
00174     else // rectangular spin-image structure
00175     {
00176       beta = direction_norm * cos_dir_axis;
00177       alpha = direction_norm * sqrt (1.0 - cos_dir_axis*cos_dir_axis);
00178 
00179       if (fabs (beta) >= bin_size * image_width_ || alpha >= bin_size * image_width_)
00180       {
00181         continue;  // outside the cylinder
00182       }
00183     }
00184 
00185     assert (alpha >= 0.0);
00186     assert (alpha <= bin_size * image_width_ + 20 * std::numeric_limits<float>::epsilon () );
00187 
00188 
00189     // bilinear interpolation
00190     double beta_bin_size = is_radial_ ? (PI / 2 / image_width_) : bin_size;
00191     int beta_bin = round (beta / beta_bin_size) + int(image_width_);
00192     assert (0 <= beta_bin && beta_bin < m_matrix.cols ());
00193     int alpha_bin = round (alpha / bin_size);
00194     assert (0 <= alpha_bin && alpha_bin < m_matrix.rows ());
00195 
00196     if (alpha_bin == (int)image_width_)  // border points
00197     {
00198       alpha_bin--;
00199       // HACK: to prevent a > 1
00200       alpha = bin_size * (alpha_bin + 1) - std::numeric_limits<double>::epsilon ();
00201     }
00202     if (beta_bin == int(2*image_width_) )  // border points
00203     {
00204       beta_bin--;
00205       // HACK: to prevent b > 1
00206       beta = beta_bin_size * (beta_bin - int(image_width_) + 1) - std::numeric_limits<double>::epsilon ();
00207     }
00208 
00209     double a = alpha/bin_size - double(alpha_bin);
00210     double b = beta/beta_bin_size - double(beta_bin-int(image_width_)); 
00211 
00212     assert (0 <= a && a <= 1);
00213     assert (0 <= b && b <= 1);
00214 
00215     m_matrix(alpha_bin, beta_bin) += (1-a) * (1-b);
00216     m_matrix(alpha_bin+1, beta_bin) += a * (1-b);
00217     m_matrix(alpha_bin, beta_bin+1) += (1-a) * b;
00218     m_matrix(alpha_bin+1, beta_bin+1) += a * b;
00219 
00220     if (is_angular_)
00221     {
00222       m_averAngles(alpha_bin, beta_bin) += (1-a) * (1-b) * acos (cos_between_normals); 
00223       m_averAngles(alpha_bin+1, beta_bin) += a * (1-b) * acos (cos_between_normals);
00224       m_averAngles(alpha_bin, beta_bin+1) += (1-a) * b * acos (cos_between_normals);
00225       m_averAngles(alpha_bin+1, beta_bin+1) += a * b * acos (cos_between_normals);
00226     }
00227   }
00228 
00229   if (is_angular_)
00230   {
00231     // transform sum to average
00232     m_matrix = m_averAngles / (m_matrix + std::numeric_limits<double>::epsilon ()); // +eps to avoid division by zero
00233   }
00234   else if (neighb_cnt > 1) // to avoid division by zero, also no need to divide by 1
00235   {
00236     // normalization
00237     m_matrix /= m_matrix.sum();
00238   }
00239 
00240   return m_matrix;
00241 }
00242 
00243 
00245 template <typename PointInT, typename PointNT, typename PointOutT> bool 
00246 pcl::SpinImageEstimation<PointInT, PointNT, PointOutT>::initCompute ()
00247 {
00248   // If the surface won't be set, make fake surface and fake surface normals
00249   // if we wouldn't do it here, the following method would alarm that no surface normals is given
00250   if (!surface_)
00251   {
00252     surface_ = input_;
00253     normals_ = input_normals_;
00254     fake_surface_ = true;
00255   }
00256 
00257   if (!FeatureFromNormals<PointInT, PointNT, PointOutT>::initCompute ())
00258   {
00259     PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
00260     return (false);
00261   }
00262 
00263   if (fake_surface_ && !input_normals_)
00264   {
00265     input_normals_ = normals_; // normals_ is set, as checked earlier
00266   }
00267   
00268 
00269   assert(!(use_custom_axis_ && use_custom_axes_cloud_));
00270 
00271   if (!use_custom_axis_ && !use_custom_axes_cloud_ // use input normals as rotation axes
00272     && !input_normals_)
00273   {
00274     PCL_ERROR ("[pcl::%s::initCompute] No normals for input cloud were given!\n", getClassName ().c_str ());
00275     // Cleanup
00276     FeatureFromNormals<PointInT, PointNT, PointOutT>::deinitCompute ();
00277     return (false);
00278   }
00279 
00280   if ((is_angular_ || support_angle_cos_ > 0.0) // support angle is not bogus NOTE this is for randomly-flipped normals
00281     && !input_normals_)
00282   {
00283     PCL_ERROR ("[pcl::%s::initCompute] No normals for input cloud were given!\n", getClassName ().c_str ());
00284     // Cleanup
00285     FeatureFromNormals<PointInT, PointNT, PointOutT>::deinitCompute ();
00286     return (false);
00287   }
00288 
00289   if (use_custom_axes_cloud_ 
00290     && rotation_axes_cloud_->size () == input_->size ())
00291   {
00292     PCL_ERROR ("[pcl::%s::initCompute] Rotation axis cloud have different size from input!\n", getClassName ().c_str ());
00293     // Cleanup
00294     FeatureFromNormals<PointInT, PointNT, PointOutT>::deinitCompute ();
00295     return (false);
00296   }
00297 
00298   return true;
00299 }
00300 
00301 
00303 template <typename PointInT, typename PointNT, typename PointOutT> void 
00304 SpinImageEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
00305 { 
00306   for (int i_input = 0; i_input < (int)indices_->size (); ++i_input)
00307   {
00308     Eigen::ArrayXXd res = computeSiForPoint (indices_->at(i_input));
00309 
00310     // Copy into the resultant cloud
00311     for (int iRow = 0; iRow < res.rows () ; iRow++)
00312     {
00313       for (int iCol = 0; iCol < res.cols () ; iCol++)
00314       {
00315         output.points[i_input].histogram[ iRow*res.cols () + iCol ] = (float)res(iRow, iCol);
00316       }
00317     }   
00318   } 
00319 }
00320 
00321 
00322 #define PCL_INSTANTIATE_SpinImageEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::SpinImageEstimation<T,NT,OutT>;
00323 
00324 #endif    // PCL_FEATURES_IMPL_SPIN_IMAGE_H_
00325 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines