Point Cloud Library (PCL)  1.3.1
3dsc.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: 3dsc.hpp 3023 2011-11-01 03:42:32Z svn $
00035   *
00036   */
00037 
00038 #ifndef PCL_FEATURES_IMPL_3DSC_HPP_
00039 #define PCL_FEATURES_IMPL_3DSC_HPP_
00040 
00041 #include <pcl/features/3dsc.h>
00042 #include <pcl/common/utils.h>
00043 #include <pcl/common/geometry.h>
00044 #include <pcl/common/angles.h>
00045 
00046 template <typename PointInT, typename PointNT, typename PointOutT> bool
00047 pcl::ShapeContext3DEstimation<PointInT, PointNT, PointOutT>::initCompute()
00048 {
00049   if (!FeatureFromNormals<PointInT, PointNT, 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 PointNT, typename PointOutT> void
00124 pcl::ShapeContext3DEstimation<PointInT, PointNT, PointOutT>::computePoint(size_t index, const pcl::PointCloud<PointInT> &input, const pcl::PointCloud<PointNT> &normals, float rf[9], std::vector<float> &desc)
00125 {
00127   Eigen::Map<Eigen::Vector3f> x_axis (rf);
00128   Eigen::Map<Eigen::Vector3f> y_axis (rf + 3);
00129   Eigen::Map<Eigen::Vector3f> normal (rf + 6);
00130 
00132   Vector3fMapConst origin = input[(*indices_)[index]].getVector3fMap ();
00135   normal = normals[(*indices_)[index]].getNormalVector3fMap ();
00136 
00138   if(!pcl::utils::equal(normal[2], 0.0f))
00139   {
00140     x_axis[0] = (float)rand() / ((float)RAND_MAX + 1);
00141     x_axis[1] = (float)rand() / ((float)RAND_MAX + 1);
00142     x_axis[2] = - (normal[0]*x_axis[0] + normal[1]*x_axis[1]) / normal[2];
00143   }
00144   else if(!pcl::utils::equal(normal[1], 0.0f))
00145   {
00146     x_axis[0] = (float)rand() / ((float)RAND_MAX + 1);
00147     x_axis[2] = (float)rand() / ((float)RAND_MAX + 1);
00148     x_axis[1] = - (normal[0]*x_axis[0] + normal[2]*x_axis[2]) / normal[1];
00149   }
00150   else if(!pcl::utils::equal(normal[0], 0.0f))
00151   {
00152     x_axis[1] = (float)rand() / ((float)RAND_MAX + 1);
00153     x_axis[2] = (float)rand() / ((float)RAND_MAX + 1);
00154     x_axis[0] = - (normal[1]*x_axis[1] + normal[2]*x_axis[2]) / normal[0];
00155   }
00156 
00157   x_axis.normalize();
00158 
00160   assert(pcl::utils::equal(x_axis[0]*normal[0] + x_axis[1]*normal[1] + x_axis[2]*normal[2], 0.0f, 1E-6f));
00161 
00163   y_axis = normal.cross (x_axis);
00165   std::vector<int> nn_indices;
00166   std::vector<float> nn_dists;
00167   const size_t neighb_cnt = searchForNeighbors (index, search_radius_, nn_indices, nn_dists);
00169   for(size_t ne = 0; ne < neighb_cnt; ne++)
00170   {
00171     if(nn_indices[ne] == (*indices_)[index])
00172       continue;
00174     Eigen::Vector3f neighbour = input[nn_indices[ne]].getVector3fMap ();
00175 
00177     
00179     float r = sqrt (nn_dists[ne]); 
00180     
00182     Eigen::Vector3f proj;
00183     pcl::geometry::project(neighbour, origin, normal, proj);
00184     proj-= origin;
00185 
00187     proj.normalize ();
00188     
00190     Eigen::Vector3f cross = x_axis.cross(proj);
00191     float phi = rad2deg (std::atan2(cross.norm (), x_axis.dot (proj)));
00192     phi = cross.dot (normal) < 0.f ? (360.0 - phi) : phi;
00194     Eigen::Vector3f no = neighbour - origin;
00195     no.normalize ();
00196     float theta = normal.dot (no);
00197     theta = rad2deg (acos(std::min(1.0f, std::max( -1.0f, theta))));
00198 
00200     size_t j = 0;
00201     size_t k = 0;
00202     size_t l = 0;
00203 
00205     for(size_t rad = 1; rad < radius_bins_+1; rad++) {
00206       if(r <= radii_interval_[rad]) {
00207         j = rad-1;
00208         break;
00209       }
00210     }
00211 
00212     for(size_t ang = 1; ang < elevation_bins_+1; ang++) {
00213       if(theta <= theta_divisions_[ang]) {
00214         k = ang-1;
00215         break;
00216       }
00217     }
00218 
00219     for(size_t ang = 1; ang < azimuth_bins_+1; ang++) {
00220       if(phi <= phi_divisions_[ang]) {
00221         l = ang-1;
00222         break;
00223       }
00224     }
00225 
00227     std::vector<int> neighbour_indices;
00228     std::vector<float> neighbour_didtances;
00229     float point_density = (float) searchForNeighbors (nn_indices[ne], point_density_radius_, neighbour_indices, neighbour_didtances);
00231     float w = (1.0 / point_density) * volume_lut_[ (l*elevation_bins_*radius_bins_) + 
00232                                                  (k*radius_bins_) + 
00233                                                  j ];
00234       
00235     assert(w >= 0.0);
00236     if(w == std::numeric_limits<float>::infinity())
00237       PCL_ERROR("Shape Context Error INF!\n");
00238     if(w != w)
00239       PCL_ERROR("Shape Context Error IND!\n");
00241     desc[ (l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j ] += w;
00242 
00243   assert(desc[ (l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j ] >= 0);
00244   } // end for each neighbour
00245 }
00246 
00247 template <typename PointInT, typename PointNT, typename PointOutT> void
00248 pcl::ShapeContext3DEstimation<PointInT, PointNT, PointOutT>::shiftAlongAzimuth(size_t block_size, std::vector<float>& desc)
00249 {
00250   assert(desc.size () == descriptor_length_);
00251   // L rotations for each descriptor
00252   desc.resize(descriptor_length_ * azimuth_bins_); 
00253   // Create L azimuth rotated descriptors from reference descriptor
00254   // The descriptor_length_ first ones are the same so start at 1
00255   for(size_t l = 1; l < azimuth_bins_; l++)
00256     for(size_t bin = 0; bin < descriptor_length_; bin++)
00257       desc[(l * descriptor_length_) + bin] = desc[(l*block_size + bin) % descriptor_length_];
00258 }
00259 
00260 template <typename PointInT, typename PointNT, typename PointOutT> void
00261 pcl::ShapeContext3DEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
00262 {
00263   if(!shift_)
00264   {
00265     for(size_t point_index = 0; point_index < indices_->size (); point_index++)
00266     {
00267       output[point_index].descriptor.resize (descriptor_length_);
00268       computePoint(point_index, *input_, *normals_, output[point_index].rf, output[point_index].descriptor);
00269     }
00270   }
00271   else
00272   {
00273     size_t block_size = descriptor_length_ / azimuth_bins_; // Size of each L-block
00274     for(size_t point_index = 0; point_index < indices_->size (); point_index++)
00275     {
00276       output[point_index].descriptor.resize (descriptor_length_);
00277       computePoint(point_index, *input_, *normals_, output[point_index].rf, output[point_index].descriptor);
00278       shiftAlongAzimuth(block_size, output[point_index].descriptor);
00279     }
00280   }
00281 }
00282 
00283 #define PCL_INSTANTIATE_ShapeContext3DEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::ShapeContext3DEstimation<T,NT,OutT>;
00284 
00285 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines