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