spin_image.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) 2010-2012, Willow Garage, Inc.
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of Willow Garage, Inc. nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * $Id: spin_image.hpp 4961 2012-03-07 23:44:07Z rusu $
00037  *
00038  */
00039 
00040 #ifndef PCL_FEATURES_IMPL_SPIN_IMAGE_H_
00041 #define PCL_FEATURES_IMPL_SPIN_IMAGE_H_
00042 
00043 #include <limits>
00044 #include <pcl/point_cloud.h>
00045 #include <pcl/point_types.h>
00046 #include <pcl/exceptions.h>
00047 #include <pcl/kdtree/kdtree_flann.h>
00048 #include <pcl/features/spin_image.h>
00049 #include <cmath>
00050 
00052 template <typename PointInT, typename PointNT, typename PointOutT>
00053 pcl::SpinImageEstimation<PointInT, PointNT, PointOutT>::SpinImageEstimation (
00054   unsigned int image_width, double support_angle_cos, unsigned int min_pts_neighb) :
00055   input_normals_ (), rotation_axes_cloud_ (), 
00056   is_angular_ (false), rotation_axis_ (), use_custom_axis_(false), use_custom_axes_cloud_ (false), 
00057   is_radial_ (false), image_width_ (image_width), support_angle_cos_ (support_angle_cos), 
00058   min_pts_neighb_ (min_pts_neighb)
00059 {
00060   assert (support_angle_cos_ <= 1.0 && support_angle_cos_ >= 0.0); // may be permit negative cosine?
00061 
00062   feature_name_ = "SpinImageEstimation";
00063 }
00064 
00065 
00067 template <typename PointInT, typename PointNT, typename PointOutT> Eigen::ArrayXXd 
00068 pcl::SpinImageEstimation<PointInT, PointNT, PointOutT>::computeSiForPoint (int index) const
00069 {
00070   assert (image_width_ > 0);
00071   assert (support_angle_cos_ <= 1.0 && support_angle_cos_ >= 0.0); // may be permit negative cosine?
00072 
00073   const Eigen::Vector3f origin_point (input_->points[index].getVector3fMap ());
00074 
00075   Eigen::Vector3f origin_normal;
00076   origin_normal = 
00077     input_normals_ ? 
00078       input_normals_->points[index].getNormalVector3fMap () :
00079       Eigen::Vector3f (); // just a placeholder; should never be used!
00080 
00081   const Eigen::Vector3f rotation_axis = use_custom_axis_ ? 
00082     rotation_axis_.getNormalVector3fMap () : 
00083     use_custom_axes_cloud_ ?
00084       rotation_axes_cloud_->points[index].getNormalVector3fMap () :
00085       origin_normal;  
00086 
00087   Eigen::ArrayXXd m_matrix (Eigen::ArrayXXd::Zero (image_width_+1, 2*image_width_+1));
00088   Eigen::ArrayXXd m_averAngles (Eigen::ArrayXXd::Zero (image_width_+1, 2*image_width_+1));
00089 
00090   // OK, we are interested in the points of the cylinder of height 2*r and
00091   // base radius r, where r = m_dBinSize * in_iImageWidth
00092   // it can be embedded to the sphere of radius sqrt(2) * m_dBinSize * in_iImageWidth
00093   // suppose that points are uniformly distributed, so we lose ~40%
00094   // according to the volumes ratio
00095   double bin_size = 0.0;
00096   if (is_radial_)
00097     bin_size = search_radius_ / image_width_;  
00098   else
00099     bin_size = search_radius_ / image_width_ / sqrt(2.0);
00100 
00101   std::vector<int> nn_indices;
00102   std::vector<float> nn_sqr_dists;
00103   const int neighb_cnt = this->searchForNeighbors (index, search_radius_, nn_indices, nn_sqr_dists);
00104   if (neighb_cnt < static_cast<int> (min_pts_neighb_))
00105   {
00106     throw PCLException (
00107       "Too few points for spin image, use setMinPointCountInNeighbourhood() to decrease the threshold or use larger feature radius",
00108       "spin_image.hpp", "computeSiForPoint");
00109   }
00110 
00111   // for all neighbor points
00112   for (int i_neigh = 0; i_neigh < neighb_cnt ; i_neigh++)
00113   {
00114     // first, skip the points with distant normals
00115     double cos_between_normals = -2.0; // should be initialized if used
00116     if (support_angle_cos_ > 0.0 || is_angular_) // not bogus
00117     {
00118       cos_between_normals = origin_normal.dot (input_normals_->points[nn_indices[i_neigh]].getNormalVector3fMap ());
00119       if (fabs (cos_between_normals) > (1.0 + 10*std::numeric_limits<float>::epsilon ())) // should be okay for numeric stability
00120       {      
00121         PCL_ERROR ("[pcl::%s::computeSiForPoint] Normal for the point %d and/or the point %d are not normalized, dot ptoduct is %f.\n", 
00122           getClassName ().c_str (), nn_indices[i_neigh], index, cos_between_normals);
00123         throw PCLException ("Some normals are not normalized",
00124           "spin_image.hpp", "computeSiForPoint");
00125       }
00126       cos_between_normals = std::max (-1.0, std::min (1.0, cos_between_normals));
00127 
00128       if (fabs (cos_between_normals) < support_angle_cos_ )    // allow counter-directed normals
00129       {
00130         continue;
00131       }
00132 
00133       if (cos_between_normals < 0.0)
00134       {
00135         cos_between_normals = -cos_between_normals; // the normal is not used explicitly from now
00136       }
00137     }
00138     
00139     // now compute the coordinate in cylindric coordinate system associated with the origin point
00140     const Eigen::Vector3f direction (
00141       surface_->points[nn_indices[i_neigh]].getVector3fMap () - origin_point);
00142     const double direction_norm = direction.norm ();
00143     if (fabs(direction_norm) < 10*std::numeric_limits<double>::epsilon ())  
00144       continue;  // ignore the point itself; it does not contribute really
00145     assert (direction_norm > 0.0);
00146 
00147     // the angle between the normal vector and the direction to the point
00148     double cos_dir_axis = direction.dot(rotation_axis) / direction_norm;
00149     if (fabs(cos_dir_axis) > (1.0 + 10*std::numeric_limits<float>::epsilon())) // should be okay for numeric stability
00150     {      
00151       PCL_ERROR ("[pcl::%s::computeSiForPoint] Rotation axis for the point %d are not normalized, dot ptoduct is %f.\n", 
00152         getClassName ().c_str (), index, cos_dir_axis);
00153       throw PCLException ("Some rotation axis is not normalized",
00154         "spin_image.hpp", "computeSiForPoint");
00155     }
00156     cos_dir_axis = std::max (-1.0, std::min (1.0, cos_dir_axis));
00157 
00158     // compute coordinates w.r.t. the reference frame
00159     double beta = std::numeric_limits<double>::signaling_NaN ();
00160     double alpha = std::numeric_limits<double>::signaling_NaN ();
00161     if (is_radial_) // radial spin image structure
00162     {
00163             beta = asin (cos_dir_axis);  // yes, arc sine! to get the angle against tangent, not normal!
00164             alpha = direction_norm;
00165     }
00166     else // rectangular spin-image structure
00167     {
00168       beta = direction_norm * cos_dir_axis;
00169       alpha = direction_norm * sqrt (1.0 - cos_dir_axis*cos_dir_axis);
00170 
00171       if (fabs (beta) >= bin_size * image_width_ || alpha >= bin_size * image_width_)
00172       {
00173         continue;  // outside the cylinder
00174       }
00175     }
00176 
00177     assert (alpha >= 0.0);
00178     assert (alpha <= bin_size * image_width_ + 20 * std::numeric_limits<float>::epsilon () );
00179 
00180 
00181     // bilinear interpolation
00182     double beta_bin_size = is_radial_ ? (M_PI / 2 / image_width_) : bin_size;
00183     int beta_bin = int(std::floor (beta / beta_bin_size)) + int(image_width_);
00184     assert (0 <= beta_bin && beta_bin < m_matrix.cols ());
00185     int alpha_bin = int(std::floor (alpha / bin_size));
00186     assert (0 <= alpha_bin && alpha_bin < m_matrix.rows ());
00187 
00188     if (alpha_bin == static_cast<int> (image_width_))  // border points
00189     {
00190       alpha_bin--;
00191       // HACK: to prevent a > 1
00192       alpha = bin_size * (alpha_bin + 1) - std::numeric_limits<double>::epsilon ();
00193     }
00194     if (beta_bin == int(2*image_width_) )  // border points
00195     {
00196       beta_bin--;
00197       // HACK: to prevent b > 1
00198       beta = beta_bin_size * (beta_bin - int(image_width_) + 1) - std::numeric_limits<double>::epsilon ();
00199     }
00200 
00201     double a = alpha/bin_size - double(alpha_bin);
00202     double b = beta/beta_bin_size - double(beta_bin-int(image_width_)); 
00203 
00204     assert (0 <= a && a <= 1);
00205     assert (0 <= b && b <= 1);
00206 
00207     m_matrix (alpha_bin, beta_bin) += (1-a) * (1-b);
00208     m_matrix (alpha_bin+1, beta_bin) += a * (1-b);
00209     m_matrix (alpha_bin, beta_bin+1) += (1-a) * b;
00210     m_matrix (alpha_bin+1, beta_bin+1) += a * b;
00211 
00212     if (is_angular_)
00213     {
00214       m_averAngles (alpha_bin, beta_bin) += (1-a) * (1-b) * acos (cos_between_normals); 
00215       m_averAngles (alpha_bin+1, beta_bin) += a * (1-b) * acos (cos_between_normals);
00216       m_averAngles (alpha_bin, beta_bin+1) += (1-a) * b * acos (cos_between_normals);
00217       m_averAngles (alpha_bin+1, beta_bin+1) += a * b * acos (cos_between_normals);
00218     }
00219   }
00220 
00221   if (is_angular_)
00222   {
00223     // transform sum to average
00224     m_matrix = m_averAngles / (m_matrix + std::numeric_limits<double>::epsilon ()); // +eps to avoid division by zero
00225   }
00226   else if (neighb_cnt > 1) // to avoid division by zero, also no need to divide by 1
00227   {
00228     // normalization
00229     m_matrix /= m_matrix.sum();
00230   }
00231 
00232   return m_matrix;
00233 }
00234 
00235 
00237 template <typename PointInT, typename PointNT, typename PointOutT> bool 
00238 pcl::SpinImageEstimation<PointInT, PointNT, PointOutT>::initCompute ()
00239 {
00240   if (!Feature<PointInT, PointOutT>::initCompute ())
00241   {
00242     PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
00243     return (false);
00244   }
00245 
00246   // Check if input normals are set
00247   if (!input_normals_)
00248   {
00249     PCL_ERROR ("[pcl::%s::initCompute] No input dataset containing normals was given!\n", getClassName ().c_str ());
00250     Feature<PointInT, PointOutT>::deinitCompute ();
00251     return (false);
00252   }
00253 
00254   // Check if the size of normals is the same as the size of the surface
00255   if (input_normals_->points.size () != input_->points.size ())
00256   {
00257     PCL_ERROR ("[pcl::%s::initCompute] ", getClassName ().c_str ());
00258     PCL_ERROR ("The number of points in the input dataset differs from ");
00259     PCL_ERROR ("the number of points in the dataset containing the normals!\n");
00260     Feature<PointInT, PointOutT>::deinitCompute ();
00261     return (false);
00262   }
00263 
00264    // We need a positive definite search radius to continue
00265   if (search_radius_ == 0)
00266   {
00267     PCL_ERROR ("[pcl::%s::initCompute] Need a search radius different than 0!\n", getClassName ().c_str ());
00268     Feature<PointInT, PointOutT>::deinitCompute ();
00269     return (false);
00270   }
00271   if (k_ != 0)
00272   {
00273     PCL_ERROR ("[pcl::%s::initCompute] K-nearest neighbor search for spin images not implemented. Used a search radius instead!\n", getClassName ().c_str ());
00274     Feature<PointInT, PointOutT>::deinitCompute ();
00275     return (false);
00276   }
00277   // If the surface won't be set, make fake surface and fake surface normals
00278   // if we wouldn't do it here, the following method would alarm that no surface normals is given
00279   if (!surface_)
00280   {
00281     surface_ = input_;
00282     fake_surface_ = true;
00283   }
00284 
00285   //if (fake_surface_ && !input_normals_)
00286   //  input_normals_ = normals_; // normals_ is set, as checked earlier
00287   
00288   assert(!(use_custom_axis_ && use_custom_axes_cloud_));
00289 
00290   if (!use_custom_axis_ && !use_custom_axes_cloud_ // use input normals as rotation axes
00291     && !input_normals_)
00292   {
00293     PCL_ERROR ("[pcl::%s::initCompute] No normals for input cloud were given!\n", getClassName ().c_str ());
00294     // Cleanup
00295     Feature<PointInT, PointOutT>::deinitCompute ();
00296     return (false);
00297   }
00298 
00299   if ((is_angular_ || support_angle_cos_ > 0.0) // support angle is not bogus NOTE this is for randomly-flipped normals
00300     && !input_normals_)
00301   {
00302     PCL_ERROR ("[pcl::%s::initCompute] No normals for input cloud were given!\n", getClassName ().c_str ());
00303     // Cleanup
00304     Feature<PointInT, PointOutT>::deinitCompute ();
00305     return (false);
00306   }
00307 
00308   if (use_custom_axes_cloud_ 
00309     && rotation_axes_cloud_->size () == input_->size ())
00310   {
00311     PCL_ERROR ("[pcl::%s::initCompute] Rotation axis cloud have different size from input!\n", getClassName ().c_str ());
00312     // Cleanup
00313     Feature<PointInT, PointOutT>::deinitCompute ();
00314     return (false);
00315   }
00316 
00317   return (true);
00318 }
00319 
00320 
00322 template <typename PointInT, typename PointNT, typename PointOutT> void 
00323 pcl::SpinImageEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
00324 { 
00325   for (int i_input = 0; i_input < static_cast<int> (indices_->size ()); ++i_input)
00326   {
00327     Eigen::ArrayXXd res = computeSiForPoint (indices_->at (i_input));
00328 
00329     // Copy into the resultant cloud
00330     for (int iRow = 0; iRow < res.rows () ; iRow++)
00331     {
00332       for (int iCol = 0; iCol < res.cols () ; iCol++)
00333       {
00334         output.points[i_input].histogram[ iRow*res.cols () + iCol ] = static_cast<float> (res (iRow, iCol));
00335       }
00336     }   
00337   } 
00338 }
00339 
00341 template <typename PointInT, typename PointNT> void 
00342 pcl::SpinImageEstimation<PointInT, PointNT, Eigen::MatrixXf>::computeFeatureEigen (pcl::PointCloud<Eigen::MatrixXf> &output)
00343 { 
00344   // Set up the output channels
00345   output.channels["spin_image"].name     = "spin_image";
00346   output.channels["spin_image"].offset   = 0;
00347   output.channels["spin_image"].size     = 4;
00348   output.channels["spin_image"].count    = 153;
00349   output.channels["spin_image"].datatype = sensor_msgs::PointField::FLOAT32;
00350 
00351   output.points.resize (indices_->size (), 153);
00352   for (int i_input = 0; i_input < static_cast<int> (indices_->size ()); ++i_input)
00353   {
00354     Eigen::ArrayXXd res = this->computeSiForPoint (indices_->at (i_input));
00355 
00356     // Copy into the resultant cloud
00357     for (int iRow = 0; iRow < res.rows () ; iRow++)
00358     {
00359       for (int iCol = 0; iCol < res.cols () ; iCol++)
00360       {
00361         output.points (i_input, iRow*res.cols () + iCol) = static_cast<float> (res (iRow, iCol));
00362       }
00363     }   
00364   } 
00365 }
00366 
00367 
00368 #define PCL_INSTANTIATE_SpinImageEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::SpinImageEstimation<T,NT,OutT>;
00369 
00370 #endif    // PCL_FEATURES_IMPL_SPIN_IMAGE_H_
00371 


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:18:02