Point Cloud Library (PCL)  1.7.1
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_SHOTEstimation(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTEstimation<T,NT,OutT,RFT>;
899 #define PCL_INSTANTIATE_SHOTColorEstimation(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTColorEstimation<T,NT,OutT,RFT>;
900 
901 #endif // PCL_FEATURES_IMPL_SHOT_H_