Point Cloud Library (PCL)  1.3.1
usc.hpp
Go to the documentation of this file.
00001 /*
00002   * Software License Agreement (BSD License)
00003   *
00004   *  Copyright (c) 2011, www.pointclouds.org
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: usc.hpp 3275 2011-11-30 16:59:04Z mdixon $
00035   *
00036   */
00037 
00038 #ifndef PCL_FEATURES_IMPL_USC_HPP_
00039 #define PCL_FEATURES_IMPL_USC_HPP_
00040 
00041 #include <pcl/features/usc.h>
00042 #include <pcl/features/shot_common.h>
00043 #include <pcl/common/geometry.h>
00044 #include <pcl/common/angles.h>
00045 
00046 template <typename PointInT, typename PointOutT> bool
00047 pcl::UniqueShapeContext<PointInT, PointOutT>::initCompute()
00048 {
00049   if (!Feature<PointInT, PointOutT>::initCompute ())
00050   {
00051     PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
00052     return (false);
00053   }
00054 
00055   if( search_radius_< min_radius_)
00056   {
00057     PCL_ERROR ("[pcl::%s::initCompute] search_radius_ must be GREATER than min_radius_.\n", getClassName ().c_str ());
00058     return (false);
00059   }
00060 
00061   // Update descriptor length
00062   descriptor_length_ = elevation_bins_ * azimuth_bins_ * radius_bins_;
00063 
00064   // Compute radial, elevation and azimuth divisions
00065   float azimuth_interval = (float) 360 / azimuth_bins_;
00066   float elevation_interval = (float) 180 / elevation_bins_;
00067 
00068   // Reallocate divisions and volume lut
00069   radii_interval_.clear ();
00070   phi_divisions_.clear ();
00071   theta_divisions_.clear ();
00072   volume_lut_.clear ();
00073 
00074   // Fills radii interval based on formula (1) in section 2.1 of Frome's paper
00075   radii_interval_.resize(radius_bins_+1);
00076   for(size_t j = 0; j < radius_bins_+1; j++)
00077     radii_interval_[j] = exp (log (min_radius_) + (((float) j / (radius_bins_)) * log (search_radius_/min_radius_)));
00078   // Fill theta didvisions of elevation
00079   theta_divisions_.resize (elevation_bins_+1);
00080   for(size_t k = 0; k < elevation_bins_+1; k++)
00081     theta_divisions_[k] = k*elevation_interval;
00082   // Fill phi didvisions of elevation
00083   phi_divisions_.resize (azimuth_bins_+1);
00084   for(size_t l = 0; l < azimuth_bins_+1; l++)
00085     phi_divisions_[l] = l*azimuth_interval;
00086 
00087   // LookUp Table that contains the volume of all the bins
00088   // "phi" term of the volume integral
00089   // "integr_phi" has always the same value so we compute it only one time
00090   float integr_phi  = pcl::deg2rad(phi_divisions_[1]) - pcl::deg2rad(phi_divisions_[0]);
00091   // exponential to compute the cube root using pow
00092   float e = 1.0 / 3.0;    
00093   // Resize volume look up table
00094   volume_lut_.resize (radius_bins_*elevation_bins_*azimuth_bins_);
00095   // Fill volumes look up table
00096   for(size_t j = 0; j < radius_bins_; j++)
00097   {
00098     // "r" term of the volume integral
00099     float integr_r = (radii_interval_[j+1]*radii_interval_[j+1]*radii_interval_[j+1] / 3) - (radii_interval_[j]*radii_interval_[j]*radii_interval_[j]/ 3);
00100     
00101     for(size_t k = 0; k < elevation_bins_; k++)
00102     {
00103       // "theta" term of the volume integral
00104       float integr_theta = cos (deg2rad (theta_divisions_[k])) - cos (deg2rad (theta_divisions_[k+1]));
00105       // Volume
00106       float V = integr_phi * integr_theta * integr_r;
00107       // Compute cube root of the computed volume commented for performance but left 
00108       // here for clarity
00109       // float cbrt = pow(V, e);
00110       // cbrt = 1 / cbrt;
00111       
00112       for(size_t l = 0; l < azimuth_bins_; l++)
00113       {
00114         // Store in lut 1/cbrt
00115         //volume_lut_[ (l*elevation_bins_*radius_bins_) + k*radius_bins_ + j ] = cbrt;
00116         volume_lut_[ (l*elevation_bins_*radius_bins_) + k*radius_bins_ + j ] = 1 / pow(V,e);
00117       }
00118     }
00119   }
00120   return (true);
00121 }
00122 
00123 template <typename PointInT, typename PointOutT> void
00124 pcl::UniqueShapeContext<PointInT, PointOutT>::computePointRF(size_t index, float rf[9])
00125 {
00126   std::vector<int> nn_indices;
00127   std::vector<float> nn_dists;
00128   size_t nb_neighbours = searchForNeighbors ((*indices_)[index], local_radius_, nn_indices, nn_dists);
00129   std::cout << "nb of neighbors " << nb_neighbours << std::endl;
00131   if (nb_neighbours < 5)
00132   {
00133     PCL_WARN ("[pcl::%s::computePointRF] Neighborhood has %d vertices which is less than 5, aborting description of point index %d\n!", getClassName ().c_str (), nb_neighbours, index);
00134     return;
00135   }
00136   std::vector<Eigen::Vector4f, Eigen::aligned_allocator<Eigen::Vector4f> > rf_(3);
00137   Eigen::Vector4f central_point = input_->points[(*indices_)[index]].getVector4fMap ();
00138   central_point[3] = 0;
00139   pcl::getLocalRF(*surface_, local_radius_, central_point /*(*indices_)[index]*/, nn_indices, nn_dists, rf_);
00140   for (int d = 0; d < 9; ++d)
00141     rf[d] = rf_[d/3][d%3];
00142 }
00143 
00144 template <typename PointInT, typename PointOutT> void
00145 pcl::UniqueShapeContext<PointInT, PointOutT>::computePointDescriptor(size_t index, float rf[9], std::vector<float> &desc)
00146 {
00147   pcl::Vector3fMapConst origin = input_->points[(*indices_)[index]].getVector3fMap ();
00148   const Eigen::Map<Eigen::Vector3f> x_axis (rf);
00149   const Eigen::Map<Eigen::Vector3f> y_axis (rf + 3);
00150   const Eigen::Map<Eigen::Vector3f> normal (rf + 6);
00152   std::vector<int> nn_indices;
00153   std::vector<float> nn_dists;
00154   const size_t neighb_cnt = searchForNeighbors (index, search_radius_, nn_indices, nn_dists);
00156   for(size_t ne = 0; ne < neighb_cnt; ne++)
00157   {
00158     if(nn_indices[ne] == (*indices_)[index])
00159       continue;
00161     Eigen::Vector3f neighbour = surface_->points[nn_indices[ne]].getVector3fMap ();
00162 
00164     
00166     float r = sqrt (nn_dists[ne]); 
00167     
00169     Eigen::Vector3f proj;
00170     pcl::geometry::project(neighbour, origin, normal, proj);
00171     proj-= origin;
00172 
00174     proj.normalize ();
00175     
00177     Eigen::Vector3f cross = x_axis.cross(proj);
00178     float phi = rad2deg (std::atan2(cross.norm (), x_axis.dot (proj)));
00179     phi = cross.dot (normal) < 0.f ? (360.0 - phi) : phi;
00181     Eigen::Vector3f no = neighbour - origin;
00182     no.normalize ();
00183     float theta = normal.dot (no);
00184     theta = rad2deg (acos(std::min(1.0f, std::max( -1.0f, theta))));
00185 
00187     size_t j = 0;
00188     size_t k = 0;
00189     size_t l = 0;
00190 
00192     for(size_t rad = 1; rad < radius_bins_+1; rad++) {
00193       if(r <= radii_interval_[rad]) {
00194         j = rad-1;
00195         break;
00196       }
00197     }
00198 
00199     for(size_t ang = 1; ang < elevation_bins_+1; ang++) {
00200       if(theta <= theta_divisions_[ang]) {
00201         k = ang-1;
00202         break;
00203       }
00204     }
00205 
00206     for(size_t ang = 1; ang < azimuth_bins_+1; ang++) {
00207       if(phi <= phi_divisions_[ang]) {
00208         l = ang-1;
00209         break;
00210       }
00211     }
00212 
00214     std::vector<int> neighbour_indices;
00215     std::vector<float> neighbour_didtances;
00216     float point_density = (float) searchForNeighbors (nn_indices[ne], point_density_radius_, neighbour_indices, neighbour_didtances);
00218     float w = (1.0 / point_density) * volume_lut_[ (l*elevation_bins_*radius_bins_) + 
00219                                                  (k*radius_bins_) + 
00220                                                  j ];
00221       
00222     assert(w >= 0.0);
00223     if(w == std::numeric_limits<float>::infinity())
00224       PCL_ERROR("Shape Context Error INF!\n");
00225     if(w != w)
00226       PCL_ERROR("Shape Context Error IND!\n");
00228     desc[ (l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j ] += w;
00229 
00230     assert(desc[ (l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j ] >= 0);
00231   } // end for each neighbour
00232 }
00233 
00234 template <typename PointInT, typename PointOutT> void
00235 pcl::UniqueShapeContext<PointInT, PointOutT>::computeFeature (PointCloudOut &output)
00236 {
00237   for(size_t point_index = 0; point_index < indices_->size (); point_index++)
00238   {
00239     output[point_index].descriptor.resize (descriptor_length_);
00240     computePointRF(point_index, output[point_index].rf);
00241     computePointDescriptor(point_index, output[point_index].rf, output[point_index].descriptor);
00242   }
00243 }
00244 
00245 #define PCL_INSTANTIATE_UniqueShapeContext(T,OutT) template class PCL_EXPORTS pcl::UniqueShapeContext<T,OutT>;
00246 
00247 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines