Point Cloud Library (PCL)  1.9.1-dev
3dsc.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  */
38 
39 #ifndef PCL_FEATURES_IMPL_3DSC_HPP_
40 #define PCL_FEATURES_IMPL_3DSC_HPP_
41 
42 #include <cmath>
43 #include <pcl/features/3dsc.h>
44 #include <pcl/common/utils.h>
45 #include <pcl/common/geometry.h>
46 #include <pcl/common/angles.h>
47 
48 //////////////////////////////////////////////////////////////////////////////////////////////
49 template <typename PointInT, typename PointNT, typename PointOutT> bool
51 {
53  {
54  PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
55  return (false);
56  }
57 
58  if (search_radius_< min_radius_)
59  {
60  PCL_ERROR ("[pcl::%s::initCompute] search_radius_ must be GREATER than min_radius_.\n", getClassName ().c_str ());
61  return (false);
62  }
63 
64  // Update descriptor length
65  descriptor_length_ = elevation_bins_ * azimuth_bins_ * radius_bins_;
66 
67  // Compute radial, elevation and azimuth divisions
68  float azimuth_interval = 360.0f / static_cast<float> (azimuth_bins_);
69  float elevation_interval = 180.0f / static_cast<float> (elevation_bins_);
70 
71  // Reallocate divisions and volume lut
72  radii_interval_.clear ();
73  phi_divisions_.clear ();
74  theta_divisions_.clear ();
75  volume_lut_.clear ();
76 
77  // Fills radii interval based on formula (1) in section 2.1 of Frome's paper
78  radii_interval_.resize (radius_bins_ + 1);
79  for (size_t j = 0; j < radius_bins_ + 1; j++)
80  radii_interval_[j] = static_cast<float> (exp (log (min_radius_) + ((static_cast<float> (j) / static_cast<float> (radius_bins_)) * log (search_radius_ / min_radius_))));
81 
82  // Fill theta divisions of elevation
83  theta_divisions_.resize (elevation_bins_ + 1);
84  for (size_t k = 0; k < elevation_bins_ + 1; k++)
85  theta_divisions_[k] = static_cast<float> (k) * elevation_interval;
86 
87  // Fill phi didvisions of elevation
88  phi_divisions_.resize (azimuth_bins_ + 1);
89  for (size_t l = 0; l < azimuth_bins_ + 1; l++)
90  phi_divisions_[l] = static_cast<float> (l) * azimuth_interval;
91 
92  // LookUp Table that contains the volume of all the bins
93  // "phi" term of the volume integral
94  // "integr_phi" has always the same value so we compute it only one time
95  float integr_phi = pcl::deg2rad (phi_divisions_[1]) - pcl::deg2rad (phi_divisions_[0]);
96  // exponential to compute the cube root using pow
97  float e = 1.0f / 3.0f;
98  // Resize volume look up table
99  volume_lut_.resize (radius_bins_ * elevation_bins_ * azimuth_bins_);
100  // Fill volumes look up table
101  for (size_t j = 0; j < radius_bins_; j++)
102  {
103  // "r" term of the volume integral
104  float integr_r = (radii_interval_[j+1] * radii_interval_[j+1] * radii_interval_[j+1] / 3.0f) - (radii_interval_[j] * radii_interval_[j] * radii_interval_[j] / 3.0f);
105 
106  for (size_t k = 0; k < elevation_bins_; k++)
107  {
108  // "theta" term of the volume integral
109  float integr_theta = cosf (pcl::deg2rad (theta_divisions_[k])) - cosf (pcl::deg2rad (theta_divisions_[k+1]));
110  // Volume
111  float V = integr_phi * integr_theta * integr_r;
112  // Compute cube root of the computed volume commented for performance but left
113  // here for clarity
114  // float cbrt = pow(V, e);
115  // cbrt = 1 / cbrt;
116 
117  for (size_t l = 0; l < azimuth_bins_; l++)
118  {
119  // Store in lut 1/cbrt
120  //volume_lut_[ (l*elevation_bins_*radius_bins_) + k*radius_bins_ + j ] = cbrt;
121  volume_lut_[(l*elevation_bins_*radius_bins_) + k*radius_bins_ + j] = 1.0f / powf (V, e);
122  }
123  }
124  }
125  return (true);
126 }
127 
128 //////////////////////////////////////////////////////////////////////////////////////////////
129 template <typename PointInT, typename PointNT, typename PointOutT> bool
131  size_t index, const pcl::PointCloud<PointNT> &normals, float rf[9], std::vector<float> &desc)
132 {
133  // The RF is formed as this x_axis | y_axis | normal
134  Eigen::Map<Eigen::Vector3f> x_axis (rf);
135  Eigen::Map<Eigen::Vector3f> y_axis (rf + 3);
136  Eigen::Map<Eigen::Vector3f> normal (rf + 6);
137 
138  // Find every point within specified search_radius_
139  std::vector<int> nn_indices;
140  std::vector<float> nn_dists;
141  const size_t neighb_cnt = searchForNeighbors ((*indices_)[index], search_radius_, nn_indices, nn_dists);
142  if (neighb_cnt == 0)
143  {
144  for (float &descriptor : desc)
145  descriptor = std::numeric_limits<float>::quiet_NaN ();
146 
147  memset (rf, 0, sizeof (rf[0]) * 9);
148  return (false);
149  }
150 
151  float minDist = std::numeric_limits<float>::max ();
152  int minIndex = -1;
153  for (size_t i = 0; i < nn_indices.size (); i++)
154  {
155  if (nn_dists[i] < minDist)
156  {
157  minDist = nn_dists[i];
158  minIndex = nn_indices[i];
159  }
160  }
161 
162  // Get origin point
163  Vector3fMapConst origin = input_->points[(*indices_)[index]].getVector3fMap ();
164  // Get origin normal
165  // Use pre-computed normals
166  normal = normals[minIndex].getNormalVector3fMap ();
167 
168  // Compute and store the RF direction
169  x_axis[0] = rnd ();
170  x_axis[1] = rnd ();
171  x_axis[2] = rnd ();
172  if (!pcl::utils::equal (normal[2], 0.0f))
173  x_axis[2] = - (normal[0]*x_axis[0] + normal[1]*x_axis[1]) / normal[2];
174  else if (!pcl::utils::equal (normal[1], 0.0f))
175  x_axis[1] = - (normal[0]*x_axis[0] + normal[2]*x_axis[2]) / normal[1];
176  else if (!pcl::utils::equal (normal[0], 0.0f))
177  x_axis[0] = - (normal[1]*x_axis[1] + normal[2]*x_axis[2]) / normal[0];
178 
179  x_axis.normalize ();
180 
181  // Check if the computed x axis is orthogonal to the normal
182  assert (pcl::utils::equal (x_axis[0]*normal[0] + x_axis[1]*normal[1] + x_axis[2]*normal[2], 0.0f, 1E-6f));
183 
184  // Store the 3rd frame vector
185  y_axis.matrix () = normal.cross (x_axis);
186 
187  // For each point within radius
188  for (size_t ne = 0; ne < neighb_cnt; ne++)
189  {
190  if (pcl::utils::equal (nn_dists[ne], 0.0f))
191  continue;
192  // Get neighbours coordinates
193  Eigen::Vector3f neighbour = surface_->points[nn_indices[ne]].getVector3fMap ();
194 
195  /// ----- Compute current neighbour polar coordinates -----
196  /// Get distance between the neighbour and the origin
197  float r = std::sqrt (nn_dists[ne]);
198 
199  /// Project point into the tangent plane
200  Eigen::Vector3f proj;
201  pcl::geometry::project (neighbour, origin, normal, proj);
202  proj -= origin;
203 
204  /// Normalize to compute the dot product
205  proj.normalize ();
206 
207  /// Compute the angle between the projection and the x axis in the interval [0,360]
208  Eigen::Vector3f cross = x_axis.cross (proj);
209  float phi = pcl::rad2deg (std::atan2 (cross.norm (), x_axis.dot (proj)));
210  phi = cross.dot (normal) < 0.f ? (360.0f - phi) : phi;
211  /// Compute the angle between the neighbour and the z axis (normal) in the interval [0, 180]
212  Eigen::Vector3f no = neighbour - origin;
213  no.normalize ();
214  float theta = normal.dot (no);
215  theta = pcl::rad2deg (acosf (std::min (1.0f, std::max (-1.0f, theta))));
216 
217  // Bin (j, k, l)
218  size_t j = 0;
219  size_t k = 0;
220  size_t l = 0;
221 
222  // Compute the Bin(j, k, l) coordinates of current neighbour
223  for (size_t rad = 1; rad < radius_bins_+1; rad++)
224  {
225  if (r <= radii_interval_[rad])
226  {
227  j = rad-1;
228  break;
229  }
230  }
231 
232  for (size_t ang = 1; ang < elevation_bins_+1; ang++)
233  {
234  if (theta <= theta_divisions_[ang])
235  {
236  k = ang-1;
237  break;
238  }
239  }
240 
241  for (size_t ang = 1; ang < azimuth_bins_+1; ang++)
242  {
243  if (phi <= phi_divisions_[ang])
244  {
245  l = ang-1;
246  break;
247  }
248  }
249 
250  // Local point density = number of points in a sphere of radius "point_density_radius_" around the current neighbour
251  std::vector<int> neighbour_indices;
252  std::vector<float> neighbour_distances;
253  int point_density = searchForNeighbors (*surface_, nn_indices[ne], point_density_radius_, neighbour_indices, neighbour_distances);
254  // point_density is NOT always bigger than 0 (on error, searchForNeighbors returns 0), so we must check for that
255  if (point_density == 0)
256  continue;
257 
258  float w = (1.0f / static_cast<float> (point_density)) *
259  volume_lut_[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j];
260 
261  assert (w >= 0.0);
262  if (w == std::numeric_limits<float>::infinity ())
263  PCL_ERROR ("Shape Context Error INF!\n");
264  if (std::isnan(w))
265  PCL_ERROR ("Shape Context Error IND!\n");
266  /// Accumulate w into correspondent Bin(j,k,l)
267  desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] += w;
268 
269  assert (desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] >= 0);
270  } // end for each neighbour
271 
272  // 3DSC does not define a repeatable local RF, we set it to zero to signal it to the user
273  memset (rf, 0, sizeof (rf[0]) * 9);
274  return (true);
275 }
276 
277 //////////////////////////////////////////////////////////////////////////////////////////////
278 template <typename PointInT, typename PointNT, typename PointOutT> void
280 {
281  assert (descriptor_length_ == 1980);
282 
283  output.is_dense = true;
284  // Iterate over all points and compute the descriptors
285  for (size_t point_index = 0; point_index < indices_->size (); point_index++)
286  {
287  //output[point_index].descriptor.resize (descriptor_length_);
288 
289  // If the point is not finite, set the descriptor to NaN and continue
290  if (!isFinite ((*input_)[(*indices_)[point_index]]))
291  {
292  for (size_t i = 0; i < descriptor_length_; ++i)
293  output[point_index].descriptor[i] = std::numeric_limits<float>::quiet_NaN ();
294 
295  memset (output[point_index].rf, 0, sizeof (output[point_index].rf[0]) * 9);
296  output.is_dense = false;
297  continue;
298  }
299 
300  std::vector<float> descriptor (descriptor_length_);
301  if (!computePoint (point_index, *normals_, output[point_index].rf, descriptor))
302  output.is_dense = false;
303  for (size_t j = 0; j < descriptor_length_; ++j)
304  output[point_index].descriptor[j] = descriptor[j];
305  }
306 }
307 
308 #define PCL_INSTANTIATE_ShapeContext3DEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::ShapeContext3DEstimation<T,NT,OutT>;
309 
310 #endif
void project(const PointT &point, const PointT &plane_origin, const NormalT &plane_normal, PointT &projected)
Definition: geometry.h:81
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:53
float deg2rad(float alpha)
Convert an angle from degrees to radians.
Definition: angles.hpp:67
bool initCompute() override
Initialize computation by allocating all the intervals and the volume lookup table.
Definition: 3dsc.hpp:50
typename Feature< PointInT, PointOutT >::PointCloudOut PointCloudOut
Definition: 3dsc.h:89
float rad2deg(float alpha)
Convert an angle from radians to degrees.
Definition: angles.hpp:61
bool computePoint(size_t index, const pcl::PointCloud< PointNT > &normals, float rf[9], std::vector< float > &desc)
Estimate a descriptor for a given point.
Definition: 3dsc.hpp:130
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:54
const Eigen::Map< const Eigen::Vector3f > Vector3fMapConst
void computeFeature(PointCloudOut &output) override
Estimate the actual feature.
Definition: 3dsc.hpp:279