usc.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  *
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: usc.hpp 6144 2012-07-04 22:06:28Z rusu $
00037  *
00038  */
00039 
00040 #ifndef PCL_FEATURES_IMPL_USC_HPP_
00041 #define PCL_FEATURES_IMPL_USC_HPP_
00042 
00043 #include <pcl/features/usc.h>
00044 #include <pcl/features/shot_lrf.h>
00045 #include <pcl/common/geometry.h>
00046 #include <pcl/common/angles.h>
00047 #include <pcl/common/utils.h>
00048 
00050 template <typename PointInT, typename PointOutT, typename PointRFT> bool
00051 pcl::UniqueShapeContext<PointInT, PointOutT, PointRFT>::initCompute ()
00052 {
00053   if (!Feature<PointInT, PointOutT>::initCompute ())
00054   {
00055     PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
00056     return (false);
00057   }
00058 
00059   // Default LRF estimation alg: SHOTLocalReferenceFrameEstimation
00060   typename SHOTLocalReferenceFrameEstimation<PointInT, PointRFT>::Ptr lrf_estimator(new SHOTLocalReferenceFrameEstimation<PointInT, PointRFT>());
00061   lrf_estimator->setRadiusSearch (local_radius_);
00062   lrf_estimator->setInputCloud (input_);
00063   lrf_estimator->setIndices (indices_);
00064   if (!fake_surface_)
00065     lrf_estimator->setSearchSurface(surface_);
00066 
00067   if (!FeatureWithLocalReferenceFrames<PointInT, PointRFT>::initLocalReferenceFrames (indices_->size (), lrf_estimator))
00068   {
00069     PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
00070     return (false);
00071   }
00072 
00073   if (search_radius_< min_radius_)
00074   {
00075     PCL_ERROR ("[pcl::%s::initCompute] search_radius_ must be GREATER than min_radius_.\n", getClassName ().c_str ());
00076     return (false);
00077   }
00078 
00079   // Update descriptor length
00080   descriptor_length_ = elevation_bins_ * azimuth_bins_ * radius_bins_;
00081 
00082   // Compute radial, elevation and azimuth divisions
00083   float azimuth_interval = 360.0f / static_cast<float> (azimuth_bins_);
00084   float elevation_interval = 180.0f / static_cast<float> (elevation_bins_);
00085 
00086   // Reallocate divisions and volume lut
00087   radii_interval_.clear ();
00088   phi_divisions_.clear ();
00089   theta_divisions_.clear ();
00090   volume_lut_.clear ();
00091 
00092   // Fills radii interval based on formula (1) in section 2.1 of Frome's paper
00093   radii_interval_.resize (radius_bins_ + 1);
00094   for (size_t j = 0; j < radius_bins_ + 1; j++)
00095     radii_interval_[j] = static_cast<float> (exp (log (min_radius_) + ((static_cast<float> (j) / static_cast<float> (radius_bins_)) * log (search_radius_/min_radius_))));
00096 
00097   // Fill theta didvisions of elevation
00098   theta_divisions_.resize (elevation_bins_+1);
00099   for (size_t k = 0; k < elevation_bins_+1; k++)
00100     theta_divisions_[k] = static_cast<float> (k) * elevation_interval;
00101 
00102   // Fill phi didvisions of elevation
00103   phi_divisions_.resize (azimuth_bins_+1);
00104   for (size_t l = 0; l < azimuth_bins_+1; l++)
00105     phi_divisions_[l] = static_cast<float> (l) * azimuth_interval;
00106 
00107   // LookUp Table that contains the volume of all the bins
00108   // "phi" term of the volume integral
00109   // "integr_phi" has always the same value so we compute it only one time
00110   float integr_phi  = pcl::deg2rad (phi_divisions_[1]) - pcl::deg2rad (phi_divisions_[0]);
00111   // exponential to compute the cube root using pow
00112   float e = 1.0f / 3.0f;
00113   // Resize volume look up table
00114   volume_lut_.resize (radius_bins_ * elevation_bins_ * azimuth_bins_);
00115   // Fill volumes look up table
00116   for (size_t j = 0; j < radius_bins_; j++)
00117   {
00118     // "r" term of the volume integral
00119     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);
00120 
00121     for (size_t k = 0; k < elevation_bins_; k++)
00122     {
00123       // "theta" term of the volume integral
00124       float integr_theta = cosf (deg2rad (theta_divisions_[k])) - cosf (deg2rad (theta_divisions_[k+1]));
00125       // Volume
00126       float V = integr_phi * integr_theta * integr_r;
00127       // Compute cube root of the computed volume commented for performance but left
00128       // here for clarity
00129       // float cbrt = pow(V, e);
00130       // cbrt = 1 / cbrt;
00131 
00132       for (size_t l = 0; l < azimuth_bins_; l++)
00133         // Store in lut 1/cbrt
00134         //volume_lut_[ (l*elevation_bins_*radius_bins_) + k*radius_bins_ + j ] = cbrt;
00135         volume_lut_[(l*elevation_bins_*radius_bins_) + k*radius_bins_ + j] = 1.0f / powf (V, e);
00136     }
00137   }
00138   return (true);
00139 }
00140 
00142 template <typename PointInT, typename PointOutT, typename PointRFT> void
00143 pcl::UniqueShapeContext<PointInT, PointOutT, PointRFT>::computePointDescriptor (size_t index, /*float rf[9],*/ std::vector<float> &desc)
00144 {
00145   pcl::Vector3fMapConst origin = input_->points[(*indices_)[index]].getVector3fMap ();
00146 
00147   const Eigen::Vector3f& x_axis = frames_->points[index].x_axis.getNormalVector3fMap ();
00148   //const Eigen::Vector3f& y_axis = frames_->points[index].y_axis.getNormalVector3fMap ();
00149   const Eigen::Vector3f& normal = frames_->points[index].z_axis.getNormalVector3fMap ();
00150 
00151   // Find every point within specified search_radius_
00152   std::vector<int> nn_indices;
00153   std::vector<float> nn_dists;
00154   const size_t neighb_cnt = searchForNeighbors ((*indices_)[index], search_radius_, nn_indices, nn_dists);
00155   // For each point within radius
00156   for (size_t ne = 0; ne < neighb_cnt; ne++)
00157   {
00158     if (pcl::utils::equal(nn_dists[ne], 0.0f))
00159       continue;
00160     // Get neighbours coordinates
00161     Eigen::Vector3f neighbour = surface_->points[nn_indices[ne]].getVector3fMap ();
00162 
00163     // ----- Compute current neighbour polar coordinates -----
00164 
00165     // Get distance between the neighbour and the origin
00166     float r = sqrt (nn_dists[ne]);
00167 
00168     // Project point into the tangent plane
00169     Eigen::Vector3f proj;
00170     pcl::geometry::project (neighbour, origin, normal, proj);
00171     proj -= origin;
00172 
00173     // Normalize to compute the dot product
00174     proj.normalize ();
00175 
00176     // Compute the angle between the projection and the x axis in the interval [0,360]
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.0f - phi) : phi;
00181     Eigen::Vector3f no = neighbour - origin;
00182     no.normalize ();
00183     float theta = normal.dot (no);
00184     theta = pcl::rad2deg (acosf (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     {
00194       if (r <= radii_interval_[rad])
00195       {
00196         j = rad - 1;
00197         break;
00198       }
00199     }
00200 
00201     for (size_t ang = 1; ang < elevation_bins_ + 1; ang++)
00202     {
00203       if (theta <= theta_divisions_[ang])
00204       {
00205         k = ang - 1;
00206         break;
00207       }
00208     }
00209 
00210     for (size_t ang = 1; ang < azimuth_bins_ + 1; ang++)
00211     {
00212       if (phi <= phi_divisions_[ang])
00213       {
00214         l = ang - 1;
00215         break;
00216       }
00217     }
00218 
00220     std::vector<int> neighbour_indices;
00221     std::vector<float> neighbour_didtances;
00222     float point_density = static_cast<float> (searchForNeighbors (*surface_, nn_indices[ne], point_density_radius_, neighbour_indices, neighbour_didtances));
00224     float w = (1.0f / point_density) * volume_lut_[(l*elevation_bins_*radius_bins_) +
00225                                                    (k*radius_bins_) +
00226                                                    j];
00227 
00228     assert (w >= 0.0);
00229     if (w == std::numeric_limits<float>::infinity ())
00230       PCL_ERROR ("Shape Context Error INF!\n");
00231     if (w != w)
00232       PCL_ERROR ("Shape Context Error IND!\n");
00234     desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] += w;
00235 
00236     assert (desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] >= 0);
00237   } // end for each neighbour
00238 }
00239 
00241 template <typename PointInT, typename PointOutT, typename PointRFT> void
00242 pcl::UniqueShapeContext<PointInT, PointOutT, PointRFT>::computeFeature (PointCloudOut &output)
00243 {
00244   for (size_t point_index = 0; point_index < indices_->size (); point_index++)
00245   {
00246     output[point_index].descriptor.resize (descriptor_length_);
00247     for (int d = 0; d < 9; ++d)
00248       output.points[point_index].rf[d] = frames_->points[point_index].rf[ (4*(d/3) + (d%3)) ];
00249 
00250     computePointDescriptor (point_index, output[point_index].descriptor);
00251   }
00252 }
00253 
00255 template <typename PointInT, typename PointRFT> void
00256 pcl::UniqueShapeContext<PointInT, Eigen::MatrixXf, PointRFT>::computeFeatureEigen (pcl::PointCloud<Eigen::MatrixXf> &output)
00257 {
00258   output.points.resize (indices_->size (), descriptor_length_ + 9);
00259 
00260   for (size_t point_index = 0; point_index < indices_->size (); point_index++)
00261   {
00262     for (int d = 0; d < 9; ++d)
00263       output.points (point_index, d) = frames_->points[point_index].rf[ (4*(d/3) + (d%3)) ];
00264 
00265     std::vector<float> descriptor (descriptor_length_);
00266     computePointDescriptor (point_index, descriptor);
00267     for (size_t j = 0; j < descriptor_length_; ++j)
00268       output.points (point_index, 9 + j) = descriptor[j];
00269   }
00270 }
00271 
00272 #define PCL_INSTANTIATE_UniqueShapeContext(T,OutT,RFT) template class PCL_EXPORTS pcl::UniqueShapeContext<T,OutT,RFT>;
00273 
00274 #endif


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