Point Cloud Library (PCL)  1.7.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 #ifndef PCL_FEATURES_IMPL_USC_HPP_
42 #define PCL_FEATURES_IMPL_USC_HPP_
43 
44 #include <pcl/features/usc.h>
45 #include <pcl/features/shot_lrf.h>
46 #include <pcl/common/geometry.h>
47 #include <pcl/common/angles.h>
48 #include <pcl/common/utils.h>
49 
50 //////////////////////////////////////////////////////////////////////////////////////////////
51 template <typename PointInT, typename PointOutT, typename PointRFT> bool
53 {
55  {
56  PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
57  return (false);
58  }
59 
60  // Default LRF estimation alg: SHOTLocalReferenceFrameEstimation
62  lrf_estimator->setRadiusSearch (local_radius_);
63  lrf_estimator->setInputCloud (input_);
64  lrf_estimator->setIndices (indices_);
65  if (!fake_surface_)
66  lrf_estimator->setSearchSurface(surface_);
67 
69  {
70  PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
71  return (false);
72  }
73 
74  if (search_radius_< min_radius_)
75  {
76  PCL_ERROR ("[pcl::%s::initCompute] search_radius_ must be GREATER than min_radius_.\n", getClassName ().c_str ());
77  return (false);
78  }
79 
80  // Update descriptor length
81  descriptor_length_ = elevation_bins_ * azimuth_bins_ * radius_bins_;
82 
83  // Compute radial, elevation and azimuth divisions
84  float azimuth_interval = 360.0f / static_cast<float> (azimuth_bins_);
85  float elevation_interval = 180.0f / static_cast<float> (elevation_bins_);
86 
87  // Reallocate divisions and volume lut
88  radii_interval_.clear ();
89  phi_divisions_.clear ();
90  theta_divisions_.clear ();
91  volume_lut_.clear ();
92 
93  // Fills radii interval based on formula (1) in section 2.1 of Frome's paper
94  radii_interval_.resize (radius_bins_ + 1);
95  for (size_t j = 0; j < radius_bins_ + 1; j++)
96  radii_interval_[j] = static_cast<float> (exp (log (min_radius_) + ((static_cast<float> (j) / static_cast<float> (radius_bins_)) * log (search_radius_/min_radius_))));
97 
98  // Fill theta didvisions of elevation
99  theta_divisions_.resize (elevation_bins_+1);
100  for (size_t k = 0; k < elevation_bins_+1; k++)
101  theta_divisions_[k] = static_cast<float> (k) * elevation_interval;
102 
103  // Fill phi didvisions of elevation
104  phi_divisions_.resize (azimuth_bins_+1);
105  for (size_t l = 0; l < azimuth_bins_+1; l++)
106  phi_divisions_[l] = static_cast<float> (l) * azimuth_interval;
107 
108  // LookUp Table that contains the volume of all the bins
109  // "phi" term of the volume integral
110  // "integr_phi" has always the same value so we compute it only one time
111  float integr_phi = pcl::deg2rad (phi_divisions_[1]) - pcl::deg2rad (phi_divisions_[0]);
112  // exponential to compute the cube root using pow
113  float e = 1.0f / 3.0f;
114  // Resize volume look up table
115  volume_lut_.resize (radius_bins_ * elevation_bins_ * azimuth_bins_);
116  // Fill volumes look up table
117  for (size_t j = 0; j < radius_bins_; j++)
118  {
119  // "r" term of the volume integral
120  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);
121 
122  for (size_t k = 0; k < elevation_bins_; k++)
123  {
124  // "theta" term of the volume integral
125  float integr_theta = cosf (deg2rad (theta_divisions_[k])) - cosf (deg2rad (theta_divisions_[k+1]));
126  // Volume
127  float V = integr_phi * integr_theta * integr_r;
128  // Compute cube root of the computed volume commented for performance but left
129  // here for clarity
130  // float cbrt = pow(V, e);
131  // cbrt = 1 / cbrt;
132 
133  for (size_t l = 0; l < azimuth_bins_; l++)
134  // Store in lut 1/cbrt
135  //volume_lut_[ (l*elevation_bins_*radius_bins_) + k*radius_bins_ + j ] = cbrt;
136  volume_lut_[(l*elevation_bins_*radius_bins_) + k*radius_bins_ + j] = 1.0f / powf (V, e);
137  }
138  }
139  return (true);
140 }
141 
142 //////////////////////////////////////////////////////////////////////////////////////////////
143 template <typename PointInT, typename PointOutT, typename PointRFT> void
144 pcl::UniqueShapeContext<PointInT, PointOutT, PointRFT>::computePointDescriptor (size_t index, /*float rf[9],*/ std::vector<float> &desc)
145 {
146  pcl::Vector3fMapConst origin = input_->points[(*indices_)[index]].getVector3fMap ();
147 
148  const Eigen::Vector3f x_axis (frames_->points[index].x_axis[0],
149  frames_->points[index].x_axis[1],
150  frames_->points[index].x_axis[2]);
151  //const Eigen::Vector3f& y_axis = frames_->points[index].y_axis.getNormalVector3fMap ();
152  const Eigen::Vector3f normal (frames_->points[index].z_axis[0],
153  frames_->points[index].z_axis[1],
154  frames_->points[index].z_axis[2]);
155 
156  // Find every point within specified search_radius_
157  std::vector<int> nn_indices;
158  std::vector<float> nn_dists;
159  const size_t neighb_cnt = searchForNeighbors ((*indices_)[index], search_radius_, nn_indices, nn_dists);
160  // For each point within radius
161  for (size_t ne = 0; ne < neighb_cnt; ne++)
162  {
163  if (pcl::utils::equal(nn_dists[ne], 0.0f))
164  continue;
165  // Get neighbours coordinates
166  Eigen::Vector3f neighbour = surface_->points[nn_indices[ne]].getVector3fMap ();
167 
168  // ----- Compute current neighbour polar coordinates -----
169 
170  // Get distance between the neighbour and the origin
171  float r = sqrtf (nn_dists[ne]);
172 
173  // Project point into the tangent plane
174  Eigen::Vector3f proj;
175  pcl::geometry::project (neighbour, origin, normal, proj);
176  proj -= origin;
177 
178  // Normalize to compute the dot product
179  proj.normalize ();
180 
181  // Compute the angle between the projection and the x axis in the interval [0,360]
182  Eigen::Vector3f cross = x_axis.cross (proj);
183  float phi = rad2deg (std::atan2 (cross.norm (), x_axis.dot (proj)));
184  phi = cross.dot (normal) < 0.f ? (360.0f - phi) : phi;
185  /// Compute the angle between the neighbour and the z axis (normal) in the interval [0, 180]
186  Eigen::Vector3f no = neighbour - origin;
187  no.normalize ();
188  float theta = normal.dot (no);
189  theta = pcl::rad2deg (acosf (std::min (1.0f, std::max (-1.0f, theta))));
190 
191  /// Bin (j, k, l)
192  size_t j = 0;
193  size_t k = 0;
194  size_t l = 0;
195 
196  /// Compute the Bin(j, k, l) coordinates of current neighbour
197  for (size_t rad = 1; rad < radius_bins_ + 1; rad++)
198  {
199  if (r <= radii_interval_[rad])
200  {
201  j = rad - 1;
202  break;
203  }
204  }
205 
206  for (size_t ang = 1; ang < elevation_bins_ + 1; ang++)
207  {
208  if (theta <= theta_divisions_[ang])
209  {
210  k = ang - 1;
211  break;
212  }
213  }
214 
215  for (size_t ang = 1; ang < azimuth_bins_ + 1; ang++)
216  {
217  if (phi <= phi_divisions_[ang])
218  {
219  l = ang - 1;
220  break;
221  }
222  }
223 
224  /// Local point density = number of points in a sphere of radius "point_density_radius_" around the current neighbour
225  std::vector<int> neighbour_indices;
226  std::vector<float> neighbour_didtances;
227  float point_density = static_cast<float> (searchForNeighbors (*surface_, nn_indices[ne], point_density_radius_, neighbour_indices, neighbour_didtances));
228  /// point_density is always bigger than 0 because FindPointsWithinRadius returns at least the point itself
229  float w = (1.0f / point_density) * volume_lut_[(l*elevation_bins_*radius_bins_) +
230  (k*radius_bins_) +
231  j];
232 
233  assert (w >= 0.0);
234  if (w == std::numeric_limits<float>::infinity ())
235  PCL_ERROR ("Shape Context Error INF!\n");
236  if (w != w)
237  PCL_ERROR ("Shape Context Error IND!\n");
238  /// Accumulate w into correspondant Bin(j,k,l)
239  desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] += w;
240 
241  assert (desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] >= 0);
242  } // end for each neighbour
243 }
244 
245 //////////////////////////////////////////////////////////////////////////////////////////////
246 template <typename PointInT, typename PointOutT, typename PointRFT> void
248 {
249  assert (descriptor_length_ == 1980);
250 
251  output.is_dense = true;
252 
253  for (size_t point_index = 0; point_index < indices_->size (); ++point_index)
254  {
255  //output[point_index].descriptor.resize (descriptor_length_);
256 
257  // If the point is not finite, set the descriptor to NaN and continue
258  const PointRFT& current_frame = (*frames_)[point_index];
259  if (!isFinite ((*input_)[(*indices_)[point_index]]) ||
260  !pcl_isfinite (current_frame.x_axis[0]) ||
261  !pcl_isfinite (current_frame.y_axis[0]) ||
262  !pcl_isfinite (current_frame.z_axis[0]) )
263  {
264  for (size_t i = 0; i < descriptor_length_; ++i)
265  output[point_index].descriptor[i] = std::numeric_limits<float>::quiet_NaN ();
266 
267  memset (output[point_index].rf, 0, sizeof (output[point_index].rf[0]) * 9);
268  output.is_dense = false;
269  continue;
270  }
271 
272  for (int d = 0; d < 3; ++d)
273  {
274  output.points[point_index].rf[0 + d] = current_frame.x_axis[d];
275  output.points[point_index].rf[3 + d] = current_frame.y_axis[d];
276  output.points[point_index].rf[6 + d] = current_frame.z_axis[d];
277  }
278 
279  std::vector<float> descriptor (descriptor_length_);
280  computePointDescriptor (point_index, descriptor);
281  for (size_t j = 0; j < descriptor_length_; ++j)
282  output [point_index].descriptor[j] = descriptor[j];
283  }
284 }
285 
286 #define PCL_INSTANTIATE_UniqueShapeContext(T,OutT,RFT) template class PCL_EXPORTS pcl::UniqueShapeContext<T,OutT,RFT>;
287 
288 #endif
void computePointDescriptor(size_t index, std::vector< float > &desc)
Compute 3D shape context feature descriptor.
Definition: usc.hpp:144
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
Definition: point_cloud.h:418
bool equal(T val1, T val2, T eps=std::numeric_limits< T >::min())
Check if val1 and val2 are equals to an epsilon extent.
Definition: utils.h:55
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:200
FeatureWithLocalReferenceFrames provides a public interface for descriptor extractor classes which ne...
Definition: feature.h:447
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested.
Definition: point_tests.h:53
virtual bool initCompute()
Initialize computation by allocating all the intervals and the volume lookup table.
Definition: usc.hpp:52
boost::shared_ptr< SHOTLocalReferenceFrameEstimation< PointInT, PointOutT > > Ptr
Definition: shot_lrf.h:69
virtual void computeFeature(PointCloudOut &output)
The actual feature computation.
Definition: usc.hpp:247
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:148
float deg2rad(float alpha)
Convert an angle from degrees to radians.
Definition: angles.hpp:67
void project(const PointT &point, const PointT &plane_origin, const NormalT &plane_normal, PointT &projected)
Definition: geometry.h:81
Feature represents the base feature class.
Definition: feature.h:105
const Eigen::Map< const Eigen::Vector3f > Vector3fMapConst
SHOTLocalReferenceFrameEstimation estimates the Local Reference Frame used in the calculation of the ...
Definition: shot_lrf.h:66
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
float rad2deg(float alpha)
Convert an angle from radians to degrees.
Definition: angles.hpp:61