template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> bool
pcl::SHOTEstimationBase<PointInT, PointNT, PointOutT, PointRFT>::initCompute ()
{
if (!FeatureFromNormals<PointInT, PointNT, PointOutT>::initCompute ())
{
PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
return (false);
}
// SHOT cannot work with k-search
if (this->getKSearch () != 0)
{
PCL_ERROR(
"[pcl::%s::initCompute] Error! Search method set to k-neighborhood. Call setKSearch(0) and setRadiusSearch( radius ) to use this class.\n",
getClassName().c_str ());
return (false);
}
// Default LRF estimation alg: SHOTLocalReferenceFrameEstimation
typename SHOTLocalReferenceFrameEstimation<PointInT, PointRFT>::Ptr lrf_estimator(new SHOTLocalReferenceFrameEstimation<PointInT, PointRFT>());
lrf_estimator->setRadiusSearch ((lrf_radius_ > 0 ? lrf_radius_ : search_radius_));
lrf_estimator->setInputCloud (input_);
lrf_estimator->setIndices (indices_);
if (!fake_surface_)
lrf_estimator->setSearchSurface(surface_);
if (!FeatureWithLocalReferenceFrames<PointInT, PointRFT>::initLocalReferenceFrames (indices_->size (), lrf_estimator))
{
PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
return (false);
}
return (true);
}
template <typename PointInT, typename PointOutT, typename PointRFT> bool
pcl::UniqueShapeContext<PointInT, PointOutT, PointRFT>::initCompute ()
{
if (!Feature<PointInT, PointOutT>::initCompute ())
{
PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
return (false);
}
// Default LRF estimation alg: SHOTLocalReferenceFrameEstimation
typename SHOTLocalReferenceFrameEstimation<PointInT, PointRFT>::Ptr lrf_estimator(new SHOTLocalReferenceFrameEstimation<PointInT, PointRFT>());
lrf_estimator->setRadiusSearch (local_radius_);
lrf_estimator->setInputCloud (input_);
lrf_estimator->setIndices (indices_);
if (!fake_surface_)
lrf_estimator->setSearchSurface(surface_);
if (!FeatureWithLocalReferenceFrames<PointInT, PointRFT>::initLocalReferenceFrames (indices_->size (), lrf_estimator))
{
PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
return (false);
}
if (search_radius_< min_radius_)
{
PCL_ERROR ("[pcl::%s::initCompute] search_radius_ must be GREATER than min_radius_.\n", getClassName ().c_str ());
return (false);
}
// Update descriptor length
descriptor_length_ = elevation_bins_ * azimuth_bins_ * radius_bins_;
// Compute radial, elevation and azimuth divisions
float azimuth_interval = 360.0f / static_cast<float> (azimuth_bins_);
float elevation_interval = 180.0f / static_cast<float> (elevation_bins_);
// Reallocate divisions and volume lut
radii_interval_.clear ();
phi_divisions_.clear ();
theta_divisions_.clear ();
volume_lut_.clear ();
// Fills radii interval based on formula (1) in section 2.1 of Frome's paper
radii_interval_.resize (radius_bins_ + 1);
for (size_t j = 0; j < radius_bins_ + 1; j++)
radii_interval_[j] = static_cast<float> (exp (log (min_radius_) + ((static_cast<float> (j) / static_cast<float> (radius_bins_)) * log (search_radius_/min_radius_))));
// Fill theta didvisions of elevation
theta_divisions_.resize (elevation_bins_+1);
for (size_t k = 0; k < elevation_bins_+1; k++)
theta_divisions_[k] = static_cast<float> (k) * elevation_interval;
// Fill phi didvisions of elevation
phi_divisions_.resize (azimuth_bins_+1);
for (size_t l = 0; l < azimuth_bins_+1; l++)
phi_divisions_[l] = static_cast<float> (l) * azimuth_interval;
// LookUp Table that contains the volume of all the bins
// "phi" term of the volume integral
// "integr_phi" has always the same value so we compute it only one time
float integr_phi = pcl::deg2rad (phi_divisions_[1]) - pcl::deg2rad (phi_divisions_[0]);
// exponential to compute the cube root using pow
float e = 1.0f / 3.0f;
// Resize volume look up table
volume_lut_.resize (radius_bins_ * elevation_bins_ * azimuth_bins_);
// Fill volumes look up table
for (size_t j = 0; j < radius_bins_; j++)
{
// "r" term of the volume integral
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);
for (size_t k = 0; k < elevation_bins_; k++)
{
// "theta" term of the volume integral
float integr_theta = cosf (deg2rad (theta_divisions_[k])) - cosf (deg2rad (theta_divisions_[k+1]));
// Volume
float V = integr_phi * integr_theta * integr_r;
// Compute cube root of the computed volume commented for performance but left
// here for clarity
// float cbrt = pow(V, e);
// cbrt = 1 / cbrt;
for (size_t l = 0; l < azimuth_bins_; l++)
// Store in lut 1/cbrt
//volume_lut_[ (l*elevation_bins_*radius_bins_) + k*radius_bins_ + j ] = cbrt;
volume_lut_[(l*elevation_bins_*radius_bins_) + k*radius_bins_ + j] = 1.0f / powf (V, e);
}
}
return (true);
}
