Point Cloud Library (PCL)  1.11.1
usc.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2011, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  * $Id$
38  *
39  */
40 
41 #pragma once
42 
43 #include <pcl/features/usc.h>
44 #include <pcl/features/shot_lrf.h>
45 #include <pcl/common/angles.h>
46 #include <pcl/common/geometry.h>
47 #include <pcl/common/point_tests.h> // for pcl::isFinite
48 #include <pcl/common/utils.h>
49 
50 
51 //////////////////////////////////////////////////////////////////////////////////////////////
52 template <typename PointInT, typename PointOutT, typename PointRFT> bool
54 {
56  {
57  PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
58  return (false);
59  }
60 
61  // Default LRF estimation alg: SHOTLocalReferenceFrameEstimation
63  lrf_estimator->setRadiusSearch (local_radius_);
64  lrf_estimator->setInputCloud (input_);
65  lrf_estimator->setIndices (indices_);
66  if (!fake_surface_)
67  lrf_estimator->setSearchSurface(surface_);
68 
70  {
71  PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
72  return (false);
73  }
74 
75  if (search_radius_< min_radius_)
76  {
77  PCL_ERROR ("[pcl::%s::initCompute] search_radius_ must be GREATER than min_radius_.\n", getClassName ().c_str ());
78  return (false);
79  }
80 
81  // Update descriptor length
82  descriptor_length_ = elevation_bins_ * azimuth_bins_ * radius_bins_;
83 
84  // Compute radial, elevation and azimuth divisions
85  float azimuth_interval = 360.0f / static_cast<float> (azimuth_bins_);
86  float elevation_interval = 180.0f / static_cast<float> (elevation_bins_);
87 
88  // Reallocate divisions and volume lut
89  radii_interval_.clear ();
90  phi_divisions_.clear ();
91  theta_divisions_.clear ();
92  volume_lut_.clear ();
93 
94  // Fills radii interval based on formula (1) in section 2.1 of Frome's paper
95  radii_interval_.resize (radius_bins_ + 1);
96  for (std::size_t j = 0; j < radius_bins_ + 1; j++)
97  radii_interval_[j] = static_cast<float> (std::exp (std::log (min_radius_) + ((static_cast<float> (j) / static_cast<float> (radius_bins_)) * std::log (search_radius_/min_radius_))));
98 
99  // Fill theta divisions of elevation
100  theta_divisions_.resize (elevation_bins_ + 1, elevation_interval);
101  theta_divisions_[0] = 0;
102  std::partial_sum(theta_divisions_.begin (), theta_divisions_.end (), theta_divisions_.begin ());
103 
104  // Fill phi divisions of elevation
105  phi_divisions_.resize (azimuth_bins_ + 1, azimuth_interval);
106  phi_divisions_[0] = 0;
107  std::partial_sum(phi_divisions_.begin (), phi_divisions_.end (), phi_divisions_.begin ());
108 
109  // LookUp Table that contains the volume of all the bins
110  // "phi" term of the volume integral
111  // "integr_phi" has always the same value so we compute it only one time
112  float integr_phi = pcl::deg2rad (phi_divisions_[1]) - pcl::deg2rad (phi_divisions_[0]);
113  // exponential to compute the cube root using pow
114  float e = 1.0f / 3.0f;
115  // Resize volume look up table
116  volume_lut_.resize (radius_bins_ * elevation_bins_ * azimuth_bins_);
117  // Fill volumes look up table
118  for (std::size_t j = 0; j < radius_bins_; j++)
119  {
120  // "r" term of the volume integral
121  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);
122 
123  for (std::size_t k = 0; k < elevation_bins_; k++)
124  {
125  // "theta" term of the volume integral
126  float integr_theta = std::cos (deg2rad (theta_divisions_[k])) - std::cos (deg2rad (theta_divisions_[k+1]));
127  // Volume
128  float V = integr_phi * integr_theta * integr_r;
129  // Compute cube root of the computed volume commented for performance but left
130  // here for clarity
131  // float cbrt = pow(V, e);
132  // cbrt = 1 / cbrt;
133 
134  for (std::size_t l = 0; l < azimuth_bins_; l++)
135  // Store in lut 1/cbrt
136  //volume_lut_[ (l*elevation_bins_*radius_bins_) + k*radius_bins_ + j ] = cbrt;
137  volume_lut_[(l*elevation_bins_*radius_bins_) + k*radius_bins_ + j] = 1.0f / powf (V, e);
138  }
139  }
140  return (true);
141 }
142 
143 //////////////////////////////////////////////////////////////////////////////////////////////
144 template <typename PointInT, typename PointOutT, typename PointRFT> void
145 pcl::UniqueShapeContext<PointInT, PointOutT, PointRFT>::computePointDescriptor (std::size_t index, /*float rf[9],*/ std::vector<float> &desc)
146 {
147  pcl::Vector3fMapConst origin = (*input_)[(*indices_)[index]].getVector3fMap ();
148 
149  const Eigen::Vector3f x_axis ((*frames_)[index].x_axis[0],
150  (*frames_)[index].x_axis[1],
151  (*frames_)[index].x_axis[2]);
152  //const Eigen::Vector3f& y_axis = (*frames_)[index].y_axis.getNormalVector3fMap ();
153  const Eigen::Vector3f normal ((*frames_)[index].z_axis[0],
154  (*frames_)[index].z_axis[1],
155  (*frames_)[index].z_axis[2]);
156 
157  // Find every point within specified search_radius_
158  std::vector<int> nn_indices;
159  std::vector<float> nn_dists;
160  const std::size_t neighb_cnt = searchForNeighbors ((*indices_)[index], search_radius_, nn_indices, nn_dists);
161  // For each point within radius
162  for (std::size_t ne = 0; ne < neighb_cnt; ne++)
163  {
164  if (pcl::utils::equal(nn_dists[ne], 0.0f))
165  continue;
166  // Get neighbours coordinates
167  Eigen::Vector3f neighbour = (*surface_)[nn_indices[ne]].getVector3fMap ();
168 
169  // ----- Compute current neighbour polar coordinates -----
170 
171  // Get distance between the neighbour and the origin
172  float r = std::sqrt (nn_dists[ne]);
173 
174  // Project point into the tangent plane
175  Eigen::Vector3f proj;
176  pcl::geometry::project (neighbour, origin, normal, proj);
177  proj -= origin;
178 
179  // Normalize to compute the dot product
180  proj.normalize ();
181 
182  // Compute the angle between the projection and the x axis in the interval [0,360]
183  Eigen::Vector3f cross = x_axis.cross (proj);
184  float phi = rad2deg (std::atan2 (cross.norm (), x_axis.dot (proj)));
185  phi = cross.dot (normal) < 0.f ? (360.0f - phi) : phi;
186  /// Compute the angle between the neighbour and the z axis (normal) in the interval [0, 180]
187  Eigen::Vector3f no = neighbour - origin;
188  no.normalize ();
189  float theta = normal.dot (no);
190  theta = pcl::rad2deg (std::acos (std::min (1.0f, std::max (-1.0f, theta))));
191 
192  /// Compute the Bin(j, k, l) coordinates of current neighbour
193  const auto rad_min = std::lower_bound(std::next (radii_interval_.cbegin ()), radii_interval_.cend (), r);
194  const auto theta_min = std::lower_bound(std::next (theta_divisions_.cbegin ()), theta_divisions_.cend (), theta);
195  const auto phi_min = std::lower_bound(std::next (phi_divisions_.cbegin ()), phi_divisions_.cend (), phi);
196 
197  /// Bin (j, k, l)
198  const auto j = std::distance(radii_interval_.cbegin (), std::prev(rad_min));
199  const auto k = std::distance(theta_divisions_.cbegin (), std::prev(theta_min));
200  const auto l = std::distance(phi_divisions_.cbegin (), std::prev(phi_min));
201 
202  /// Local point density = number of points in a sphere of radius "point_density_radius_" around the current neighbour
203  std::vector<int> neighbour_indices;
204  std::vector<float> neighbour_didtances;
205  float point_density = static_cast<float> (searchForNeighbors (*surface_, nn_indices[ne], point_density_radius_, neighbour_indices, neighbour_didtances));
206  /// point_density is always bigger than 0 because FindPointsWithinRadius returns at least the point itself
207  float w = (1.0f / point_density) * volume_lut_[(l*elevation_bins_*radius_bins_) +
208  (k*radius_bins_) +
209  j];
210 
211  assert (w >= 0.0);
212  if (w == std::numeric_limits<float>::infinity ())
213  PCL_ERROR ("Shape Context Error INF!\n");
214  if (std::isnan(w))
215  PCL_ERROR ("Shape Context Error IND!\n");
216  /// Accumulate w into correspondent Bin(j,k,l)
217  desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] += w;
218 
219  assert (desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] >= 0);
220  } // end for each neighbour
221 }
222 
223 //////////////////////////////////////////////////////////////////////////////////////////////
224 template <typename PointInT, typename PointOutT, typename PointRFT> void
226 {
227  assert (descriptor_length_ == 1960);
228 
229  output.is_dense = true;
230 
231  for (std::size_t point_index = 0; point_index < indices_->size (); ++point_index)
232  {
233  //output[point_index].descriptor.resize (descriptor_length_);
234 
235  // If the point is not finite, set the descriptor to NaN and continue
236  const PointRFT& current_frame = (*frames_)[point_index];
237  if (!isFinite ((*input_)[(*indices_)[point_index]]) ||
238  !std::isfinite (current_frame.x_axis[0]) ||
239  !std::isfinite (current_frame.y_axis[0]) ||
240  !std::isfinite (current_frame.z_axis[0]) )
241  {
242  std::fill (output[point_index].descriptor, output[point_index].descriptor + descriptor_length_,
243  std::numeric_limits<float>::quiet_NaN ());
244  std::fill (output[point_index].rf, output[point_index].rf + 9, 0);
245  output.is_dense = false;
246  continue;
247  }
248 
249  for (int d = 0; d < 3; ++d)
250  {
251  output[point_index].rf[0 + d] = current_frame.x_axis[d];
252  output[point_index].rf[3 + d] = current_frame.y_axis[d];
253  output[point_index].rf[6 + d] = current_frame.z_axis[d];
254  }
255 
256  std::vector<float> descriptor (descriptor_length_);
257  computePointDescriptor (point_index, descriptor);
258  std::copy (descriptor.begin (), descriptor.end (), output[point_index].descriptor);
259  }
260 }
261 
262 #define PCL_INSTANTIATE_UniqueShapeContext(T,OutT,RFT) template class PCL_EXPORTS pcl::UniqueShapeContext<T,OutT,RFT>;
263 
pcl::FeatureWithLocalReferenceFrames
FeatureWithLocalReferenceFrames provides a public interface for descriptor extractor classes which ne...
Definition: feature.h:449
pcl::geometry::distance
float distance(const PointT &p1, const PointT &p2)
Definition: geometry.h:60
pcl::isFinite
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
Definition: point_tests.h:55
geometry.h
Defines some geometrical functions and utility functions.
pcl::PCLBase< PointInT >::setInputCloud
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: pcl_base.hpp:65
angles.h
Define standard C methods to do angle calculations.
pcl::geometry::project
void project(const PointT &point, const PointT &plane_origin, const NormalT &plane_normal, PointT &projected)
Definition: geometry.h:81
pcl::UniqueShapeContext::PointCloudOut
typename Feature< PointInT, PointOutT >::PointCloudOut PointCloudOut
Definition: usc.h:78
pcl::SHOTLocalReferenceFrameEstimation
SHOTLocalReferenceFrameEstimation estimates the Local Reference Frame used in the calculation of the ...
Definition: shot_lrf.h:66
pcl::PCLBase< PointInT >::setIndices
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
Definition: pcl_base.hpp:72
pcl::deg2rad
float deg2rad(float alpha)
Convert an angle from degrees to radians.
Definition: angles.hpp:67
pcl::UniqueShapeContext::computePointDescriptor
void computePointDescriptor(std::size_t index, std::vector< float > &desc)
Compute 3D shape context feature descriptor.
Definition: usc.hpp:145
pcl::SHOTLocalReferenceFrameEstimation::Ptr
shared_ptr< SHOTLocalReferenceFrameEstimation< PointInT, PointOutT > > Ptr
Definition: shot_lrf.h:68
pcl::Vector3fMapConst
const Eigen::Map< const Eigen::Vector3f > Vector3fMapConst
Definition: point_types.hpp:181
pcl::Feature< PointInT, ReferenceFrame >::setSearchSurface
void setSearchSurface(const PointCloudInConstPtr &cloud)
Provide a pointer to a dataset to add additional information to estimate the features for every point...
Definition: feature.h:149
pcl::UniqueShapeContext::computeFeature
void computeFeature(PointCloudOut &output) override
The actual feature computation.
Definition: usc.hpp:225
pcl::UniqueShapeContext::initCompute
bool initCompute() override
Initialize computation by allocating all the intervals and the volume lookup table.
Definition: usc.hpp:53
pcl::Feature< PointInT, ReferenceFrame >::setRadiusSearch
void setRadiusSearch(double radius)
Set the sphere radius that is to be used for determining the nearest neighbors used for the feature e...
Definition: feature.h:201
pcl::utils::equal
bool equal(T val1, T val2, T eps=std::numeric_limits< T >::min())
Check if val1 and val2 are equal to an epsilon extent.
Definition: utils.h:55
pcl::rad2deg
float rad2deg(float alpha)
Convert an angle from radians to degrees.
Definition: angles.hpp:61
pcl::Feature
Feature represents the base feature class.
Definition: feature.h:107