Point Cloud Library (PCL)  1.9.1
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Modules Pages
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 (fabs (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 (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  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_->points[index];
202  //if (!pcl_isfinite (current_frame.rf[0]) || !pcl_isfinite (current_frame.rf[4]) || !pcl_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 (size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
209  {
210  // check NaN normal
211  const Eigen::Vector4f& normal_vec = normals_->points[indices[i_idx]].getNormalVector4fMap ();
212  if (!pcl_isfinite (normal_vec[0]) ||
213  !pcl_isfinite (normal_vec[1]) ||
214  !pcl_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 (size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
271  {
272  if (!pcl_isfinite(binDistance[i_idx]))
273  continue;
274 
275  Eigen::Vector4f delta = surface_->points[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 (fabs (yInFeatRef) < 1E-30)
290  yInFeatRef = 0;
291  if (fabs (xInFeatRef) < 1E-30)
292  xInFeatRef = 0;
293  if (fabs (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 += (fabs (xInFeatRef) >= fabs (yInFeatRef)) ? 0 : 4;
308  else
309  desc_index += (fabs (xInFeatRef) > fabs (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>(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- fabs (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 = acos (inclinationCos);
363 
364  assert (inclination >= 0.0 && inclination <= PST_RAD_180);
365 
366  if (inclination > PST_RAD_90 || (fabs (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 = 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 (size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
450  {
451  if (!pcl_isfinite(binDistanceShape[i_idx]))
452  continue;
453 
454  Eigen::Vector4f delta = surface_->points[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 (fabs (yInFeatRef) < 1E-30)
469  yInFeatRef = 0;
470  if (fabs (xInFeatRef) < 1E-30)
471  xInFeatRef = 0;
472  if (fabs (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 += (fabs (xInFeatRef) >= fabs (yInFeatRef)) ? 0 : 4;
486  else
487  desc_index += (fabs (xInFeatRef) > fabs (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>(floor (binDistanceShape[i_idx] +0.5));
495  int step_index_color = static_cast<int>(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- fabs (binDistanceShape[i_idx]));
505  double intWeightColor = (1- fabs (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 = acos (inclinationCos);
562 
563  assert (inclination >= 0.0 && inclination <= PST_RAD_180);
564 
565  if (inclination > PST_RAD_90 || (fabs (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 = 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  std::vector<double> binDistanceShape;
652  std::vector<double> binDistanceColor;
653  size_t nNeighbors = indices.size ();
654  //Skip the current feature if the number of its neighbors is not sufficient for its description
655  if (nNeighbors < 5)
656  {
657  PCL_WARN ("[pcl::%s::computePointSHOT] Warning! Neighborhood has less than 5 vertexes. Aborting description of point with index %d\n",
658  getClassName ().c_str (), (*indices_)[index]);
659 
660  shot.setConstant(descLength_, 1, std::numeric_limits<float>::quiet_NaN () );
661 
662  return;
663  }
664 
665  //If shape description is enabled, compute the bins activated by each neighbor of the current feature in the shape histogram
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  if (b_describe_color_)
673  {
674  binDistanceColor.resize (nNeighbors);
675 
676  //unsigned char redRef = input_->points[(*indices_)[index]].rgba >> 16 & 0xFF;
677  //unsigned char greenRef = input_->points[(*indices_)[index]].rgba >> 8& 0xFF;
678  //unsigned char blueRef = input_->points[(*indices_)[index]].rgba & 0xFF;
679  unsigned char redRef = input_->points[(*indices_)[index]].r;
680  unsigned char greenRef = input_->points[(*indices_)[index]].g;
681  unsigned char blueRef = input_->points[(*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 (size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
691  {
692  //unsigned char red = surface_->points[indices[i_idx]].rgba >> 16 & 0xFF;
693  //unsigned char green = surface_->points[indices[i_idx]].rgba >> 8 & 0xFF;
694  //unsigned char blue = surface_->points[indices[i_idx]].rgba & 0xFF;
695  unsigned char red = surface_->points[indices[i_idx]].r;
696  unsigned char green = surface_->points[indices[i_idx]].g;
697  unsigned char blue = surface_->points[indices[i_idx]].b;
698 
699  float L, a, b;
700 
701  RGB2CIELAB (red, green, blue, L, a, b);
702  L /= 100.0f;
703  a /= 120.0f;
704  b /= 120.0f; //normalized LAB components (0<L<1, -1<a<1, -1<b<1)
705 
706  double colorDistance = (fabs (LRef - L) + ((fabs (aRef - a) + fabs (bRef - b)) / 2)) /3;
707 
708  if (colorDistance > 1.0)
709  colorDistance = 1.0;
710  if (colorDistance < 0.0)
711  colorDistance = 0.0;
712 
713  binDistanceColor[i_idx] = colorDistance * nr_color_bins_;
714  }
715  }
716 
717  //Apply quadrilinear interpolation on the activated bins in the shape and/or color histogram(s)
718 
719  if (b_describe_shape_ && b_describe_color_)
720  interpolateDoubleChannel (indices, sqr_dists, index, binDistanceShape, binDistanceColor,
721  nr_shape_bins_, nr_color_bins_,
722  shot);
723  else if (b_describe_color_)
724  interpolateSingleChannel (indices, sqr_dists, index, binDistanceColor, nr_color_bins_, shot);
725  else
726  interpolateSingleChannel (indices, sqr_dists, index, binDistanceShape, nr_shape_bins_, shot);
727 
728  // Normalize the final histogram
729  this->normalizeHistogram (shot, descLength_);
730 }
731 
732 //////////////////////////////////////////////////////////////////////////////////////////////
733 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
735  const int index, const std::vector<int> &indices, const std::vector<float> &sqr_dists, Eigen::VectorXf &shot)
736 {
737  //Skip the current feature if the number of its neighbors is not sufficient for its description
738  if (indices.size () < 5)
739  {
740  PCL_WARN ("[pcl::%s::computePointSHOT] Warning! Neighborhood has less than 5 vertexes. Aborting description of point with index %d\n",
741  getClassName ().c_str (), (*indices_)[index]);
742 
743  shot.setConstant(descLength_, 1, std::numeric_limits<float>::quiet_NaN () );
744 
745  return;
746  }
747 
748  // Clear the resultant shot
749  std::vector<double> binDistanceShape;
750  this->createBinDistanceShape (index, indices, binDistanceShape);
751 
752  // Interpolate
753  shot.setZero ();
754  interpolateSingleChannel (indices, sqr_dists, index, binDistanceShape, nr_shape_bins_, shot);
755 
756  // Normalize the final histogram
757  this->normalizeHistogram (shot, descLength_);
758 }
759 
760 //////////////////////////////////////////////////////////////////////////////////////////////
761 //////////////////////////////////////////////////////////////////////////////////////////////
762 //////////////////////////////////////////////////////////////////////////////////////////////
763 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
765 {
766  descLength_ = nr_grid_sector_ * (nr_shape_bins_+1);
767 
768  sqradius_ = search_radius_ * search_radius_;
769  radius3_4_ = (search_radius_*3) / 4;
770  radius1_4_ = search_radius_ / 4;
771  radius1_2_ = search_radius_ / 2;
772 
773  assert(descLength_ == 352);
774 
775  shot_.setZero (descLength_);
776 
777  // Allocate enough space to hold the results
778  // \note This resize is irrelevant for a radiusSearch ().
779  std::vector<int> nn_indices (k_);
780  std::vector<float> nn_dists (k_);
781 
782  output.is_dense = true;
783  // Iterating over the entire index vector
784  for (size_t idx = 0; idx < indices_->size (); ++idx)
785  {
786  bool lrf_is_nan = false;
787  const PointRFT& current_frame = (*frames_)[idx];
788  if (!pcl_isfinite (current_frame.x_axis[0]) ||
789  !pcl_isfinite (current_frame.y_axis[0]) ||
790  !pcl_isfinite (current_frame.z_axis[0]))
791  {
792  PCL_WARN ("[pcl::%s::computeFeature] The local reference frame is not valid! Aborting description of point with index %d\n",
793  getClassName ().c_str (), (*indices_)[idx]);
794  lrf_is_nan = true;
795  }
796 
797  if (!isFinite ((*input_)[(*indices_)[idx]]) ||
798  lrf_is_nan ||
799  this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
800  {
801  // Copy into the resultant cloud
802  for (int d = 0; d < descLength_; ++d)
803  output.points[idx].descriptor[d] = std::numeric_limits<float>::quiet_NaN ();
804  for (int d = 0; d < 9; ++d)
805  output.points[idx].rf[d] = std::numeric_limits<float>::quiet_NaN ();
806 
807  output.is_dense = false;
808  continue;
809  }
810 
811  // Estimate the SHOT descriptor at each patch
812  computePointSHOT (static_cast<int> (idx), nn_indices, nn_dists, shot_);
813 
814  // Copy into the resultant cloud
815  for (int d = 0; d < descLength_; ++d)
816  output.points[idx].descriptor[d] = shot_[d];
817  for (int d = 0; d < 3; ++d)
818  {
819  output.points[idx].rf[d + 0] = frames_->points[idx].x_axis[d];
820  output.points[idx].rf[d + 3] = frames_->points[idx].y_axis[d];
821  output.points[idx].rf[d + 6] = frames_->points[idx].z_axis[d];
822  }
823  }
824 }
825 
826 //////////////////////////////////////////////////////////////////////////////////////////////
827 //////////////////////////////////////////////////////////////////////////////////////////////
828 //////////////////////////////////////////////////////////////////////////////////////////////
829 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
831 {
832  // Compute the current length of the descriptor
833  descLength_ = (b_describe_shape_) ? nr_grid_sector_*(nr_shape_bins_+1) : 0;
834  descLength_ += (b_describe_color_) ? nr_grid_sector_*(nr_color_bins_+1) : 0;
835 
836  assert( (!b_describe_color_ && b_describe_shape_ && descLength_ == 352) ||
837  (b_describe_color_ && !b_describe_shape_ && descLength_ == 992) ||
838  (b_describe_color_ && b_describe_shape_ && descLength_ == 1344)
839  );
840 
841  // Useful values
842  sqradius_ = search_radius_*search_radius_;
843  radius3_4_ = (search_radius_*3) / 4;
844  radius1_4_ = search_radius_ / 4;
845  radius1_2_ = search_radius_ / 2;
846 
847  shot_.setZero (descLength_);
848 
849  // Allocate enough space to hold the results
850  // \note This resize is irrelevant for a radiusSearch ().
851  std::vector<int> nn_indices (k_);
852  std::vector<float> nn_dists (k_);
853 
854  output.is_dense = true;
855  // Iterating over the entire index vector
856  for (size_t idx = 0; idx < indices_->size (); ++idx)
857  {
858  bool lrf_is_nan = false;
859  const PointRFT& current_frame = (*frames_)[idx];
860  if (!pcl_isfinite (current_frame.x_axis[0]) ||
861  !pcl_isfinite (current_frame.y_axis[0]) ||
862  !pcl_isfinite (current_frame.z_axis[0]))
863  {
864  PCL_WARN ("[pcl::%s::computeFeature] The local reference frame is not valid! Aborting description of point with index %d\n",
865  getClassName ().c_str (), (*indices_)[idx]);
866  lrf_is_nan = true;
867  }
868 
869  if (!isFinite ((*input_)[(*indices_)[idx]]) ||
870  lrf_is_nan ||
871  this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
872  {
873  // Copy into the resultant cloud
874  for (int d = 0; d < descLength_; ++d)
875  output.points[idx].descriptor[d] = std::numeric_limits<float>::quiet_NaN ();
876  for (int d = 0; d < 9; ++d)
877  output.points[idx].rf[d] = std::numeric_limits<float>::quiet_NaN ();
878 
879  output.is_dense = false;
880  continue;
881  }
882 
883  // Compute the SHOT descriptor for the current 3D feature
884  computePointSHOT (static_cast<int> (idx), nn_indices, nn_dists, shot_);
885 
886  // Copy into the resultant cloud
887  for (int d = 0; d < descLength_; ++d)
888  output.points[idx].descriptor[d] = shot_[d];
889  for (int d = 0; d < 3; ++d)
890  {
891  output.points[idx].rf[d + 0] = frames_->points[idx].x_axis[d];
892  output.points[idx].rf[d + 3] = frames_->points[idx].y_axis[d];
893  output.points[idx].rf[d + 6] = frames_->points[idx].z_axis[d];
894  }
895  }
896 }
897 
898 #define PCL_INSTANTIATE_SHOTEstimationBase(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTEstimationBase<T,NT,OutT,RFT>;
899 #define PCL_INSTANTIATE_SHOTEstimation(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTEstimation<T,NT,OutT,RFT>;
900 #define PCL_INSTANTIATE_SHOTColorEstimation(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTColorEstimation<T,NT,OutT,RFT>;
901 
902 #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
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:54
SHOTColorEstimation estimates the Signature of Histograms of OrienTations (SHOT) descriptor for a giv...
Definition: shot.h:298
void computeFeature(pcl::PointCloud< PointOutT > &output)
Estimate the Signatures of Histograms of OrienTations (SHOT) descriptors at a set of points given by ...
Definition: shot.hpp:830
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
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 computeFeature(pcl::PointCloud< PointOutT > &output)
Estimate the Signatures of Histograms of OrienTations (SHOT) descriptors at a set of points given by ...
Definition: shot.hpp:764
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
virtual bool initCompute()
This method should get called before starting the actual computation.
Definition: shot.hpp:158
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
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
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
virtual void computePointSHOT(const int index, const std::vector< int > &indices, const std::vector< float > &sqr_dists, Eigen::VectorXf &shot)
Estimate the SHOT descriptor for a given point based on its spatial neighborhood of 3D points with no...
Definition: shot.hpp:646
void normalizeHistogram(Eigen::VectorXf &shot, int desc_length)
Normalize the SHOT histogram.
Definition: shot.hpp:238
SHOTLocalReferenceFrameEstimation estimates the Local Reference Frame used in the calculation of the ...
Definition: shot_lrf.h:66
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
FeatureWithLocalReferenceFrames provides a public interface for descriptor extractor classes which ne...
Definition: feature.h:447
Definition: norms.h:55
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
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
virtual void computePointSHOT(const int index, const std::vector< int > &indices, const std::vector< float > &sqr_dists, Eigen::VectorXf &shot)
Estimate the SHOT descriptor for a given point based on its spatial neighborhood of 3D points with no...
Definition: shot.hpp:734
boost::shared_ptr< SHOTLocalReferenceFrameEstimation< PointInT, PointOutT > > Ptr
Definition: shot_lrf.h:69