Point Cloud Library (PCL)  1.7.0
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