3dsc.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-2011, Willow Garage, Inc.
00006  *  Copyright (c) 2012-, Open Perception, Inc.
00007  *
00008  *  All rights reserved.
00009  *
00010  *  Redistribution and use in source and binary forms, with or without
00011  *  modification, are permitted provided that the following conditions
00012  *  are met:
00013  *
00014  *   * Redistributions of source code must retain the above copyright
00015  *     notice, this list of conditions and the following disclaimer.
00016  *   * Redistributions in binary form must reproduce the above
00017  *     copyright notice, this list of conditions and the following
00018  *     disclaimer in the documentation and/or other materials provided
00019  *     with the distribution.
00020  *   * Neither the name of the copyright holder(s) nor the names of its
00021  *     contributors may be used to endorse or promote products derived
00022  *     from this software without specific prior written permission.
00023  *
00024  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00025  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00026  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00027  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00028  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00029  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00030  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00031  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00032  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00033  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00034  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00035  *  POSSIBILITY OF SUCH DAMAGE.
00036  *
00037  */
00038 
00039 #ifndef PCL_FEATURES_IMPL_3DSC_HPP_
00040 #define PCL_FEATURES_IMPL_3DSC_HPP_
00041 
00042 #include <cmath>
00043 #include <pcl/features/3dsc.h>
00044 #include <pcl/common/utils.h>
00045 #include <pcl/common/geometry.h>
00046 #include <pcl/common/angles.h>
00047 
00049 template <typename PointInT, typename PointNT, typename PointOutT> bool
00050 pcl::ShapeContext3DEstimation<PointInT, PointNT, PointOutT>::initCompute ()
00051 {
00052   if (!FeatureFromNormals<PointInT, PointNT, PointOutT>::initCompute ())
00053   {
00054     PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
00055     return (false);
00056   }
00057 
00058   if (search_radius_< min_radius_)
00059   {
00060     PCL_ERROR ("[pcl::%s::initCompute] search_radius_ must be GREATER than min_radius_.\n", getClassName ().c_str ());
00061     return (false);
00062   }
00063 
00064   // Update descriptor length
00065   descriptor_length_ = elevation_bins_ * azimuth_bins_ * radius_bins_;
00066 
00067   // Compute radial, elevation and azimuth divisions
00068   float azimuth_interval = 360.0f / static_cast<float> (azimuth_bins_);
00069   float elevation_interval = 180.0f / static_cast<float> (elevation_bins_);
00070 
00071   // Reallocate divisions and volume lut
00072   radii_interval_.clear ();
00073   phi_divisions_.clear ();
00074   theta_divisions_.clear ();
00075   volume_lut_.clear ();
00076 
00077   // Fills radii interval based on formula (1) in section 2.1 of Frome's paper
00078   radii_interval_.resize (radius_bins_ + 1);
00079   for (size_t j = 0; j < radius_bins_ + 1; j++)
00080     radii_interval_[j] = static_cast<float> (exp (log (min_radius_) + ((static_cast<float> (j) / static_cast<float> (radius_bins_)) * log (search_radius_ / min_radius_))));
00081 
00082   // Fill theta divisions of elevation
00083   theta_divisions_.resize (elevation_bins_ + 1);
00084   for (size_t k = 0; k < elevation_bins_ + 1; k++)
00085     theta_divisions_[k] = static_cast<float> (k) * elevation_interval;
00086 
00087   // Fill phi didvisions of elevation
00088   phi_divisions_.resize (azimuth_bins_ + 1);
00089   for (size_t l = 0; l < azimuth_bins_ + 1; l++)
00090     phi_divisions_[l] = static_cast<float> (l) * azimuth_interval;
00091 
00092   // LookUp Table that contains the volume of all the bins
00093   // "phi" term of the volume integral
00094   // "integr_phi" has always the same value so we compute it only one time
00095   float integr_phi  = pcl::deg2rad (phi_divisions_[1]) - pcl::deg2rad (phi_divisions_[0]);
00096   // exponential to compute the cube root using pow
00097   float e = 1.0f / 3.0f;
00098   // Resize volume look up table
00099   volume_lut_.resize (radius_bins_ * elevation_bins_ * azimuth_bins_);
00100   // Fill volumes look up table
00101   for (size_t j = 0; j < radius_bins_; j++)
00102   {
00103     // "r" term of the volume integral
00104     float integr_r = (radii_interval_[j+1] * radii_interval_[j+1] * radii_interval_[j+1] / 3.0f) - (radii_interval_[j] * radii_interval_[j] * radii_interval_[j] / 3.0f);
00105 
00106     for (size_t k = 0; k < elevation_bins_; k++)
00107     {
00108       // "theta" term of the volume integral
00109       float integr_theta = cosf (pcl::deg2rad (theta_divisions_[k])) - cosf (pcl::deg2rad (theta_divisions_[k+1]));
00110       // Volume
00111       float V = integr_phi * integr_theta * integr_r;
00112       // Compute cube root of the computed volume commented for performance but left
00113       // here for clarity
00114       // float cbrt = pow(V, e);
00115       // cbrt = 1 / cbrt;
00116 
00117       for (size_t l = 0; l < azimuth_bins_; l++)
00118       {
00119         // Store in lut 1/cbrt
00120         //volume_lut_[ (l*elevation_bins_*radius_bins_) + k*radius_bins_ + j ] = cbrt;
00121         volume_lut_[(l*elevation_bins_*radius_bins_) + k*radius_bins_ + j] = 1.0f / powf (V, e);
00122       }
00123     }
00124   }
00125   return (true);
00126 }
00127 
00129 template <typename PointInT, typename PointNT, typename PointOutT> bool
00130 pcl::ShapeContext3DEstimation<PointInT, PointNT, PointOutT>::computePoint (
00131     size_t index, const pcl::PointCloud<PointNT> &normals, float rf[9], std::vector<float> &desc)
00132 {
00133   // The RF is formed as this x_axis | y_axis | normal
00134   Eigen::Map<Eigen::Vector3f> x_axis (rf);
00135   Eigen::Map<Eigen::Vector3f> y_axis (rf + 3);
00136   Eigen::Map<Eigen::Vector3f> normal (rf + 6);
00137 
00138   // Find every point within specified search_radius_
00139   std::vector<int> nn_indices;
00140   std::vector<float> nn_dists;
00141   const size_t neighb_cnt = searchForNeighbors ((*indices_)[index], search_radius_, nn_indices, nn_dists);
00142   if (neighb_cnt == 0)
00143   {
00144     for (size_t i = 0; i < desc.size (); ++i)
00145       desc[i] = std::numeric_limits<float>::quiet_NaN ();
00146 
00147     memset (rf, 0, sizeof (rf[0]) * 9);
00148     return (false);
00149   }
00150 
00151   float minDist = std::numeric_limits<float>::max ();
00152   int minIndex = -1;
00153   for (size_t i = 0; i < nn_indices.size (); i++)
00154   {
00155           if (nn_dists[i] < minDist)
00156           {
00157       minDist = nn_dists[i];
00158       minIndex = nn_indices[i];
00159           }
00160   }
00161 
00162   // Get origin point
00163   Vector3fMapConst origin = input_->points[(*indices_)[index]].getVector3fMap ();
00164   // Get origin normal
00165   // Use pre-computed normals
00166   normal = normals[minIndex].getNormalVector3fMap ();
00167 
00168   // Compute and store the RF direction
00169   x_axis[0] = static_cast<float> (rnd ());
00170   x_axis[1] = static_cast<float> (rnd ());
00171   x_axis[2] = static_cast<float> (rnd ());
00172   if (!pcl::utils::equal (normal[2], 0.0f))
00173     x_axis[2] = - (normal[0]*x_axis[0] + normal[1]*x_axis[1]) / normal[2];
00174   else if (!pcl::utils::equal (normal[1], 0.0f))
00175     x_axis[1] = - (normal[0]*x_axis[0] + normal[2]*x_axis[2]) / normal[1];
00176   else if (!pcl::utils::equal (normal[0], 0.0f))
00177     x_axis[0] = - (normal[1]*x_axis[1] + normal[2]*x_axis[2]) / normal[0];
00178 
00179   x_axis.normalize ();
00180 
00181   // Check if the computed x axis is orthogonal to the normal
00182   assert (pcl::utils::equal (x_axis[0]*normal[0] + x_axis[1]*normal[1] + x_axis[2]*normal[2], 0.0f, 1E-6f));
00183 
00184   // Store the 3rd frame vector
00185   y_axis.matrix () = normal.cross (x_axis);
00186 
00187   // For each point within radius
00188   for (size_t ne = 0; ne < neighb_cnt; ne++)
00189   {
00190     if (pcl::utils::equal (nn_dists[ne], 0.0f))
00191                   continue;
00192     // Get neighbours coordinates
00193     Eigen::Vector3f neighbour = surface_->points[nn_indices[ne]].getVector3fMap ();
00194 
00197     float r = sqrtf (nn_dists[ne]);
00198 
00200     Eigen::Vector3f proj;
00201     pcl::geometry::project (neighbour, origin, normal, proj);
00202     proj -= origin;
00203 
00205     proj.normalize ();
00206 
00208     Eigen::Vector3f cross = x_axis.cross (proj);
00209     float phi = pcl::rad2deg (std::atan2 (cross.norm (), x_axis.dot (proj)));
00210     phi = cross.dot (normal) < 0.f ? (360.0f - phi) : phi;
00212     Eigen::Vector3f no = neighbour - origin;
00213     no.normalize ();
00214     float theta = normal.dot (no);
00215     theta = pcl::rad2deg (acosf (std::min (1.0f, std::max (-1.0f, theta))));
00216 
00217     // Bin (j, k, l)
00218     size_t j = 0;
00219     size_t k = 0;
00220     size_t l = 0;
00221 
00222     // Compute the Bin(j, k, l) coordinates of current neighbour
00223     for (size_t rad = 1; rad < radius_bins_+1; rad++)
00224     {
00225       if (r <= radii_interval_[rad])
00226       {
00227         j = rad-1;
00228         break;
00229       }
00230     }
00231 
00232     for (size_t ang = 1; ang < elevation_bins_+1; ang++)
00233     {
00234       if (theta <= theta_divisions_[ang])
00235       {
00236         k = ang-1;
00237         break;
00238       }
00239     }
00240 
00241     for (size_t ang = 1; ang < azimuth_bins_+1; ang++)
00242     {
00243       if (phi <= phi_divisions_[ang])
00244       {
00245         l = ang-1;
00246         break;
00247       }
00248     }
00249 
00250     // Local point density = number of points in a sphere of radius "point_density_radius_" around the current neighbour
00251     std::vector<int> neighbour_indices;
00252     std::vector<float> neighbour_distances;
00253     int point_density = searchForNeighbors (*surface_, nn_indices[ne], point_density_radius_, neighbour_indices, neighbour_distances);
00254     // point_density is NOT always bigger than 0 (on error, searchForNeighbors returns 0), so we must check for that
00255     if (point_density == 0)
00256       continue;
00257 
00258     float w = (1.0f / static_cast<float> (point_density)) *
00259               volume_lut_[(l*elevation_bins_*radius_bins_) +  (k*radius_bins_) + j];
00260 
00261     assert (w >= 0.0);
00262     if (w == std::numeric_limits<float>::infinity ())
00263       PCL_ERROR ("Shape Context Error INF!\n");
00264     if (w != w)
00265       PCL_ERROR ("Shape Context Error IND!\n");
00267     desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] += w;
00268 
00269     assert (desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] >= 0);
00270   } // end for each neighbour
00271 
00272   // 3DSC does not define a repeatable local RF, we set it to zero to signal it to the user
00273   memset (rf, 0, sizeof (rf[0]) * 9);
00274   return (true);
00275 }
00276 
00278 template <typename PointInT, typename PointNT, typename PointOutT> void
00279 pcl::ShapeContext3DEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
00280 {
00281   assert (descriptor_length_ == 1980);
00282 
00283   output.is_dense = true;
00284   // Iterate over all points and compute the descriptors
00285         for (size_t point_index = 0; point_index < indices_->size (); point_index++)
00286   {
00287     //output[point_index].descriptor.resize (descriptor_length_);
00288 
00289     // If the point is not finite, set the descriptor to NaN and continue
00290     if (!isFinite ((*input_)[(*indices_)[point_index]]))
00291     {
00292       for (size_t i = 0; i < descriptor_length_; ++i)
00293         output[point_index].descriptor[i] = std::numeric_limits<float>::quiet_NaN ();
00294 
00295       memset (output[point_index].rf, 0, sizeof (output[point_index].rf[0]) * 9);
00296       output.is_dense = false;
00297       continue;
00298     }
00299 
00300     std::vector<float> descriptor (descriptor_length_);
00301     if (!computePoint (point_index, *normals_, output[point_index].rf, descriptor))
00302       output.is_dense = false;
00303     for (size_t j = 0; j < descriptor_length_; ++j)
00304       output[point_index].descriptor[j] = descriptor[j];
00305   }
00306 }
00307 
00308 #define PCL_INSTANTIATE_ShapeContext3DEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::ShapeContext3DEstimation<T,NT,OutT>;
00309 
00310 #endif


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:22:31