Point Cloud Library (PCL)  1.11.1-dev
shot.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 
40 #ifndef PCL_FEATURES_IMPL_SHOT_H_
41 #define PCL_FEATURES_IMPL_SHOT_H_
42 
43 #include <pcl/features/shot.h>
44 #include <pcl/features/shot_lrf.h>
45 
46 // Useful constants.
47 #define PST_PI 3.1415926535897932384626433832795
48 #define PST_RAD_45 0.78539816339744830961566084581988
49 #define PST_RAD_90 1.5707963267948966192313216916398
50 #define PST_RAD_135 2.3561944901923449288469825374596
51 #define PST_RAD_180 PST_PI
52 #define PST_RAD_360 6.283185307179586476925286766558
53 #define PST_RAD_PI_7_8 2.7488935718910690836548129603691
54 
55 const double zeroDoubleEps15 = 1E-15;
56 const float zeroFloatEps8 = 1E-8f;
57 
58 //////////////////////////////////////////////////////////////////////////////////////////////
59 /** \brief Check if val1 and val2 are equals.
60  *
61  * \param[in] val1 first number to check.
62  * \param[in] val2 second number to check.
63  * \param[in] zeroDoubleEps epsilon
64  * \return true if val1 is equal to val2, false otherwise.
65  */
66 inline bool
67 areEquals (double val1, double val2, double zeroDoubleEps = zeroDoubleEps15)
68 {
69  return (std::abs (val1 - val2)<zeroDoubleEps);
70 }
71 
72 //////////////////////////////////////////////////////////////////////////////////////////////
73 /** \brief Check if val1 and val2 are equals.
74  *
75  * \param[in] val1 first number to check.
76  * \param[in] val2 second number to check.
77  * \param[in] zeroFloatEps epsilon
78  * \return true if val1 is equal to val2, false otherwise.
79  */
80 inline bool
81 areEquals (float val1, float val2, float zeroFloatEps = zeroFloatEps8)
82 {
83  return (std::fabs (val1 - val2)<zeroFloatEps);
84 }
85 
86 //////////////////////////////////////////////////////////////////////////////////////////////
87 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> float
89 
90 //////////////////////////////////////////////////////////////////////////////////////////////
91 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> float
93 
94 //////////////////////////////////////////////////////////////////////////////////////////////
95 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
97  unsigned char B, float &L, float &A,
98  float &B2)
99 {
100  // @TODO: C++17 supports constexpr lambda for compile time init
101  if (sRGB_LUT[0] < 0)
102  {
103  for (int i = 0; i < 256; i++)
104  {
105  float f = static_cast<float> (i) / 255.0f;
106  if (f > 0.04045)
107  sRGB_LUT[i] = powf ((f + 0.055f) / 1.055f, 2.4f);
108  else
109  sRGB_LUT[i] = f / 12.92f;
110  }
111 
112  for (int i = 0; i < 4000; i++)
113  {
114  float f = static_cast<float> (i) / 4000.0f;
115  if (f > 0.008856)
116  sXYZ_LUT[i] = static_cast<float> (powf (f, 0.3333f));
117  else
118  sXYZ_LUT[i] = static_cast<float>((7.787 * f) + (16.0 / 116.0));
119  }
120  }
121 
122  float fr = sRGB_LUT[R];
123  float fg = sRGB_LUT[G];
124  float fb = sRGB_LUT[B];
125 
126  // Use white = D65
127  const float x = fr * 0.412453f + fg * 0.357580f + fb * 0.180423f;
128  const float y = fr * 0.212671f + fg * 0.715160f + fb * 0.072169f;
129  const float z = fr * 0.019334f + fg * 0.119193f + fb * 0.950227f;
130 
131  float vx = x / 0.95047f;
132  float vy = y;
133  float vz = z / 1.08883f;
134 
135  vx = sXYZ_LUT[int(vx*4000)];
136  vy = sXYZ_LUT[int(vy*4000)];
137  vz = sXYZ_LUT[int(vz*4000)];
138 
139  L = 116.0f * vy - 16.0f;
140  if (L > 100)
141  L = 100.0f;
142 
143  A = 500.0f * (vx - vy);
144  if (A > 120)
145  A = 120.0f;
146  else if (A <- 120)
147  A = -120.0f;
148 
149  B2 = 200.0f * (vy - vz);
150  if (B2 > 120)
151  B2 = 120.0f;
152  else if (B2<- 120)
153  B2 = -120.0f;
154 }
155 
156 //////////////////////////////////////////////////////////////////////////////////////////////
157 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> bool
159 {
161  {
162  PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
163  return (false);
164  }
165 
166  // SHOT cannot work with k-search
167  if (this->getKSearch () != 0)
168  {
169  PCL_ERROR(
170  "[pcl::%s::initCompute] Error! Search method set to k-neighborhood. Call setKSearch(0) and setRadiusSearch( radius ) to use this class.\n",
171  getClassName().c_str ());
172  return (false);
173  }
174 
175  // Default LRF estimation alg: SHOTLocalReferenceFrameEstimation
177  lrf_estimator->setRadiusSearch ((lrf_radius_ > 0 ? lrf_radius_ : search_radius_));
178  lrf_estimator->setInputCloud (input_);
179  lrf_estimator->setIndices (indices_);
180  if (!fake_surface_)
181  lrf_estimator->setSearchSurface(surface_);
182 
184  {
185  PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
186  return (false);
187  }
188 
189  return (true);
190 }
191 
192 //////////////////////////////////////////////////////////////////////////////////////////////
193 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
195  int index,
196  const std::vector<int> &indices,
197  std::vector<double> &bin_distance_shape)
198 {
199  bin_distance_shape.resize (indices.size ());
200 
201  const PointRFT& current_frame = (*frames_)[index];
202  //if (!std::isfinite (current_frame.rf[0]) || !std::isfinite (current_frame.rf[4]) || !std::isfinite (current_frame.rf[11]))
203  //return;
204 
205  Eigen::Vector4f current_frame_z (current_frame.z_axis[0], current_frame.z_axis[1], current_frame.z_axis[2], 0);
206 
207  unsigned nan_counter = 0;
208  for (std::size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
209  {
210  // check NaN normal
211  const Eigen::Vector4f& normal_vec = (*normals_)[indices[i_idx]].getNormalVector4fMap ();
212  if (!std::isfinite (normal_vec[0]) ||
213  !std::isfinite (normal_vec[1]) ||
214  !std::isfinite (normal_vec[2]))
215  {
216  bin_distance_shape[i_idx] = std::numeric_limits<double>::quiet_NaN ();
217  ++nan_counter;
218  } else
219  {
220  //double cosineDesc = feat[i].rf[6]*normal[0] + feat[i].rf[7]*normal[1] + feat[i].rf[8]*normal[2];
221  double cosineDesc = normal_vec.dot (current_frame_z);
222 
223  if (cosineDesc > 1.0)
224  cosineDesc = 1.0;
225  if (cosineDesc < - 1.0)
226  cosineDesc = - 1.0;
227 
228  bin_distance_shape[i_idx] = ((1.0 + cosineDesc) * nr_shape_bins_) / 2;
229  }
230  }
231  if (nan_counter > 0)
232  PCL_WARN ("[pcl::%s::createBinDistanceShape] Point %d has %d (%f%%) NaN normals in its neighbourhood\n",
233  getClassName ().c_str (), index, nan_counter, (static_cast<float>(nan_counter)*100.f/static_cast<float>(indices.size ())));
234 }
235 
236 //////////////////////////////////////////////////////////////////////////////////////////////
237 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
239  Eigen::VectorXf &shot, int desc_length)
240 {
241  // Normalization is performed by considering the L2 norm
242  // and not the sum of bins, as reported in the ECCV paper.
243  // This is due to additional experiments performed by the authors after its pubblication,
244  // where L2 normalization turned out better at handling point density variations.
245  double acc_norm = 0;
246  for (int j = 0; j < desc_length; j++)
247  acc_norm += shot[j] * shot[j];
248  acc_norm = sqrt (acc_norm);
249  for (int j = 0; j < desc_length; j++)
250  shot[j] /= static_cast<float> (acc_norm);
251 }
252 
253 //////////////////////////////////////////////////////////////////////////////////////////////
254 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
256  const std::vector<int> &indices,
257  const std::vector<float> &sqr_dists,
258  const int index,
259  std::vector<double> &binDistance,
260  const int nr_bins,
261  Eigen::VectorXf &shot)
262 {
263  const Eigen::Vector4f& central_point = (*input_)[(*indices_)[index]].getVector4fMap ();
264  const PointRFT& current_frame = (*frames_)[index];
265 
266  Eigen::Vector4f current_frame_x (current_frame.x_axis[0], current_frame.x_axis[1], current_frame.x_axis[2], 0);
267  Eigen::Vector4f current_frame_y (current_frame.y_axis[0], current_frame.y_axis[1], current_frame.y_axis[2], 0);
268  Eigen::Vector4f current_frame_z (current_frame.z_axis[0], current_frame.z_axis[1], current_frame.z_axis[2], 0);
269 
270  for (std::size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
271  {
272  if (!std::isfinite(binDistance[i_idx]))
273  continue;
274 
275  Eigen::Vector4f delta = (*surface_)[indices[i_idx]].getVector4fMap () - central_point;
276  delta[3] = 0;
277 
278  // Compute the Euclidean norm
279  double distance = sqrt (sqr_dists[i_idx]);
280 
281  if (areEquals (distance, 0.0))
282  continue;
283 
284  double xInFeatRef = delta.dot (current_frame_x);
285  double yInFeatRef = delta.dot (current_frame_y);
286  double zInFeatRef = delta.dot (current_frame_z);
287 
288  // To avoid numerical problems afterwards
289  if (std::abs (yInFeatRef) < 1E-30)
290  yInFeatRef = 0;
291  if (std::abs (xInFeatRef) < 1E-30)
292  xInFeatRef = 0;
293  if (std::abs (zInFeatRef) < 1E-30)
294  zInFeatRef = 0;
295 
296 
297  unsigned char bit4 = ((yInFeatRef > 0) || ((yInFeatRef == 0.0) && (xInFeatRef < 0))) ? 1 : 0;
298  unsigned char bit3 = static_cast<unsigned char> (((xInFeatRef > 0) || ((xInFeatRef == 0.0) && (yInFeatRef > 0))) ? !bit4 : bit4);
299 
300  assert (bit3 == 0 || bit3 == 1);
301 
302  int desc_index = (bit4<<3) + (bit3<<2);
303 
304  desc_index = desc_index << 1;
305 
306  if ((xInFeatRef * yInFeatRef > 0) || (xInFeatRef == 0.0))
307  desc_index += (std::abs (xInFeatRef) >= std::abs (yInFeatRef)) ? 0 : 4;
308  else
309  desc_index += (std::abs (xInFeatRef) > std::abs (yInFeatRef)) ? 4 : 0;
310 
311  desc_index += zInFeatRef > 0 ? 1 : 0;
312 
313  // 2 RADII
314  desc_index += (distance > radius1_2_) ? 2 : 0;
315 
316  int step_index = static_cast<int>(std::floor (binDistance[i_idx] +0.5));
317  int volume_index = desc_index * (nr_bins+1);
318 
319  //Interpolation on the cosine (adjacent bins in the histogram)
320  binDistance[i_idx] -= step_index;
321  double intWeight = (1- std::abs (binDistance[i_idx]));
322 
323  if (binDistance[i_idx] > 0)
324  shot[volume_index + ((step_index+1) % nr_bins)] += static_cast<float> (binDistance[i_idx]);
325  else
326  shot[volume_index + ((step_index - 1 + nr_bins) % nr_bins)] += - static_cast<float> (binDistance[i_idx]);
327 
328  //Interpolation on the distance (adjacent husks)
329 
330  if (distance > radius1_2_) //external sphere
331  {
332  double radiusDistance = (distance - radius3_4_) / radius1_2_;
333 
334  if (distance > radius3_4_) //most external sector, votes only for itself
335  intWeight += 1 - radiusDistance; //peso=1-d
336  else //3/4 of radius, votes also for the internal sphere
337  {
338  intWeight += 1 + radiusDistance;
339  shot[(desc_index - 2) * (nr_bins+1) + step_index] -= static_cast<float> (radiusDistance);
340  }
341  }
342  else //internal sphere
343  {
344  double radiusDistance = (distance - radius1_4_) / radius1_2_;
345 
346  if (distance < radius1_4_) //most internal sector, votes only for itself
347  intWeight += 1 + radiusDistance; //weight=1-d
348  else //3/4 of radius, votes also for the external sphere
349  {
350  intWeight += 1 - radiusDistance;
351  shot[(desc_index + 2) * (nr_bins+1) + step_index] += static_cast<float> (radiusDistance);
352  }
353  }
354 
355  //Interpolation on the inclination (adjacent vertical volumes)
356  double inclinationCos = zInFeatRef / distance;
357  if (inclinationCos < - 1.0)
358  inclinationCos = - 1.0;
359  if (inclinationCos > 1.0)
360  inclinationCos = 1.0;
361 
362  double inclination = std::acos (inclinationCos);
363 
364  assert (inclination >= 0.0 && inclination <= PST_RAD_180);
365 
366  if (inclination > PST_RAD_90 || (std::abs (inclination - PST_RAD_90) < 1e-30 && zInFeatRef <= 0))
367  {
368  double inclinationDistance = (inclination - PST_RAD_135) / PST_RAD_90;
369  if (inclination > PST_RAD_135)
370  intWeight += 1 - inclinationDistance;
371  else
372  {
373  intWeight += 1 + inclinationDistance;
374  assert ((desc_index + 1) * (nr_bins+1) + step_index >= 0 && (desc_index + 1) * (nr_bins+1) + step_index < descLength_);
375  shot[(desc_index + 1) * (nr_bins+1) + step_index] -= static_cast<float> (inclinationDistance);
376  }
377  }
378  else
379  {
380  double inclinationDistance = (inclination - PST_RAD_45) / PST_RAD_90;
381  if (inclination < PST_RAD_45)
382  intWeight += 1 + inclinationDistance;
383  else
384  {
385  intWeight += 1 - inclinationDistance;
386  assert ((desc_index - 1) * (nr_bins+1) + step_index >= 0 && (desc_index - 1) * (nr_bins+1) + step_index < descLength_);
387  shot[(desc_index - 1) * (nr_bins+1) + step_index] += static_cast<float> (inclinationDistance);
388  }
389  }
390 
391  if (yInFeatRef != 0.0 || xInFeatRef != 0.0)
392  {
393  //Interpolation on the azimuth (adjacent horizontal volumes)
394  double azimuth = std::atan2 (yInFeatRef, xInFeatRef);
395 
396  int sel = desc_index >> 2;
397  double angularSectorSpan = PST_RAD_45;
398  double angularSectorStart = - PST_RAD_PI_7_8;
399 
400  double azimuthDistance = (azimuth - (angularSectorStart + angularSectorSpan*sel)) / angularSectorSpan;
401 
402  assert ((azimuthDistance < 0.5 || areEquals (azimuthDistance, 0.5)) && (azimuthDistance > - 0.5 || areEquals (azimuthDistance, - 0.5)));
403 
404  azimuthDistance = (std::max)(- 0.5, std::min (azimuthDistance, 0.5));
405 
406  if (azimuthDistance > 0)
407  {
408  intWeight += 1 - azimuthDistance;
409  int interp_index = (desc_index + 4) % maxAngularSectors_;
410  assert (interp_index * (nr_bins+1) + step_index >= 0 && interp_index * (nr_bins+1) + step_index < descLength_);
411  shot[interp_index * (nr_bins+1) + step_index] += static_cast<float> (azimuthDistance);
412  }
413  else
414  {
415  int interp_index = (desc_index - 4 + maxAngularSectors_) % maxAngularSectors_;
416  assert (interp_index * (nr_bins+1) + step_index >= 0 && interp_index * (nr_bins+1) + step_index < descLength_);
417  intWeight += 1 + azimuthDistance;
418  shot[interp_index * (nr_bins+1) + step_index] -= static_cast<float> (azimuthDistance);
419  }
420 
421  }
422 
423  assert (volume_index + step_index >= 0 && volume_index + step_index < descLength_);
424  shot[volume_index + step_index] += static_cast<float> (intWeight);
425  }
426 }
427 
428 //////////////////////////////////////////////////////////////////////////////////////////////
429 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
431  const std::vector<int> &indices,
432  const std::vector<float> &sqr_dists,
433  const int index,
434  std::vector<double> &binDistanceShape,
435  std::vector<double> &binDistanceColor,
436  const int nr_bins_shape,
437  const int nr_bins_color,
438  Eigen::VectorXf &shot)
439 {
440  const Eigen::Vector4f &central_point = (*input_)[(*indices_)[index]].getVector4fMap ();
441  const PointRFT& current_frame = (*frames_)[index];
442 
443  int shapeToColorStride = nr_grid_sector_*(nr_bins_shape+1);
444 
445  Eigen::Vector4f current_frame_x (current_frame.x_axis[0], current_frame.x_axis[1], current_frame.x_axis[2], 0);
446  Eigen::Vector4f current_frame_y (current_frame.y_axis[0], current_frame.y_axis[1], current_frame.y_axis[2], 0);
447  Eigen::Vector4f current_frame_z (current_frame.z_axis[0], current_frame.z_axis[1], current_frame.z_axis[2], 0);
448 
449  for (std::size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
450  {
451  if (!std::isfinite(binDistanceShape[i_idx]))
452  continue;
453 
454  Eigen::Vector4f delta = (*surface_)[indices[i_idx]].getVector4fMap () - central_point;
455  delta[3] = 0;
456 
457  // Compute the Euclidean norm
458  double distance = sqrt (sqr_dists[i_idx]);
459 
460  if (areEquals (distance, 0.0))
461  continue;
462 
463  double xInFeatRef = delta.dot (current_frame_x);
464  double yInFeatRef = delta.dot (current_frame_y);
465  double zInFeatRef = delta.dot (current_frame_z);
466 
467  // To avoid numerical problems afterwards
468  if (std::abs (yInFeatRef) < 1E-30)
469  yInFeatRef = 0;
470  if (std::abs (xInFeatRef) < 1E-30)
471  xInFeatRef = 0;
472  if (std::abs (zInFeatRef) < 1E-30)
473  zInFeatRef = 0;
474 
475  unsigned char bit4 = ((yInFeatRef > 0) || ((yInFeatRef == 0.0) && (xInFeatRef < 0))) ? 1 : 0;
476  unsigned char bit3 = static_cast<unsigned char> (((xInFeatRef > 0) || ((xInFeatRef == 0.0) && (yInFeatRef > 0))) ? !bit4 : bit4);
477 
478  assert (bit3 == 0 || bit3 == 1);
479 
480  int desc_index = (bit4<<3) + (bit3<<2);
481 
482  desc_index = desc_index << 1;
483 
484  if ((xInFeatRef * yInFeatRef > 0) || (xInFeatRef == 0.0))
485  desc_index += (std::abs (xInFeatRef) >= std::abs (yInFeatRef)) ? 0 : 4;
486  else
487  desc_index += (std::abs (xInFeatRef) > std::abs (yInFeatRef)) ? 4 : 0;
488 
489  desc_index += zInFeatRef > 0 ? 1 : 0;
490 
491  // 2 RADII
492  desc_index += (distance > radius1_2_) ? 2 : 0;
493 
494  int step_index_shape = static_cast<int>(std::floor (binDistanceShape[i_idx] +0.5));
495  int step_index_color = static_cast<int>(std::floor (binDistanceColor[i_idx] +0.5));
496 
497  int volume_index_shape = desc_index * (nr_bins_shape+1);
498  int volume_index_color = shapeToColorStride + desc_index * (nr_bins_color+1);
499 
500  //Interpolation on the cosine (adjacent bins in the histrogram)
501  binDistanceShape[i_idx] -= step_index_shape;
502  binDistanceColor[i_idx] -= step_index_color;
503 
504  double intWeightShape = (1- std::abs (binDistanceShape[i_idx]));
505  double intWeightColor = (1- std::abs (binDistanceColor[i_idx]));
506 
507  if (binDistanceShape[i_idx] > 0)
508  shot[volume_index_shape + ((step_index_shape + 1) % nr_bins_shape)] += static_cast<float> (binDistanceShape[i_idx]);
509  else
510  shot[volume_index_shape + ((step_index_shape - 1 + nr_bins_shape) % nr_bins_shape)] -= static_cast<float> (binDistanceShape[i_idx]);
511 
512  if (binDistanceColor[i_idx] > 0)
513  shot[volume_index_color + ((step_index_color+1) % nr_bins_color)] += static_cast<float> (binDistanceColor[i_idx]);
514  else
515  shot[volume_index_color + ((step_index_color - 1 + nr_bins_color) % nr_bins_color)] -= static_cast<float> (binDistanceColor[i_idx]);
516 
517  //Interpolation on the distance (adjacent husks)
518 
519  if (distance > radius1_2_) //external sphere
520  {
521  double radiusDistance = (distance - radius3_4_) / radius1_2_;
522 
523  if (distance > radius3_4_) //most external sector, votes only for itself
524  {
525  intWeightShape += 1 - radiusDistance; //weight=1-d
526  intWeightColor += 1 - radiusDistance; //weight=1-d
527  }
528  else //3/4 of radius, votes also for the internal sphere
529  {
530  intWeightShape += 1 + radiusDistance;
531  intWeightColor += 1 + radiusDistance;
532  shot[(desc_index - 2) * (nr_bins_shape+1) + step_index_shape] -= static_cast<float> (radiusDistance);
533  shot[shapeToColorStride + (desc_index - 2) * (nr_bins_color+1) + step_index_color] -= static_cast<float> (radiusDistance);
534  }
535  }
536  else //internal sphere
537  {
538  double radiusDistance = (distance - radius1_4_) / radius1_2_;
539 
540  if (distance < radius1_4_) //most internal sector, votes only for itself
541  {
542  intWeightShape += 1 + radiusDistance;
543  intWeightColor += 1 + radiusDistance; //weight=1-d
544  }
545  else //3/4 of radius, votes also for the external sphere
546  {
547  intWeightShape += 1 - radiusDistance; //weight=1-d
548  intWeightColor += 1 - radiusDistance; //weight=1-d
549  shot[(desc_index + 2) * (nr_bins_shape+1) + step_index_shape] += static_cast<float> (radiusDistance);
550  shot[shapeToColorStride + (desc_index + 2) * (nr_bins_color+1) + step_index_color] += static_cast<float> (radiusDistance);
551  }
552  }
553 
554  //Interpolation on the inclination (adjacent vertical volumes)
555  double inclinationCos = zInFeatRef / distance;
556  if (inclinationCos < - 1.0)
557  inclinationCos = - 1.0;
558  if (inclinationCos > 1.0)
559  inclinationCos = 1.0;
560 
561  double inclination = std::acos (inclinationCos);
562 
563  assert (inclination >= 0.0 && inclination <= PST_RAD_180);
564 
565  if (inclination > PST_RAD_90 || (std::abs (inclination - PST_RAD_90) < 1e-30 && zInFeatRef <= 0))
566  {
567  double inclinationDistance = (inclination - PST_RAD_135) / PST_RAD_90;
568  if (inclination > PST_RAD_135)
569  {
570  intWeightShape += 1 - inclinationDistance;
571  intWeightColor += 1 - inclinationDistance;
572  }
573  else
574  {
575  intWeightShape += 1 + inclinationDistance;
576  intWeightColor += 1 + inclinationDistance;
577  assert ((desc_index + 1) * (nr_bins_shape+1) + step_index_shape >= 0 && (desc_index + 1) * (nr_bins_shape+1) + step_index_shape < descLength_);
578  assert (shapeToColorStride + (desc_index + 1) * (nr_bins_color+ 1) + step_index_color >= 0 && shapeToColorStride + (desc_index + 1) * (nr_bins_color+1) + step_index_color < descLength_);
579  shot[(desc_index + 1) * (nr_bins_shape+1) + step_index_shape] -= static_cast<float> (inclinationDistance);
580  shot[shapeToColorStride + (desc_index + 1) * (nr_bins_color+1) + step_index_color] -= static_cast<float> (inclinationDistance);
581  }
582  }
583  else
584  {
585  double inclinationDistance = (inclination - PST_RAD_45) / PST_RAD_90;
586  if (inclination < PST_RAD_45)
587  {
588  intWeightShape += 1 + inclinationDistance;
589  intWeightColor += 1 + inclinationDistance;
590  }
591  else
592  {
593  intWeightShape += 1 - inclinationDistance;
594  intWeightColor += 1 - inclinationDistance;
595  assert ((desc_index - 1) * (nr_bins_shape+1) + step_index_shape >= 0 && (desc_index - 1) * (nr_bins_shape+1) + step_index_shape < descLength_);
596  assert (shapeToColorStride + (desc_index - 1) * (nr_bins_color+ 1) + step_index_color >= 0 && shapeToColorStride + (desc_index - 1) * (nr_bins_color+1) + step_index_color < descLength_);
597  shot[(desc_index - 1) * (nr_bins_shape+1) + step_index_shape] += static_cast<float> (inclinationDistance);
598  shot[shapeToColorStride + (desc_index - 1) * (nr_bins_color+1) + step_index_color] += static_cast<float> (inclinationDistance);
599  }
600  }
601 
602  if (yInFeatRef != 0.0 || xInFeatRef != 0.0)
603  {
604  //Interpolation on the azimuth (adjacent horizontal volumes)
605  double azimuth = std::atan2 (yInFeatRef, xInFeatRef);
606 
607  int sel = desc_index >> 2;
608  double angularSectorSpan = PST_RAD_45;
609  double angularSectorStart = - PST_RAD_PI_7_8;
610 
611  double azimuthDistance = (azimuth - (angularSectorStart + angularSectorSpan*sel)) / angularSectorSpan;
612  assert ((azimuthDistance < 0.5 || areEquals (azimuthDistance, 0.5)) && (azimuthDistance > - 0.5 || areEquals (azimuthDistance, - 0.5)));
613  azimuthDistance = (std::max)(- 0.5, std::min (azimuthDistance, 0.5));
614 
615  if (azimuthDistance > 0)
616  {
617  intWeightShape += 1 - azimuthDistance;
618  intWeightColor += 1 - azimuthDistance;
619  int interp_index = (desc_index + 4) % maxAngularSectors_;
620  assert (interp_index * (nr_bins_shape+1) + step_index_shape >= 0 && interp_index * (nr_bins_shape+1) + step_index_shape < descLength_);
621  assert (shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color >= 0 && shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color < descLength_);
622  shot[interp_index * (nr_bins_shape+1) + step_index_shape] += static_cast<float> (azimuthDistance);
623  shot[shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color] += static_cast<float> (azimuthDistance);
624  }
625  else
626  {
627  int interp_index = (desc_index - 4 + maxAngularSectors_) % maxAngularSectors_;
628  intWeightShape += 1 + azimuthDistance;
629  intWeightColor += 1 + azimuthDistance;
630  assert (interp_index * (nr_bins_shape+1) + step_index_shape >= 0 && interp_index * (nr_bins_shape+1) + step_index_shape < descLength_);
631  assert (shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color >= 0 && shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color < descLength_);
632  shot[interp_index * (nr_bins_shape+1) + step_index_shape] -= static_cast<float> (azimuthDistance);
633  shot[shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color] -= static_cast<float> (azimuthDistance);
634  }
635  }
636 
637  assert (volume_index_shape + step_index_shape >= 0 && volume_index_shape + step_index_shape < descLength_);
638  assert (volume_index_color + step_index_color >= 0 && volume_index_color + step_index_color < descLength_);
639  shot[volume_index_shape + step_index_shape] += static_cast<float> (intWeightShape);
640  shot[volume_index_color + step_index_color] += static_cast<float> (intWeightColor);
641  }
642 }
643 
644 //////////////////////////////////////////////////////////////////////////////////////////////
645 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
647  const int index, const std::vector<int> &indices, const std::vector<float> &sqr_dists, Eigen::VectorXf &shot)
648 {
649  // Clear the resultant shot
650  shot.setZero ();
651  const auto nNeighbors = indices.size ();
652  //Skip the current feature if the number of its neighbors is not sufficient for its description
653  if (nNeighbors < 5)
654  {
655  PCL_WARN ("[pcl::%s::computePointSHOT] Warning! Neighborhood has less than 5 vertexes. Aborting description of point with index %d\n",
656  getClassName ().c_str (), (*indices_)[index]);
657 
658  shot.setConstant(descLength_, 1, std::numeric_limits<float>::quiet_NaN () );
659 
660  return;
661  }
662 
663  //If shape description is enabled, compute the bins activated by each neighbor of the current feature in the shape histogram
664  std::vector<double> binDistanceShape;
665  if (b_describe_shape_)
666  {
667  this->createBinDistanceShape (index, indices, binDistanceShape);
668  }
669 
670  //If color description is enabled, compute the bins activated by each neighbor of the current feature in the color histogram
671  std::vector<double> binDistanceColor;
672  if (b_describe_color_)
673  {
674  binDistanceColor.reserve (nNeighbors);
675 
676  //unsigned char redRef = (*input_)[(*indices_)[index]].rgba >> 16 & 0xFF;
677  //unsigned char greenRef = (*input_)[(*indices_)[index]].rgba >> 8& 0xFF;
678  //unsigned char blueRef = (*input_)[(*indices_)[index]].rgba & 0xFF;
679  unsigned char redRef = (*input_)[(*indices_)[index]].r;
680  unsigned char greenRef = (*input_)[(*indices_)[index]].g;
681  unsigned char blueRef = (*input_)[(*indices_)[index]].b;
682 
683  float LRef, aRef, bRef;
684 
685  RGB2CIELAB (redRef, greenRef, blueRef, LRef, aRef, bRef);
686  LRef /= 100.0f;
687  aRef /= 120.0f;
688  bRef /= 120.0f; //normalized LAB components (0<L<1, -1<a<1, -1<b<1)
689 
690  for (const auto& idx: indices)
691  {
692  unsigned char red = (*surface_)[idx].r;
693  unsigned char green = (*surface_)[idx].g;
694  unsigned char blue = (*surface_)[idx].b;
695 
696  float L, a, b;
697 
698  RGB2CIELAB (red, green, blue, L, a, b);
699  L /= 100.0f;
700  a /= 120.0f;
701  b /= 120.0f; //normalized LAB components (0<L<1, -1<a<1, -1<b<1)
702 
703  double colorDistance = (std::fabs (LRef - L) + ((std::fabs (aRef - a) + std::fabs (bRef - b)) / 2)) /3;
704 
705  if (colorDistance > 1.0)
706  colorDistance = 1.0;
707  if (colorDistance < 0.0)
708  colorDistance = 0.0;
709 
710  binDistanceColor.push_back (colorDistance * nr_color_bins_);
711  }
712  }
713 
714  //Apply quadrilinear interpolation on the activated bins in the shape and/or color histogram(s)
715 
716  if (b_describe_shape_ && b_describe_color_)
717  interpolateDoubleChannel (indices, sqr_dists, index, binDistanceShape, binDistanceColor,
718  nr_shape_bins_, nr_color_bins_,
719  shot);
720  else if (b_describe_color_)
721  interpolateSingleChannel (indices, sqr_dists, index, binDistanceColor, nr_color_bins_, shot);
722  else
723  interpolateSingleChannel (indices, sqr_dists, index, binDistanceShape, nr_shape_bins_, shot);
724 
725  // Normalize the final histogram
726  this->normalizeHistogram (shot, descLength_);
727 }
728 
729 //////////////////////////////////////////////////////////////////////////////////////////////
730 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
732  const int index, const std::vector<int> &indices, const std::vector<float> &sqr_dists, Eigen::VectorXf &shot)
733 {
734  //Skip the current feature if the number of its neighbors is not sufficient for its description
735  if (indices.size () < 5)
736  {
737  PCL_WARN ("[pcl::%s::computePointSHOT] Warning! Neighborhood has less than 5 vertexes. Aborting description of point with index %d\n",
738  getClassName ().c_str (), (*indices_)[index]);
739 
740  shot.setConstant(descLength_, 1, std::numeric_limits<float>::quiet_NaN () );
741 
742  return;
743  }
744 
745  // Clear the resultant shot
746  std::vector<double> binDistanceShape;
747  this->createBinDistanceShape (index, indices, binDistanceShape);
748 
749  // Interpolate
750  shot.setZero ();
751  interpolateSingleChannel (indices, sqr_dists, index, binDistanceShape, nr_shape_bins_, shot);
752 
753  // Normalize the final histogram
754  this->normalizeHistogram (shot, descLength_);
755 }
756 
757 //////////////////////////////////////////////////////////////////////////////////////////////
758 //////////////////////////////////////////////////////////////////////////////////////////////
759 //////////////////////////////////////////////////////////////////////////////////////////////
760 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
762 {
763  descLength_ = nr_grid_sector_ * (nr_shape_bins_+1);
764 
765  sqradius_ = search_radius_ * search_radius_;
766  radius3_4_ = (search_radius_*3) / 4;
767  radius1_4_ = search_radius_ / 4;
768  radius1_2_ = search_radius_ / 2;
769 
770  assert(descLength_ == 352);
771 
772  shot_.setZero (descLength_);
773 
774  // Allocate enough space to hold the results
775  // \note This resize is irrelevant for a radiusSearch ().
776  std::vector<int> nn_indices (k_);
777  std::vector<float> nn_dists (k_);
778 
779  output.is_dense = true;
780  // Iterating over the entire index vector
781  for (std::size_t idx = 0; idx < indices_->size (); ++idx)
782  {
783  bool lrf_is_nan = false;
784  const PointRFT& current_frame = (*frames_)[idx];
785  if (!std::isfinite (current_frame.x_axis[0]) ||
786  !std::isfinite (current_frame.y_axis[0]) ||
787  !std::isfinite (current_frame.z_axis[0]))
788  {
789  PCL_WARN ("[pcl::%s::computeFeature] The local reference frame is not valid! Aborting description of point with index %d\n",
790  getClassName ().c_str (), (*indices_)[idx]);
791  lrf_is_nan = true;
792  }
793 
794  if (!isFinite ((*input_)[(*indices_)[idx]]) ||
795  lrf_is_nan ||
796  this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
797  {
798  // Copy into the resultant cloud
799  for (int d = 0; d < descLength_; ++d)
800  output[idx].descriptor[d] = std::numeric_limits<float>::quiet_NaN ();
801  for (int d = 0; d < 9; ++d)
802  output[idx].rf[d] = std::numeric_limits<float>::quiet_NaN ();
803 
804  output.is_dense = false;
805  continue;
806  }
807 
808  // Estimate the SHOT descriptor at each patch
809  computePointSHOT (static_cast<int> (idx), nn_indices, nn_dists, shot_);
810 
811  // Copy into the resultant cloud
812  for (int d = 0; d < descLength_; ++d)
813  output[idx].descriptor[d] = shot_[d];
814  for (int d = 0; d < 3; ++d)
815  {
816  output[idx].rf[d + 0] = (*frames_)[idx].x_axis[d];
817  output[idx].rf[d + 3] = (*frames_)[idx].y_axis[d];
818  output[idx].rf[d + 6] = (*frames_)[idx].z_axis[d];
819  }
820  }
821 }
822 
823 //////////////////////////////////////////////////////////////////////////////////////////////
824 //////////////////////////////////////////////////////////////////////////////////////////////
825 //////////////////////////////////////////////////////////////////////////////////////////////
826 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
828 {
829  // Compute the current length of the descriptor
830  descLength_ = (b_describe_shape_) ? nr_grid_sector_*(nr_shape_bins_+1) : 0;
831  descLength_ += (b_describe_color_) ? nr_grid_sector_*(nr_color_bins_+1) : 0;
832 
833  assert( (!b_describe_color_ && b_describe_shape_ && descLength_ == 352) ||
834  (b_describe_color_ && !b_describe_shape_ && descLength_ == 992) ||
835  (b_describe_color_ && b_describe_shape_ && descLength_ == 1344)
836  );
837 
838  // Useful values
839  sqradius_ = search_radius_*search_radius_;
840  radius3_4_ = (search_radius_*3) / 4;
841  radius1_4_ = search_radius_ / 4;
842  radius1_2_ = search_radius_ / 2;
843 
844  shot_.setZero (descLength_);
845 
846  // Allocate enough space to hold the results
847  // \note This resize is irrelevant for a radiusSearch ().
848  std::vector<int> nn_indices (k_);
849  std::vector<float> nn_dists (k_);
850 
851  output.is_dense = true;
852  // Iterating over the entire index vector
853  for (std::size_t idx = 0; idx < indices_->size (); ++idx)
854  {
855  bool lrf_is_nan = false;
856  const PointRFT& current_frame = (*frames_)[idx];
857  if (!std::isfinite (current_frame.x_axis[0]) ||
858  !std::isfinite (current_frame.y_axis[0]) ||
859  !std::isfinite (current_frame.z_axis[0]))
860  {
861  PCL_WARN ("[pcl::%s::computeFeature] The local reference frame is not valid! Aborting description of point with index %d\n",
862  getClassName ().c_str (), (*indices_)[idx]);
863  lrf_is_nan = true;
864  }
865 
866  if (!isFinite ((*input_)[(*indices_)[idx]]) ||
867  lrf_is_nan ||
868  this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
869  {
870  // Copy into the resultant cloud
871  for (int d = 0; d < descLength_; ++d)
872  output[idx].descriptor[d] = std::numeric_limits<float>::quiet_NaN ();
873  for (int d = 0; d < 9; ++d)
874  output[idx].rf[d] = std::numeric_limits<float>::quiet_NaN ();
875 
876  output.is_dense = false;
877  continue;
878  }
879 
880  // Compute the SHOT descriptor for the current 3D feature
881  computePointSHOT (static_cast<int> (idx), nn_indices, nn_dists, shot_);
882 
883  // Copy into the resultant cloud
884  for (int d = 0; d < descLength_; ++d)
885  output[idx].descriptor[d] = shot_[d];
886  for (int d = 0; d < 3; ++d)
887  {
888  output[idx].rf[d + 0] = (*frames_)[idx].x_axis[d];
889  output[idx].rf[d + 3] = (*frames_)[idx].y_axis[d];
890  output[idx].rf[d + 6] = (*frames_)[idx].z_axis[d];
891  }
892  }
893 }
894 
895 #define PCL_INSTANTIATE_SHOTEstimationBase(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTEstimationBase<T,NT,OutT,RFT>;
896 #define PCL_INSTANTIATE_SHOTEstimation(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTEstimation<T,NT,OutT,RFT>;
897 #define PCL_INSTANTIATE_SHOTColorEstimation(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTColorEstimation<T,NT,OutT,RFT>;
898 
899 #endif // PCL_FEATURES_IMPL_SHOT_H_
pcl::FeatureWithLocalReferenceFrames
FeatureWithLocalReferenceFrames provides a public interface for descriptor extractor classes which ne...
Definition: feature.h:448
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
pcl::SHOTColorEstimation::RGB2CIELAB
static void RGB2CIELAB(unsigned char R, unsigned char G, unsigned char B, float &L, float &A, float &B2)
Converts RGB triplets to CIELab space.
Definition: shot.hpp:96
pcl::SHOTEstimationBase::initCompute
bool initCompute() override
This method should get called before starting the actual computation.
Definition: shot.hpp:158
pcl::PointCloud< PointOutT >
pcl::PCLBase< PointInT >::setInputCloud
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
Definition: pcl_base.hpp:65
pcl::SHOTColorEstimation::interpolateDoubleChannel
void interpolateDoubleChannel(const std::vector< int > &indices, const std::vector< float > &sqr_dists, const int index, std::vector< double > &binDistanceShape, std::vector< double > &binDistanceColor, const int nr_bins_shape, const int nr_bins_color, Eigen::VectorXf &shot)
Quadrilinear interpolation; used when color and shape descriptions are both activated.
Definition: shot.hpp:430
pcl::SHOTEstimationBase::createBinDistanceShape
void createBinDistanceShape(int index, const std::vector< int > &indices, std::vector< double > &bin_distance_shape)
Create a binned distance shape histogram.
Definition: shot.hpp:194
pcl::SHOTColorEstimation::computeFeature
void computeFeature(pcl::PointCloud< PointOutT > &output) override
Estimate the Signatures of Histograms of OrienTations (SHOT) descriptors at a set of points given by ...
Definition: shot.hpp:827
pcl::SHOTColorEstimation::computePointSHOT
void computePointSHOT(const int index, const std::vector< int > &indices, const std::vector< float > &sqr_dists, Eigen::VectorXf &shot) override
Estimate the SHOT descriptor for a given point based on its spatial neighborhood of 3D points with no...
Definition: shot.hpp:646
pcl::SHOTLocalReferenceFrameEstimation
SHOTLocalReferenceFrameEstimation estimates the Local Reference Frame used in the calculation of the ...
Definition: shot_lrf.h:65
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::PointCloud::is_dense
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
Definition: point_cloud.h:418
pcl::FeatureFromNormals
Definition: feature.h:311
pcl::SHOTEstimation::computeFeature
void computeFeature(pcl::PointCloud< PointOutT > &output) override
Estimate the Signatures of Histograms of OrienTations (SHOT) descriptors at a set of points given by ...
Definition: shot.hpp:761
pcl::SHOTLocalReferenceFrameEstimation::Ptr
shared_ptr< SHOTLocalReferenceFrameEstimation< PointInT, PointOutT > > Ptr
Definition: shot_lrf.h:68
pcl::SHOTEstimation::computePointSHOT
void computePointSHOT(const int index, const std::vector< int > &indices, const std::vector< float > &sqr_dists, Eigen::VectorXf &shot) override
Estimate the SHOT descriptor for a given point based on its spatial neighborhood of 3D points with no...
Definition: shot.hpp:731
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::SHOTEstimationBase::normalizeHistogram
void normalizeHistogram(Eigen::VectorXf &shot, int desc_length)
Normalize the SHOT histogram.
Definition: shot.hpp:238
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::B
@ B
Definition: norms.h:54
pcl::SHOTColorEstimation
SHOTColorEstimation estimates the Signature of Histograms of OrienTations (SHOT) descriptor for a giv...
Definition: shot.h:297
pcl::SHOTEstimationBase::interpolateSingleChannel
void interpolateSingleChannel(const std::vector< int > &indices, const std::vector< float > &sqr_dists, const int index, std::vector< double > &binDistance, const int nr_bins, Eigen::VectorXf &shot)
Quadrilinear interpolation used when color and shape descriptions are NOT activated simultaneously.
Definition: shot.hpp:255