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