Point Cloud Library (PCL)  1.6.0
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
shot.hpp
Go to the documentation of this file.
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  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of Willow Garage, Inc. nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  *
37  */
38 
39 #ifndef PCL_FEATURES_IMPL_SHOT_H_
40 #define PCL_FEATURES_IMPL_SHOT_H_
41 
42 #include <pcl/features/shot.h>
43 #include <pcl/features/shot_lrf.h>
44 #include <utility>
45 
46 // Useful constants.
47 #define PST_PI 3.1415926535897932384626433832795
48 #define PST_RAD_45 0.78539816339744830961566084581988
49 #define PST_RAD_90 1.5707963267948966192313216916398
50 #define PST_RAD_135 2.3561944901923449288469825374596
51 #define PST_RAD_180 PST_PI
52 #define PST_RAD_360 6.283185307179586476925286766558
53 #define PST_RAD_PI_7_8 2.7488935718910690836548129603691
54 
55 const double zeroDoubleEps15 = 1E-15;
56 const float zeroFloatEps8 = 1E-8f;
57 
59 
66 inline bool
67 areEquals (double val1, double val2, double zeroDoubleEps = zeroDoubleEps15)
68 {
69  return (fabs (val1 - val2)<zeroDoubleEps);
70 }
71 
73 
80 inline bool
81 areEquals (float val1, float val2, float zeroFloatEps = zeroFloatEps8)
82 {
83  return (fabs (val1 - val2)<zeroFloatEps);
84 }
85 
87 template <typename PointNT, typename PointRFT> float
88 pcl::SHOTEstimation<pcl::PointXYZRGBA, PointNT, pcl::SHOT, PointRFT>::sRGB_LUT[256] = {- 1};
89 
91 template <typename PointNT, typename PointRFT> float
92 pcl::SHOTEstimation<pcl::PointXYZRGBA, PointNT, pcl::SHOT, PointRFT>::sXYZ_LUT[4000] = {- 1};
93 
95 template <typename PointNT, typename PointRFT> void
96 pcl::SHOTEstimation<pcl::PointXYZRGBA, PointNT, pcl::SHOT, PointRFT>::RGB2CIELAB (unsigned char R, unsigned char G,
97  unsigned char B, float &L, float &A,
98  float &B2)
99 {
100  if (sRGB_LUT[0] < 0)
101  {
102  for (int i = 0; i < 256; i++)
103  {
104  float f = static_cast<float> (i) / 255.0f;
105  if (f > 0.04045)
106  sRGB_LUT[i] = powf ((f + 0.055f) / 1.055f, 2.4f);
107  else
108  sRGB_LUT[i] = f / 12.92f;
109  }
110 
111  for (int i = 0; i < 4000; i++)
112  {
113  float f = static_cast<float> (i) / 4000.0f;
114  if (f > 0.008856)
115  sXYZ_LUT[i] = static_cast<float> (powf (f, 0.3333f));
116  else
117  sXYZ_LUT[i] = static_cast<float>((7.787 * f) + (16.0 / 116.0));
118  }
119  }
120 
121  float fr = sRGB_LUT[R];
122  float fg = sRGB_LUT[G];
123  float fb = sRGB_LUT[B];
124 
125  // Use white = D65
126  const float x = fr * 0.412453f + fg * 0.357580f + fb * 0.180423f;
127  const float y = fr * 0.212671f + fg * 0.715160f + fb * 0.072169f;
128  const float z = fr * 0.019334f + fg * 0.119193f + fb * 0.950227f;
129 
130  float vx = x / 0.95047f;
131  float vy = y;
132  float vz = z / 1.08883f;
133 
134  vx = sXYZ_LUT[int(vx*4000)];
135  vy = sXYZ_LUT[int(vy*4000)];
136  vz = sXYZ_LUT[int(vz*4000)];
137 
138  L = 116.0f * vy - 16.0f;
139  if (L > 100)
140  L = 100.0f;
141 
142  A = 500.0f * (vx - vy);
143  if (A > 120)
144  A = 120.0f;
145  else if (A <- 120)
146  A = -120.0f;
147 
148  B2 = 200.0f * (vy - vz);
149  if (B2 > 120)
150  B2 = 120.0f;
151  else if (B2<- 120)
152  B2 = -120.0f;
153 }
154 
156 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> float
157 pcl::SHOTColorEstimation<PointInT, PointNT, PointOutT, PointRFT>::sRGB_LUT[256] = {- 1};
158 
160 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> float
161 pcl::SHOTColorEstimation<PointInT, PointNT, PointOutT, PointRFT>::sXYZ_LUT[4000] = {- 1};
162 
164 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
165 pcl::SHOTColorEstimation<PointInT, PointNT, PointOutT, PointRFT>::RGB2CIELAB (unsigned char R, unsigned char G,
166  unsigned char B, float &L, float &A,
167  float &B2)
168 {
169  if (sRGB_LUT[0] < 0)
170  {
171  for (int i = 0; i < 256; i++)
172  {
173  float f = static_cast<float> (i) / 255.0f;
174  if (f > 0.04045)
175  sRGB_LUT[i] = powf ((f + 0.055f) / 1.055f, 2.4f);
176  else
177  sRGB_LUT[i] = f / 12.92f;
178  }
179 
180  for (int i = 0; i < 4000; i++)
181  {
182  float f = static_cast<float> (i) / 4000.0f;
183  if (f > 0.008856)
184  sXYZ_LUT[i] = static_cast<float> (powf (f, 0.3333f));
185  else
186  sXYZ_LUT[i] = static_cast<float>((7.787 * f) + (16.0 / 116.0));
187  }
188  }
189 
190  float fr = sRGB_LUT[R];
191  float fg = sRGB_LUT[G];
192  float fb = sRGB_LUT[B];
193 
194  // Use white = D65
195  const float x = fr * 0.412453f + fg * 0.357580f + fb * 0.180423f;
196  const float y = fr * 0.212671f + fg * 0.715160f + fb * 0.072169f;
197  const float z = fr * 0.019334f + fg * 0.119193f + fb * 0.950227f;
198 
199  float vx = x / 0.95047f;
200  float vy = y;
201  float vz = z / 1.08883f;
202 
203  vx = sXYZ_LUT[int(vx*4000)];
204  vy = sXYZ_LUT[int(vy*4000)];
205  vz = sXYZ_LUT[int(vz*4000)];
206 
207  L = 116.0f * vy - 16.0f;
208  if (L > 100)
209  L = 100.0f;
210 
211  A = 500.0f * (vx - vy);
212  if (A > 120)
213  A = 120.0f;
214  else if (A <- 120)
215  A = -120.0f;
216 
217  B2 = 200.0f * (vy - vz);
218  if (B2 > 120)
219  B2 = 120.0f;
220  else if (B2<- 120)
221  B2 = -120.0f;
222 }
223 
225 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> bool
226 pcl::SHOTEstimationBase<PointInT, PointNT, PointOutT, PointRFT>::initCompute ()
227 {
228  if (!FeatureFromNormals<PointInT, PointNT, PointOutT>::initCompute ())
229  {
230  PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
231  return (false);
232  }
233 
234  // SHOT cannot work with k-search
235  if (this->getKSearch () != 0)
236  {
237  PCL_ERROR(
238  "[pcl::%s::initCompute] Error! Search method set to k-neighborhood. Call setKSearch(0) and setRadiusSearch( radius ) to use this class.\n",
239  getClassName().c_str ());
240  return (false);
241  }
242 
243  // Default LRF estimation alg: SHOTLocalReferenceFrameEstimation
244  typename SHOTLocalReferenceFrameEstimation<PointInT, PointRFT>::Ptr lrf_estimator(new SHOTLocalReferenceFrameEstimation<PointInT, PointRFT>());
245  lrf_estimator->setRadiusSearch (search_radius_);
246  lrf_estimator->setInputCloud (input_);
247  lrf_estimator->setIndices (indices_);
248  if (!fake_surface_)
249  lrf_estimator->setSearchSurface(surface_);
250 
251  if (!FeatureWithLocalReferenceFrames<PointInT, PointRFT>::initLocalReferenceFrames (indices_->size (), lrf_estimator))
252  {
253  PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
254  return (false);
255  }
256 
257  return (true);
258 }
259 
261 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
262 pcl::SHOTEstimationBase<PointInT, PointNT, PointOutT, PointRFT>::createBinDistanceShape (
263  int index,
264  const std::vector<int> &indices,
265  std::vector<double> &bin_distance_shape)
266 {
267  bin_distance_shape.resize (indices.size ());
268 
269  const PointRFT& current_frame = frames_->points[index];
270  //if (!pcl_isfinite (current_frame.rf[0]) || !pcl_isfinite (current_frame.rf[4]) || !pcl_isfinite (current_frame.rf[11]))
271  //return;
272 
273  for (size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
274  {
275  //double cosineDesc = feat[i].rf[6]*normal[0] + feat[i].rf[7]*normal[1] + feat[i].rf[8]*normal[2];
276  double cosineDesc = normals_->points[indices[i_idx]].getNormalVector4fMap ().dot (current_frame.z_axis.getNormalVector4fMap ());
277 
278  if (cosineDesc > 1.0)
279  cosineDesc = 1.0;
280  if (cosineDesc < - 1.0)
281  cosineDesc = - 1.0;
282 
283  bin_distance_shape[i_idx] = ((1.0 + cosineDesc) * nr_shape_bins_) / 2;
284  }
285 }
286 
288 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
289 pcl::SHOTEstimationBase<PointInT, PointNT, PointOutT, PointRFT>::normalizeHistogram (
290  Eigen::VectorXf &shot, int desc_length)
291 {
292  double acc_norm = 0;
293  for (int j = 0; j < desc_length; j++)
294  acc_norm += shot[j] * shot[j];
295  acc_norm = sqrt (acc_norm);
296  for (int j = 0; j < desc_length; j++)
297  shot[j] /= static_cast<float> (acc_norm);
298 }
299 
301 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
302 pcl::SHOTEstimationBase<PointInT, PointNT, PointOutT, PointRFT>::interpolateSingleChannel (
303  const std::vector<int> &indices,
304  const std::vector<float> &sqr_dists,
305  const int index,
306  std::vector<double> &binDistance,
307  const int nr_bins,
308  Eigen::VectorXf &shot)
309 {
310  const Eigen::Vector4f& central_point = (*input_)[(*indices_)[index]].getVector4fMap ();
311  const PointRFT& current_frame = (*frames_)[index];
312 
313  for (size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
314  {
315  Eigen::Vector4f delta = surface_->points[indices[i_idx]].getVector4fMap () - central_point;
316  delta[3] = 0;
317 
318  // Compute the Euclidean norm
319  double distance = sqrt (sqr_dists[i_idx]);
320 
321  if (areEquals (distance, 0.0))
322  continue;
323 
324  double xInFeatRef = delta.dot (current_frame.x_axis.getNormalVector4fMap ());
325  double yInFeatRef = delta.dot (current_frame.y_axis.getNormalVector4fMap ());
326  double zInFeatRef = delta.dot (current_frame.z_axis.getNormalVector4fMap ());
327 
328  // To avoid numerical problems afterwards
329  if (fabs (yInFeatRef) < 1E-30)
330  yInFeatRef = 0;
331  if (fabs (xInFeatRef) < 1E-30)
332  xInFeatRef = 0;
333  if (fabs (zInFeatRef) < 1E-30)
334  zInFeatRef = 0;
335 
336 
337  unsigned char bit4 = ((yInFeatRef > 0) || ((yInFeatRef == 0.0) && (xInFeatRef < 0))) ? 1 : 0;
338  unsigned char bit3 = static_cast<unsigned char> (((xInFeatRef > 0) || ((xInFeatRef == 0.0) && (yInFeatRef > 0))) ? !bit4 : bit4);
339 
340  assert (bit3 == 0 || bit3 == 1);
341 
342  int desc_index = (bit4<<3) + (bit3<<2);
343 
344  desc_index = desc_index << 1;
345 
346  if ((xInFeatRef * yInFeatRef > 0) || (xInFeatRef == 0.0))
347  desc_index += (fabs (xInFeatRef) >= fabs (yInFeatRef)) ? 0 : 4;
348  else
349  desc_index += (fabs (xInFeatRef) > fabs (yInFeatRef)) ? 4 : 0;
350 
351  desc_index += zInFeatRef > 0 ? 1 : 0;
352 
353  // 2 RADII
354  desc_index += (distance > radius1_2_) ? 2 : 0;
355 
356  int step_index = static_cast<int>(floor (binDistance[i_idx] +0.5));
357  int volume_index = desc_index * (nr_bins+1);
358 
359  //Interpolation on the cosine (adjacent bins in the histogram)
360  binDistance[i_idx] -= step_index;
361  double intWeight = (1- fabs (binDistance[i_idx]));
362 
363  if (binDistance[i_idx] > 0)
364  shot[volume_index + ((step_index+1) % nr_bins)] += static_cast<float> (binDistance[i_idx]);
365  else
366  shot[volume_index + ((step_index - 1 + nr_bins) % nr_bins)] += - static_cast<float> (binDistance[i_idx]);
367 
368  //Interpolation on the distance (adjacent husks)
369 
370  if (distance > radius1_2_) //external sphere
371  {
372  double radiusDistance = (distance - radius3_4_) / radius1_2_;
373 
374  if (distance > radius3_4_) //most external sector, votes only for itself
375  intWeight += 1 - radiusDistance; //peso=1-d
376  else //3/4 of radius, votes also for the internal sphere
377  {
378  intWeight += 1 + radiusDistance;
379  shot[(desc_index - 2) * (nr_bins+1) + step_index] -= static_cast<float> (radiusDistance);
380  }
381  }
382  else //internal sphere
383  {
384  double radiusDistance = (distance - radius1_4_) / radius1_2_;
385 
386  if (distance < radius1_4_) //most internal sector, votes only for itself
387  intWeight += 1 + radiusDistance; //weight=1-d
388  else //3/4 of radius, votes also for the external sphere
389  {
390  intWeight += 1 - radiusDistance;
391  shot[(desc_index + 2) * (nr_bins+1) + step_index] += static_cast<float> (radiusDistance);
392  }
393  }
394 
395  //Interpolation on the inclination (adjacent vertical volumes)
396  double inclinationCos = zInFeatRef / distance;
397  if (inclinationCos < - 1.0)
398  inclinationCos = - 1.0;
399  if (inclinationCos > 1.0)
400  inclinationCos = 1.0;
401 
402  double inclination = acos (inclinationCos);
403 
404  assert (inclination >= 0.0 && inclination <= PST_RAD_180);
405 
406  if (inclination > PST_RAD_90 || (fabs (inclination - PST_RAD_90) < 1e-30 && zInFeatRef <= 0))
407  {
408  double inclinationDistance = (inclination - PST_RAD_135) / PST_RAD_90;
409  if (inclination > PST_RAD_135)
410  intWeight += 1 - inclinationDistance;
411  else
412  {
413  intWeight += 1 + inclinationDistance;
414  assert ((desc_index + 1) * (nr_bins+1) + step_index >= 0 && (desc_index + 1) * (nr_bins+1) + step_index < descLength_);
415  shot[(desc_index + 1) * (nr_bins+1) + step_index] -= static_cast<float> (inclinationDistance);
416  }
417  }
418  else
419  {
420  double inclinationDistance = (inclination - PST_RAD_45) / PST_RAD_90;
421  if (inclination < PST_RAD_45)
422  intWeight += 1 + inclinationDistance;
423  else
424  {
425  intWeight += 1 - inclinationDistance;
426  assert ((desc_index - 1) * (nr_bins+1) + step_index >= 0 && (desc_index - 1) * (nr_bins+1) + step_index < descLength_);
427  shot[(desc_index - 1) * (nr_bins+1) + step_index] += static_cast<float> (inclinationDistance);
428  }
429  }
430 
431  if (yInFeatRef != 0.0 || xInFeatRef != 0.0)
432  {
433  //Interpolation on the azimuth (adjacent horizontal volumes)
434  double azimuth = atan2 (yInFeatRef, xInFeatRef);
435 
436  int sel = desc_index >> 2;
437  double angularSectorSpan = PST_RAD_45;
438  double angularSectorStart = - PST_RAD_PI_7_8;
439 
440  double azimuthDistance = (azimuth - (angularSectorStart + angularSectorSpan*sel)) / angularSectorSpan;
441 
442  assert ((azimuthDistance < 0.5 || areEquals (azimuthDistance, 0.5)) && (azimuthDistance > - 0.5 || areEquals (azimuthDistance, - 0.5)));
443 
444  azimuthDistance = (std::max)(- 0.5, std::min (azimuthDistance, 0.5));
445 
446  if (azimuthDistance > 0)
447  {
448  intWeight += 1 - azimuthDistance;
449  int interp_index = (desc_index + 4) % maxAngularSectors_;
450  assert (interp_index * (nr_bins+1) + step_index >= 0 && interp_index * (nr_bins+1) + step_index < descLength_);
451  shot[interp_index * (nr_bins+1) + step_index] += static_cast<float> (azimuthDistance);
452  }
453  else
454  {
455  int interp_index = (desc_index - 4 + maxAngularSectors_) % maxAngularSectors_;
456  assert (interp_index * (nr_bins+1) + step_index >= 0 && interp_index * (nr_bins+1) + step_index < descLength_);
457  intWeight += 1 + azimuthDistance;
458  shot[interp_index * (nr_bins+1) + step_index] -= static_cast<float> (azimuthDistance);
459  }
460 
461  }
462 
463  assert (volume_index + step_index >= 0 && volume_index + step_index < descLength_);
464  shot[volume_index + step_index] += static_cast<float> (intWeight);
465  }
466 }
467 
469 template <typename PointNT, typename PointRFT> void
470 pcl::SHOTEstimation<pcl::PointXYZRGBA, PointNT, pcl::SHOT, PointRFT>::interpolateDoubleChannel (
471  const std::vector<int> &indices,
472  const std::vector<float> &sqr_dists,
473  const int index,
474  std::vector<double> &binDistanceShape,
475  std::vector<double> &binDistanceColor,
476  const int nr_bins_shape,
477  const int nr_bins_color,
478  Eigen::VectorXf &shot)
479 {
480  const Eigen::Vector4f &central_point = (*input_)[(*indices_)[index]].getVector4fMap ();
481  const PointRFT& current_frame = (*frames_)[index];
482 
483  int shapeToColorStride = nr_grid_sector_*(nr_bins_shape+1);
484 
485  for (size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
486  {
487  Eigen::Vector4f delta = surface_->points[indices[i_idx]].getVector4fMap () - central_point;
488  delta[3] = 0;
489 
490  // Compute the Euclidean norm
491  double distance = sqrt (sqr_dists[i_idx]);
492 
493  if (areEquals (distance, 0.0))
494  continue;
495 
496  double xInFeatRef = delta.dot (current_frame.x_axis.getNormalVector4fMap ());
497  double yInFeatRef = delta.dot (current_frame.y_axis.getNormalVector4fMap ());
498  double zInFeatRef = delta.dot (current_frame.z_axis.getNormalVector4fMap ());
499 
500  // To avoid numerical problems afterwards
501  if (fabs (yInFeatRef) < 1E-30)
502  yInFeatRef = 0;
503  if (fabs (xInFeatRef) < 1E-30)
504  xInFeatRef = 0;
505  if (fabs (zInFeatRef) < 1E-30)
506  zInFeatRef = 0;
507 
508  unsigned char bit4 = ((yInFeatRef > 0) || ((yInFeatRef == 0.0) && (xInFeatRef < 0))) ? 1 : 0;
509  unsigned char bit3 = static_cast<unsigned char> (((xInFeatRef > 0) || ((xInFeatRef == 0.0) && (yInFeatRef > 0))) ? !bit4 : bit4);
510 
511  assert (bit3 == 0 || bit3 == 1);
512 
513  int desc_index = (bit4<<3) + (bit3<<2);
514 
515  desc_index = desc_index << 1;
516 
517  if ((xInFeatRef * yInFeatRef > 0) || (xInFeatRef == 0.0))
518  desc_index += (fabs (xInFeatRef) >= fabs (yInFeatRef)) ? 0 : 4;
519  else
520  desc_index += (fabs (xInFeatRef) > fabs (yInFeatRef)) ? 4 : 0;
521 
522  desc_index += zInFeatRef > 0 ? 1 : 0;
523 
524  // 2 RADII
525  desc_index += (distance > radius1_2_) ? 2 : 0;
526 
527  int step_index_shape = static_cast<int>(floor (binDistanceShape[i_idx] +0.5));
528  int step_index_color = static_cast<int>(floor (binDistanceColor[i_idx] +0.5));
529 
530  int volume_index_shape = desc_index * (nr_bins_shape+1);
531  int volume_index_color = shapeToColorStride + desc_index * (nr_bins_color+1);
532 
533  //Interpolation on the cosine (adjacent bins in the histrogram)
534  binDistanceShape[i_idx] -= step_index_shape;
535  binDistanceColor[i_idx] -= step_index_color;
536 
537  double intWeightShape = (1- fabs (binDistanceShape[i_idx]));
538  double intWeightColor = (1- fabs (binDistanceColor[i_idx]));
539 
540  if (binDistanceShape[i_idx] > 0)
541  shot[volume_index_shape + ((step_index_shape + 1) % nr_bins_shape)] += static_cast<float> (binDistanceShape[i_idx]);
542  else
543  shot[volume_index_shape + ((step_index_shape - 1 + nr_bins_shape) % nr_bins_shape)] -= static_cast<float> (binDistanceShape[i_idx]);
544 
545  if (binDistanceColor[i_idx] > 0)
546  shot[volume_index_color + ((step_index_color+1) % nr_bins_color)] += static_cast<float> (binDistanceColor[i_idx]);
547  else
548  shot[volume_index_color + ((step_index_color - 1 + nr_bins_color) % nr_bins_color)] -= static_cast<float> (binDistanceColor[i_idx]);
549 
550  //Interpolation on the distance (adjacent husks)
551 
552  if (distance > radius1_2_) //external sphere
553  {
554  double radiusDistance = (distance - radius3_4_) / radius1_2_;
555 
556  if (distance > radius3_4_) //most external sector, votes only for itself
557  {
558  intWeightShape += 1 - radiusDistance; //weight=1-d
559  intWeightColor += 1 - radiusDistance; //weight=1-d
560  }
561  else //3/4 of radius, votes also for the internal sphere
562  {
563  intWeightShape += 1 + radiusDistance;
564  intWeightColor += 1 + radiusDistance;
565  shot[(desc_index - 2) * (nr_bins_shape+1) + step_index_shape] -= static_cast<float> (radiusDistance);
566  shot[shapeToColorStride + (desc_index - 2) * (nr_bins_color+1) + step_index_color] -= static_cast<float> (radiusDistance);
567  }
568  }
569  else //internal sphere
570  {
571  double radiusDistance = (distance - radius1_4_) / radius1_2_;
572 
573  if (distance < radius1_4_) //most internal sector, votes only for itself
574  {
575  intWeightShape += 1 + radiusDistance;
576  intWeightColor += 1 + radiusDistance; //weight=1-d
577  }
578  else //3/4 of radius, votes also for the external sphere
579  {
580  intWeightShape += 1 - radiusDistance; //weight=1-d
581  intWeightColor += 1 - radiusDistance; //weight=1-d
582  shot[(desc_index + 2) * (nr_bins_shape+1) + step_index_shape] += static_cast<float> (radiusDistance);
583  shot[shapeToColorStride + (desc_index + 2) * (nr_bins_color+1) + step_index_color] += static_cast<float> (radiusDistance);
584  }
585  }
586 
587  //Interpolation on the inclination (adjacent vertical volumes)
588  double inclinationCos = zInFeatRef / distance;
589  if (inclinationCos < - 1.0)
590  inclinationCos = - 1.0;
591  if (inclinationCos > 1.0)
592  inclinationCos = 1.0;
593 
594  double inclination = acos (inclinationCos);
595 
596  assert (inclination >= 0.0 && inclination <= PST_RAD_180);
597 
598  if (inclination > PST_RAD_90 || (fabs (inclination - PST_RAD_90) < 1e-30 && zInFeatRef <= 0))
599  {
600  double inclinationDistance = (inclination - PST_RAD_135) / PST_RAD_90;
601  if (inclination > PST_RAD_135)
602  {
603  intWeightShape += 1 - inclinationDistance;
604  intWeightColor += 1 - inclinationDistance;
605  }
606  else
607  {
608  intWeightShape += 1 + inclinationDistance;
609  intWeightColor += 1 + inclinationDistance;
610  assert ((desc_index + 1) * (nr_bins_shape+1) + step_index_shape >= 0 && (desc_index + 1) * (nr_bins_shape+1) + step_index_shape < descLength_);
611  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_);
612  shot[(desc_index + 1) * (nr_bins_shape+1) + step_index_shape] -= static_cast<float> (inclinationDistance);
613  shot[shapeToColorStride + (desc_index + 1) * (nr_bins_color+1) + step_index_color] -= static_cast<float> (inclinationDistance);
614  }
615  }
616  else
617  {
618  double inclinationDistance = (inclination - PST_RAD_45) / PST_RAD_90;
619  if (inclination < PST_RAD_45)
620  {
621  intWeightShape += 1 + inclinationDistance;
622  intWeightColor += 1 + inclinationDistance;
623  }
624  else
625  {
626  intWeightShape += 1 - inclinationDistance;
627  intWeightColor += 1 - inclinationDistance;
628  assert ((desc_index - 1) * (nr_bins_shape+1) + step_index_shape >= 0 && (desc_index - 1) * (nr_bins_shape+1) + step_index_shape < descLength_);
629  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_);
630  shot[(desc_index - 1) * (nr_bins_shape+1) + step_index_shape] += static_cast<float> (inclinationDistance);
631  shot[shapeToColorStride + (desc_index - 1) * (nr_bins_color+1) + step_index_color] += static_cast<float> (inclinationDistance);
632  }
633  }
634 
635  if (yInFeatRef != 0.0 || xInFeatRef != 0.0)
636  {
637  //Interpolation on the azimuth (adjacent horizontal volumes)
638  double azimuth = atan2 (yInFeatRef, xInFeatRef);
639 
640  int sel = desc_index >> 2;
641  double angularSectorSpan = PST_RAD_45;
642  double angularSectorStart = - PST_RAD_PI_7_8;
643 
644  double azimuthDistance = (azimuth - (angularSectorStart + angularSectorSpan*sel)) / angularSectorSpan;
645  assert ((azimuthDistance < 0.5 || areEquals (azimuthDistance, 0.5)) && (azimuthDistance > - 0.5 || areEquals (azimuthDistance, - 0.5)));
646  azimuthDistance = (std::max)(- 0.5, std::min (azimuthDistance, 0.5));
647 
648  if (azimuthDistance > 0)
649  {
650  intWeightShape += 1 - azimuthDistance;
651  intWeightColor += 1 - azimuthDistance;
652  int interp_index = (desc_index + 4) % maxAngularSectors_;
653  assert (interp_index * (nr_bins_shape+1) + step_index_shape >= 0 && interp_index * (nr_bins_shape+1) + step_index_shape < descLength_);
654  assert (shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color >= 0 && shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color < descLength_);
655  shot[interp_index * (nr_bins_shape+1) + step_index_shape] += static_cast<float> (azimuthDistance);
656  shot[shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color] += static_cast<float> (azimuthDistance);
657  }
658  else
659  {
660  int interp_index = (desc_index - 4 + maxAngularSectors_) % maxAngularSectors_;
661  intWeightShape += 1 + azimuthDistance;
662  intWeightColor += 1 + azimuthDistance;
663  assert (interp_index * (nr_bins_shape+1) + step_index_shape >= 0 && interp_index * (nr_bins_shape+1) + step_index_shape < descLength_);
664  assert (shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color >= 0 && shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color < descLength_);
665  shot[interp_index * (nr_bins_shape+1) + step_index_shape] -= static_cast<float> (azimuthDistance);
666  shot[shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color] -= static_cast<float> (azimuthDistance);
667  }
668  }
669 
670  assert (volume_index_shape + step_index_shape >= 0 && volume_index_shape + step_index_shape < descLength_);
671  assert (volume_index_color + step_index_color >= 0 && volume_index_color + step_index_color < descLength_);
672  shot[volume_index_shape + step_index_shape] += static_cast<float> (intWeightShape);
673  shot[volume_index_color + step_index_color] += static_cast<float> (intWeightColor);
674  }
675 }
676 
678 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
679 pcl::SHOTColorEstimation<PointInT, PointNT, PointOutT, PointRFT>::interpolateDoubleChannel (
680  const std::vector<int> &indices,
681  const std::vector<float> &sqr_dists,
682  const int index,
683  std::vector<double> &binDistanceShape,
684  std::vector<double> &binDistanceColor,
685  const int nr_bins_shape,
686  const int nr_bins_color,
687  Eigen::VectorXf &shot)
688 {
689  const Eigen::Vector4f &central_point = (*input_)[(*indices_)[index]].getVector4fMap ();
690  const PointRFT& current_frame = (*frames_)[index];
691 
692  int shapeToColorStride = nr_grid_sector_*(nr_bins_shape+1);
693 
694  for (size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
695  {
696  Eigen::Vector4f delta = surface_->points[indices[i_idx]].getVector4fMap () - central_point;
697  delta[3] = 0;
698 
699  // Compute the Euclidean norm
700  double distance = sqrt (sqr_dists[i_idx]);
701 
702  if (areEquals (distance, 0.0))
703  continue;
704 
705  double xInFeatRef = delta.dot (current_frame.x_axis.getNormalVector4fMap ());
706  double yInFeatRef = delta.dot (current_frame.y_axis.getNormalVector4fMap ());
707  double zInFeatRef = delta.dot (current_frame.z_axis.getNormalVector4fMap ());
708 
709  // To avoid numerical problems afterwards
710  if (fabs (yInFeatRef) < 1E-30)
711  yInFeatRef = 0;
712  if (fabs (xInFeatRef) < 1E-30)
713  xInFeatRef = 0;
714  if (fabs (zInFeatRef) < 1E-30)
715  zInFeatRef = 0;
716 
717  unsigned char bit4 = ((yInFeatRef > 0) || ((yInFeatRef == 0.0) && (xInFeatRef < 0))) ? 1 : 0;
718  unsigned char bit3 = static_cast<unsigned char> (((xInFeatRef > 0) || ((xInFeatRef == 0.0) && (yInFeatRef > 0))) ? !bit4 : bit4);
719 
720  assert (bit3 == 0 || bit3 == 1);
721 
722  int desc_index = (bit4<<3) + (bit3<<2);
723 
724  desc_index = desc_index << 1;
725 
726  if ((xInFeatRef * yInFeatRef > 0) || (xInFeatRef == 0.0))
727  desc_index += (fabs (xInFeatRef) >= fabs (yInFeatRef)) ? 0 : 4;
728  else
729  desc_index += (fabs (xInFeatRef) > fabs (yInFeatRef)) ? 4 : 0;
730 
731  desc_index += zInFeatRef > 0 ? 1 : 0;
732 
733  // 2 RADII
734  desc_index += (distance > radius1_2_) ? 2 : 0;
735 
736  int step_index_shape = static_cast<int>(floor (binDistanceShape[i_idx] +0.5));
737  int step_index_color = static_cast<int>(floor (binDistanceColor[i_idx] +0.5));
738 
739  int volume_index_shape = desc_index * (nr_bins_shape+1);
740  int volume_index_color = shapeToColorStride + desc_index * (nr_bins_color+1);
741 
742  //Interpolation on the cosine (adjacent bins in the histrogram)
743  binDistanceShape[i_idx] -= step_index_shape;
744  binDistanceColor[i_idx] -= step_index_color;
745 
746  double intWeightShape = (1- fabs (binDistanceShape[i_idx]));
747  double intWeightColor = (1- fabs (binDistanceColor[i_idx]));
748 
749  if (binDistanceShape[i_idx] > 0)
750  shot[volume_index_shape + ((step_index_shape + 1) % nr_bins_shape)] += static_cast<float> (binDistanceShape[i_idx]);
751  else
752  shot[volume_index_shape + ((step_index_shape - 1 + nr_bins_shape) % nr_bins_shape)] -= static_cast<float> (binDistanceShape[i_idx]);
753 
754  if (binDistanceColor[i_idx] > 0)
755  shot[volume_index_color + ((step_index_color+1) % nr_bins_color)] += static_cast<float> (binDistanceColor[i_idx]);
756  else
757  shot[volume_index_color + ((step_index_color - 1 + nr_bins_color) % nr_bins_color)] -= static_cast<float> (binDistanceColor[i_idx]);
758 
759  //Interpolation on the distance (adjacent husks)
760 
761  if (distance > radius1_2_) //external sphere
762  {
763  double radiusDistance = (distance - radius3_4_) / radius1_2_;
764 
765  if (distance > radius3_4_) //most external sector, votes only for itself
766  {
767  intWeightShape += 1 - radiusDistance; //weight=1-d
768  intWeightColor += 1 - radiusDistance; //weight=1-d
769  }
770  else //3/4 of radius, votes also for the internal sphere
771  {
772  intWeightShape += 1 + radiusDistance;
773  intWeightColor += 1 + radiusDistance;
774  shot[(desc_index - 2) * (nr_bins_shape+1) + step_index_shape] -= static_cast<float> (radiusDistance);
775  shot[shapeToColorStride + (desc_index - 2) * (nr_bins_color+1) + step_index_color] -= static_cast<float> (radiusDistance);
776  }
777  }
778  else //internal sphere
779  {
780  double radiusDistance = (distance - radius1_4_) / radius1_2_;
781 
782  if (distance < radius1_4_) //most internal sector, votes only for itself
783  {
784  intWeightShape += 1 + radiusDistance;
785  intWeightColor += 1 + radiusDistance; //weight=1-d
786  }
787  else //3/4 of radius, votes also for the external sphere
788  {
789  intWeightShape += 1 - radiusDistance; //weight=1-d
790  intWeightColor += 1 - radiusDistance; //weight=1-d
791  shot[(desc_index + 2) * (nr_bins_shape+1) + step_index_shape] += static_cast<float> (radiusDistance);
792  shot[shapeToColorStride + (desc_index + 2) * (nr_bins_color+1) + step_index_color] += static_cast<float> (radiusDistance);
793  }
794  }
795 
796  //Interpolation on the inclination (adjacent vertical volumes)
797  double inclinationCos = zInFeatRef / distance;
798  if (inclinationCos < - 1.0)
799  inclinationCos = - 1.0;
800  if (inclinationCos > 1.0)
801  inclinationCos = 1.0;
802 
803  double inclination = acos (inclinationCos);
804 
805  assert (inclination >= 0.0 && inclination <= PST_RAD_180);
806 
807  if (inclination > PST_RAD_90 || (fabs (inclination - PST_RAD_90) < 1e-30 && zInFeatRef <= 0))
808  {
809  double inclinationDistance = (inclination - PST_RAD_135) / PST_RAD_90;
810  if (inclination > PST_RAD_135)
811  {
812  intWeightShape += 1 - inclinationDistance;
813  intWeightColor += 1 - inclinationDistance;
814  }
815  else
816  {
817  intWeightShape += 1 + inclinationDistance;
818  intWeightColor += 1 + inclinationDistance;
819  assert ((desc_index + 1) * (nr_bins_shape+1) + step_index_shape >= 0 && (desc_index + 1) * (nr_bins_shape+1) + step_index_shape < descLength_);
820  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_);
821  shot[(desc_index + 1) * (nr_bins_shape+1) + step_index_shape] -= static_cast<float> (inclinationDistance);
822  shot[shapeToColorStride + (desc_index + 1) * (nr_bins_color+1) + step_index_color] -= static_cast<float> (inclinationDistance);
823  }
824  }
825  else
826  {
827  double inclinationDistance = (inclination - PST_RAD_45) / PST_RAD_90;
828  if (inclination < PST_RAD_45)
829  {
830  intWeightShape += 1 + inclinationDistance;
831  intWeightColor += 1 + inclinationDistance;
832  }
833  else
834  {
835  intWeightShape += 1 - inclinationDistance;
836  intWeightColor += 1 - inclinationDistance;
837  assert ((desc_index - 1) * (nr_bins_shape+1) + step_index_shape >= 0 && (desc_index - 1) * (nr_bins_shape+1) + step_index_shape < descLength_);
838  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_);
839  shot[(desc_index - 1) * (nr_bins_shape+1) + step_index_shape] += static_cast<float> (inclinationDistance);
840  shot[shapeToColorStride + (desc_index - 1) * (nr_bins_color+1) + step_index_color] += static_cast<float> (inclinationDistance);
841  }
842  }
843 
844  if (yInFeatRef != 0.0 || xInFeatRef != 0.0)
845  {
846  //Interpolation on the azimuth (adjacent horizontal volumes)
847  double azimuth = atan2 (yInFeatRef, xInFeatRef);
848 
849  int sel = desc_index >> 2;
850  double angularSectorSpan = PST_RAD_45;
851  double angularSectorStart = - PST_RAD_PI_7_8;
852 
853  double azimuthDistance = (azimuth - (angularSectorStart + angularSectorSpan*sel)) / angularSectorSpan;
854  assert ((azimuthDistance < 0.5 || areEquals (azimuthDistance, 0.5)) && (azimuthDistance > - 0.5 || areEquals (azimuthDistance, - 0.5)));
855  azimuthDistance = (std::max)(- 0.5, std::min (azimuthDistance, 0.5));
856 
857  if (azimuthDistance > 0)
858  {
859  intWeightShape += 1 - azimuthDistance;
860  intWeightColor += 1 - azimuthDistance;
861  int interp_index = (desc_index + 4) % maxAngularSectors_;
862  assert (interp_index * (nr_bins_shape+1) + step_index_shape >= 0 && interp_index * (nr_bins_shape+1) + step_index_shape < descLength_);
863  assert (shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color >= 0 && shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color < descLength_);
864  shot[interp_index * (nr_bins_shape+1) + step_index_shape] += static_cast<float> (azimuthDistance);
865  shot[shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color] += static_cast<float> (azimuthDistance);
866  }
867  else
868  {
869  int interp_index = (desc_index - 4 + maxAngularSectors_) % maxAngularSectors_;
870  intWeightShape += 1 + azimuthDistance;
871  intWeightColor += 1 + azimuthDistance;
872  assert (interp_index * (nr_bins_shape+1) + step_index_shape >= 0 && interp_index * (nr_bins_shape+1) + step_index_shape < descLength_);
873  assert (shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color >= 0 && shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color < descLength_);
874  shot[interp_index * (nr_bins_shape+1) + step_index_shape] -= static_cast<float> (azimuthDistance);
875  shot[shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color] -= static_cast<float> (azimuthDistance);
876  }
877  }
878 
879  assert (volume_index_shape + step_index_shape >= 0 && volume_index_shape + step_index_shape < descLength_);
880  assert (volume_index_color + step_index_color >= 0 && volume_index_color + step_index_color < descLength_);
881  shot[volume_index_shape + step_index_shape] += static_cast<float> (intWeightShape);
882  shot[volume_index_color + step_index_color] += static_cast<float> (intWeightColor);
883  }
884 }
885 
886 
888 template <typename PointNT, typename PointRFT> void
889 pcl::SHOTEstimation<pcl::PointXYZRGBA, PointNT, pcl::SHOT, PointRFT>::computePointSHOT (
890  const int index, const std::vector<int> &indices, const std::vector<float> &sqr_dists, Eigen::VectorXf &shot)
891 {
892  // Clear the resultant shot
893  shot.setZero ();
894  std::vector<double> binDistanceShape;
895  std::vector<double> binDistanceColor;
896  size_t nNeighbors = indices.size ();
897  //Skip the current feature if the number of its neighbors is not sufficient for its description
898  if (nNeighbors < 5)
899  {
900  PCL_WARN ("[pcl::%s::computePointSHOT] Warning! Neighborhood has less than 5 vertexes. Aborting description of point with index %d\n",
901  getClassName ().c_str (), (*indices_)[index]);
902 
903  shot.setConstant(descLength_, 1, std::numeric_limits<float>::quiet_NaN () );
904 
905  return;
906  }
907 
908  const PointRFT& current_frame = frames_->points[index];
909  //if (!pcl_isfinite (current_frame.rf[0]) || !pcl_isfinite (current_frame.rf[4]) || !pcl_isfinite (current_frame.rf[11]))
910  //return;
911 
912  //If shape description is enabled, compute the bins activated by each neighbor of the current feature in the shape histogram
913  if (b_describe_shape_)
914  {
915  binDistanceShape.resize (nNeighbors);
916 
917  for (size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
918  {
919  //feat[i].rf[6]*normal[0] + feat[i].rf[7]*normal[1] + feat[i].rf[8]*normal[2];
920  double cosineDesc = normals_->points[indices[i_idx]].getNormalVector4fMap ().dot (current_frame.z_axis.getNormalVector4fMap ());
921 
922  if (cosineDesc > 1.0)
923  cosineDesc = 1.0;
924  if (cosineDesc < - 1.0)
925  cosineDesc = - 1.0;
926 
927  binDistanceShape[i_idx] = ((1.0 + cosineDesc) * nr_shape_bins_) / 2;
928  }
929  }
930 
931  //If color description is enabled, compute the bins activated by each neighbor of the current feature in the color histogram
932  if (b_describe_color_)
933  {
934  binDistanceColor.resize (nNeighbors);
935 
936  unsigned char redRef = input_->points[(*indices_)[index]].rgba >> 16 & 0xFF;
937  unsigned char greenRef = input_->points[(*indices_)[index]].rgba >> 8& 0xFF;
938  unsigned char blueRef = input_->points[(*indices_)[index]].rgba & 0xFF;
939 
940  float LRef, aRef, bRef;
941 
942  RGB2CIELAB (redRef, greenRef, blueRef, LRef, aRef, bRef);
943  LRef /= 100.0f;
944  aRef /= 120.0f;
945  bRef /= 120.0f; //normalized LAB components (0<L<1, -1<a<1, -1<b<1)
946 
947  for (size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
948  {
949  unsigned char red = surface_->points[indices[i_idx]].rgba >> 16 & 0xFF;
950  unsigned char green = surface_->points[indices[i_idx]].rgba >> 8 & 0xFF;
951  unsigned char blue = surface_->points[indices[i_idx]].rgba & 0xFF;
952 
953  float L, a, b;
954 
955  RGB2CIELAB (red, green, blue, L, a, b);
956  L /= 100.0f;
957  a /= 120.0f;
958  b /= 120.0f; //normalized LAB components (0<L<1, -1<a<1, -1<b<1)
959 
960  double colorDistance = (fabs (LRef - L) + ((fabs (aRef - a) + fabs (bRef - b)) / 2)) /3;
961 
962  if (colorDistance > 1.0)
963  colorDistance = 1.0;
964  if (colorDistance < 0.0)
965  colorDistance = 0.0;
966 
967  binDistanceColor[i_idx] = colorDistance * nr_color_bins_;
968  }
969  }
970 
971  //Apply quadrilinear interpolation on the activated bins in the shape and/or color histogram(s)
972 
973  if (b_describe_shape_ && b_describe_color_)
974  interpolateDoubleChannel (indices, sqr_dists, index, binDistanceShape, binDistanceColor,
975  nr_shape_bins_, nr_color_bins_,
976  shot);
977  else if (b_describe_color_)
978  interpolateSingleChannel (indices, sqr_dists, index, binDistanceColor, nr_color_bins_, shot);
979  else
980  interpolateSingleChannel (indices, sqr_dists, index, binDistanceShape, nr_shape_bins_, shot);
981 
982  // Normalize the final histogram
983  this->normalizeHistogram (shot, descLength_);
984 }
985 
987 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
988 pcl::SHOTColorEstimation<PointInT, PointNT, PointOutT, PointRFT>::computePointSHOT (
989  const int index, const std::vector<int> &indices, const std::vector<float> &sqr_dists, Eigen::VectorXf &shot)
990 {
991  // Clear the resultant shot
992  shot.setZero ();
993  std::vector<double> binDistanceShape;
994  std::vector<double> binDistanceColor;
995  size_t nNeighbors = indices.size ();
996  //Skip the current feature if the number of its neighbors is not sufficient for its description
997  if (nNeighbors < 5)
998  {
999  PCL_WARN ("[pcl::%s::computePointSHOT] Warning! Neighborhood has less than 5 vertexes. Aborting description of point with index %d\n",
1000  getClassName ().c_str (), (*indices_)[index]);
1001 
1002  shot.setConstant(descLength_, 1, std::numeric_limits<float>::quiet_NaN () );
1003 
1004  return;
1005  }
1006 
1007  const PointRFT& current_frame = frames_->points[index];
1008  //if (!pcl_isfinite (current_frame.rf[0]) || !pcl_isfinite (current_frame.rf[4]) || !pcl_isfinite (current_frame.rf[11]))
1009  //return;
1010 
1011  //If shape description is enabled, compute the bins activated by each neighbor of the current feature in the shape histogram
1012  if (b_describe_shape_)
1013  {
1014  binDistanceShape.resize (nNeighbors);
1015 
1016  for (size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
1017  {
1018  //feat[i].rf[6]*normal[0] + feat[i].rf[7]*normal[1] + feat[i].rf[8]*normal[2];
1019  double cosineDesc = normals_->points[indices[i_idx]].getNormalVector4fMap ().dot (current_frame.z_axis.getNormalVector4fMap ());
1020 
1021  if (cosineDesc > 1.0)
1022  cosineDesc = 1.0;
1023  if (cosineDesc < - 1.0)
1024  cosineDesc = - 1.0;
1025 
1026  binDistanceShape[i_idx] = ((1.0 + cosineDesc) * nr_shape_bins_) / 2;
1027  }
1028  }
1029 
1030  //If color description is enabled, compute the bins activated by each neighbor of the current feature in the color histogram
1031  if (b_describe_color_)
1032  {
1033  binDistanceColor.resize (nNeighbors);
1034 
1035  //unsigned char redRef = input_->points[(*indices_)[index]].rgba >> 16 & 0xFF;
1036  //unsigned char greenRef = input_->points[(*indices_)[index]].rgba >> 8& 0xFF;
1037  //unsigned char blueRef = input_->points[(*indices_)[index]].rgba & 0xFF;
1038  unsigned char redRef = input_->points[(*indices_)[index]].r;
1039  unsigned char greenRef = input_->points[(*indices_)[index]].g;
1040  unsigned char blueRef = input_->points[(*indices_)[index]].b;
1041 
1042  float LRef, aRef, bRef;
1043 
1044  RGB2CIELAB (redRef, greenRef, blueRef, LRef, aRef, bRef);
1045  LRef /= 100.0f;
1046  aRef /= 120.0f;
1047  bRef /= 120.0f; //normalized LAB components (0<L<1, -1<a<1, -1<b<1)
1048 
1049  for (size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
1050  {
1051  //unsigned char red = surface_->points[indices[i_idx]].rgba >> 16 & 0xFF;
1052  //unsigned char green = surface_->points[indices[i_idx]].rgba >> 8 & 0xFF;
1053  //unsigned char blue = surface_->points[indices[i_idx]].rgba & 0xFF;
1054  unsigned char red = surface_->points[indices[i_idx]].r;
1055  unsigned char green = surface_->points[indices[i_idx]].g;
1056  unsigned char blue = surface_->points[indices[i_idx]].b;
1057 
1058  float L, a, b;
1059 
1060  RGB2CIELAB (red, green, blue, L, a, b);
1061  L /= 100.0f;
1062  a /= 120.0f;
1063  b /= 120.0f; //normalized LAB components (0<L<1, -1<a<1, -1<b<1)
1064 
1065  double colorDistance = (fabs (LRef - L) + ((fabs (aRef - a) + fabs (bRef - b)) / 2)) /3;
1066 
1067  if (colorDistance > 1.0)
1068  colorDistance = 1.0;
1069  if (colorDistance < 0.0)
1070  colorDistance = 0.0;
1071 
1072  binDistanceColor[i_idx] = colorDistance * nr_color_bins_;
1073  }
1074  }
1075 
1076  //Apply quadrilinear interpolation on the activated bins in the shape and/or color histogram(s)
1077 
1078  if (b_describe_shape_ && b_describe_color_)
1079  interpolateDoubleChannel (indices, sqr_dists, index, binDistanceShape, binDistanceColor,
1080  nr_shape_bins_, nr_color_bins_,
1081  shot);
1082  else if (b_describe_color_)
1083  interpolateSingleChannel (indices, sqr_dists, index, binDistanceColor, nr_color_bins_, shot);
1084  else
1085  interpolateSingleChannel (indices, sqr_dists, index, binDistanceShape, nr_shape_bins_, shot);
1086 
1087  // Normalize the final histogram
1088  this->normalizeHistogram (shot, descLength_);
1089 }
1090 
1091 
1093 template <typename PointInT, typename PointNT, typename PointRFT> void
1094 pcl::SHOTEstimation<PointInT, PointNT, pcl::SHOT, PointRFT>::computePointSHOT (
1095  const int index, const std::vector<int> &indices, const std::vector<float> &sqr_dists, Eigen::VectorXf &shot)
1096 {
1097  //Skip the current feature if the number of its neighbors is not sufficient for its description
1098  if (indices.size () < 5)
1099  {
1100  PCL_WARN ("[pcl::%s::computePointSHOT] Warning! Neighborhood has less than 5 vertexes. Aborting description of point with index %d\n",
1101  getClassName ().c_str (), (*indices_)[index]);
1102 
1103  shot.setConstant(descLength_, 1, std::numeric_limits<float>::quiet_NaN () );
1104 
1105  return;
1106  }
1107 
1108  // Clear the resultant shot
1109  std::vector<double> binDistanceShape;
1110  this->createBinDistanceShape (index, indices, binDistanceShape);
1111 
1112  // Interpolate
1113  shot.setZero ();
1114  interpolateSingleChannel (indices, sqr_dists, index, binDistanceShape, nr_shape_bins_, shot);
1115 
1116  // Normalize the final histogram
1117  this->normalizeHistogram (shot, descLength_);
1118 }
1119 
1121 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
1122 pcl::SHOTEstimation<PointInT, PointNT, PointOutT, PointRFT>::computePointSHOT (
1123  const int index, const std::vector<int> &indices, const std::vector<float> &sqr_dists, Eigen::VectorXf &shot)
1124 {
1125  //Skip the current feature if the number of its neighbors is not sufficient for its description
1126  if (indices.size () < 5)
1127  {
1128  PCL_WARN ("[pcl::%s::computePointSHOT] Warning! Neighborhood has less than 5 vertexes. Aborting description of point with index %d\n",
1129  getClassName ().c_str (), (*indices_)[index]);
1130 
1131  shot.setConstant(descLength_, 1, std::numeric_limits<float>::quiet_NaN () );
1132 
1133  return;
1134  }
1135 
1136  // Clear the resultant shot
1137  std::vector<double> binDistanceShape;
1138  this->createBinDistanceShape (index, indices, binDistanceShape);
1139 
1140  // Interpolate
1141  shot.setZero ();
1142  interpolateSingleChannel (indices, sqr_dists, index, binDistanceShape, nr_shape_bins_, shot);
1143 
1144  // Normalize the final histogram
1145  this->normalizeHistogram (shot, descLength_);
1146 }
1147 
1149 //template <typename PointInT, typename PointNT, typename PointRFT> void
1150 //pcl::SHOTEstimation<PointInT, PointNT, Eigen::MatrixXf, PointRFT>::computePointSHOT (
1151  //const int index, const std::vector<int> &indices, const std::vector<float> &sqr_dists, Eigen::VectorXf &shot)
1152 //{
1154  //if (indices.size () < 5)
1155  //{
1156  //PCL_WARN ("[pcl::%s::computePointSHOT] Warning! Neighborhood has less than 5 vertexes. Aborting description of point with index %d\n",
1157  //getClassName ().c_str (), (*indices_)[index]);
1158 //
1159  //shot.setConstant(descLength_, 1, std::numeric_limits<float>::quiet_NaN () );
1160  //return;
1161  //}
1162 //
1164  //std::vector<double> binDistanceShape;
1165  //this->createBinDistanceShape (index, indices, binDistanceShape);
1166 //
1168  //shot.setZero ();
1169  //interpolateSingleChannel (indices, sqr_dists, index, binDistanceShape, nr_shape_bins_, shot);
1170 //
1172  //this->normalizeHistogram (shot, descLength_);
1173 //}
1174 
1175 
1179 template <typename PointNT, typename PointRFT> void
1180 pcl::SHOTEstimation<pcl::PointXYZRGBA, PointNT, pcl::SHOT, PointRFT>::computeFeature (PointCloudOut &output)
1181 {
1182  // Compute the current length of the descriptor
1183  descLength_ = (b_describe_shape_) ? nr_grid_sector_*(nr_shape_bins_+1) : 0;
1184  descLength_ += (b_describe_color_) ? nr_grid_sector_*(nr_color_bins_+1) : 0;
1185 
1186  // Useful values
1187  sqradius_ = search_radius_*search_radius_;
1188  radius3_4_ = (search_radius_*3) / 4;
1189  radius1_4_ = search_radius_ / 4;
1190  radius1_2_ = search_radius_ / 2;
1191 
1192  shot_.setZero (descLength_);
1193 
1194  //if (output.points[0].descriptor.size () != static_cast<size_t> (descLength_))
1195  for (size_t idx = 0; idx < indices_->size (); ++idx)
1196  output.points[idx].descriptor.resize (descLength_);
1197 // if (output.points[0].size != (size_t)descLength_)
1198 // {
1199 // PCL_ERROR ("[pcl::%s::computeFeature] The desired descriptor size (%lu) does not match the output point type feature (%lu)! Aborting...\n", getClassName ().c_str (), descLength_, (unsigned long) output.points[0].size);
1200 // return;
1201 // }
1202 
1203  // Allocate enough space to hold the results
1204  // \note This resize is irrelevant for a radiusSearch ().
1205  std::vector<int> nn_indices (k_);
1206  std::vector<float> nn_dists (k_);
1207 
1208  output.is_dense = true;
1209  // Iterating over the entire index vector
1210  for (size_t idx = 0; idx < indices_->size (); ++idx)
1211  {
1212  bool lrf_is_nan = false;
1213  const PointRFT& current_frame = (*frames_)[idx];
1214  if (!pcl_isfinite (current_frame.rf[0]) ||
1215  !pcl_isfinite (current_frame.rf[4]) ||
1216  !pcl_isfinite (current_frame.rf[11]))
1217  {
1218  PCL_WARN ("[pcl::%s::computeFeature] The local reference frame is not valid! Aborting description of point with index %d\n",
1219  getClassName ().c_str (), (*indices_)[idx]);
1220  lrf_is_nan = true;
1221  }
1222 
1223  if (!isFinite ((*input_)[(*indices_)[idx]]) ||
1224  lrf_is_nan ||
1225  this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
1226  {
1227  // Copy into the resultant cloud
1228  for (int d = 0; d < shot_.size (); ++d)
1229  output.points[idx].descriptor[d] = std::numeric_limits<float>::quiet_NaN ();
1230  for (int d = 0; d < 9; ++d)
1231  output.points[idx].rf[d] = std::numeric_limits<float>::quiet_NaN ();
1232 
1233  output.is_dense = false;
1234  continue;
1235  }
1236 
1237  // Compute the SHOT descriptor for the current 3D feature
1238  computePointSHOT (static_cast<int> (idx), nn_indices, nn_dists, shot_);
1239 
1240  // Copy into the resultant cloud
1241  for (int d = 0; d < shot_.size (); ++d)
1242  output.points[idx].descriptor[d] = shot_[d];
1243  for (int d = 0; d < 9; ++d)
1244  output.points[idx].rf[d] = frames_->points[idx].rf[ (4*(d/3) + (d%3)) ];
1245  }
1246 }
1247 
1251 template <typename PointNT, typename PointRFT> void
1252 pcl::SHOTEstimation<pcl::PointXYZRGBA, PointNT, Eigen::MatrixXf, PointRFT>::computeFeatureEigen (pcl::PointCloud<Eigen::MatrixXf> &output)
1253 {
1254  // Compute the current length of the descriptor
1255  descLength_ = (b_describe_shape_) ? nr_grid_sector_*(nr_shape_bins_+1) : 0;
1256  descLength_ += (b_describe_color_) ? nr_grid_sector_*(nr_color_bins_+1) : 0;
1257 
1258  // Set up the output channels
1259  output.channels["shot"].name = "shot";
1260  output.channels["shot"].offset = 0;
1261  output.channels["shot"].size = 4;
1262  output.channels["shot"].count = descLength_ + 9;
1263  output.channels["shot"].datatype = sensor_msgs::PointField::FLOAT32;
1264 
1265  // Useful values
1266  sqradius_ = search_radius_*search_radius_;
1267  radius3_4_ = (search_radius_*3) / 4;
1268  radius1_4_ = search_radius_ / 4;
1269  radius1_2_ = search_radius_ / 2;
1270 
1271  shot_.setZero (descLength_);
1272 
1273  output.points.resize (indices_->size (), descLength_ + 9);
1274 
1275  // Allocate enough space to hold the results
1276  // \note This resize is irrelevant for a radiusSearch ().
1277  std::vector<int> nn_indices (k_);
1278  std::vector<float> nn_dists (k_);
1279 
1280  output.is_dense = true;
1281  // Iterating over the entire index vector
1282  for (size_t idx = 0; idx < indices_->size (); ++idx)
1283  {
1284  bool lrf_is_nan = false;
1285  const PointRFT& current_frame = (*frames_)[idx];
1286  if (!pcl_isfinite (current_frame.rf[0]) ||
1287  !pcl_isfinite (current_frame.rf[4]) ||
1288  !pcl_isfinite (current_frame.rf[11]))
1289  {
1290  PCL_WARN ("[pcl::%s::computeFeature] The local reference frame is not valid! Aborting description of point with index %d\n",
1291  getClassName ().c_str (), (*indices_)[idx]);
1292  lrf_is_nan = true;
1293  }
1294 
1295  if (!isFinite ((*input_)[(*indices_)[idx]]) ||
1296  lrf_is_nan ||
1297  this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
1298  {
1299  output.points.row (idx).setConstant (std::numeric_limits<float>::quiet_NaN ());
1300 
1301  output.is_dense = false;
1302  continue;
1303  }
1304 
1305  // Compute the SHOT descriptor for the current 3D feature
1306  this->computePointSHOT (static_cast<int> (idx), nn_indices, nn_dists, shot_);
1307 
1308  // Copy into the resultant cloud
1309  for (int d = 0; d < shot_.size (); ++d)
1310  output.points (idx, d) = shot_[d];
1311  for (int d = 0; d < 9; ++d)
1312  output.points (idx, shot_.size () + d) = frames_->points[idx].rf[ (4*(d/3) + (d%3)) ];
1313  }
1314 }
1315 
1316 
1320 template <typename PointInT, typename PointNT, typename PointRFT> void
1321 pcl::SHOTEstimation<PointInT, PointNT, pcl::SHOT, PointRFT>::computeFeature (pcl::PointCloud<pcl::SHOT> &output)
1322 {
1323  descLength_ = nr_grid_sector_ * (nr_shape_bins_+1);
1324 
1325  sqradius_ = search_radius_ * search_radius_;
1326  radius3_4_ = (search_radius_*3) / 4;
1327  radius1_4_ = search_radius_ / 4;
1328  radius1_2_ = search_radius_ / 2;
1329 
1330  shot_.setZero (descLength_);
1331 
1332  for (size_t idx = 0; idx < indices_->size (); ++idx)
1333  output.points[idx].descriptor.resize (descLength_);
1334 // if (output.points[0].size != (size_t)descLength_)
1335 // {
1336 // PCL_ERROR ("[pcl::%s::computeFeature] The desired descriptor size (%lu) does not match the output point type feature (%lu)! Aborting...\n", getClassName ().c_str (), descLength_, (unsigned long) output.points[0].size);
1337 // return;
1338 // }
1339 
1340  // Allocate enough space to hold the results
1341  // \note This resize is irrelevant for a radiusSearch ().
1342  std::vector<int> nn_indices (k_);
1343  std::vector<float> nn_dists (k_);
1344 
1345  output.is_dense = true;
1346  // Iterating over the entire index vector
1347  for (size_t idx = 0; idx < indices_->size (); ++idx)
1348  {
1349  bool lrf_is_nan = false;
1350  const PointRFT& current_frame = (*frames_)[idx];
1351  if (!pcl_isfinite (current_frame.rf[0]) ||
1352  !pcl_isfinite (current_frame.rf[4]) ||
1353  !pcl_isfinite (current_frame.rf[11]))
1354  {
1355  PCL_WARN ("[pcl::%s::computeFeature] The local reference frame is not valid! Aborting description of point with index %d\n",
1356  getClassName ().c_str (), (*indices_)[idx]);
1357  lrf_is_nan = true;
1358  }
1359 
1360  if (!isFinite ((*input_)[(*indices_)[idx]]) ||
1361  lrf_is_nan ||
1362  this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
1363  {
1364  // Copy into the resultant cloud
1365  for (int d = 0; d < shot_.size (); ++d)
1366  output.points[idx].descriptor[d] = std::numeric_limits<float>::quiet_NaN ();
1367  for (int d = 0; d < 9; ++d)
1368  output.points[idx].rf[d] = std::numeric_limits<float>::quiet_NaN ();
1369 
1370  output.is_dense = false;
1371  continue;
1372  }
1373 
1374  // Estimate the SHOT at each patch
1375  computePointSHOT (static_cast<int> (idx), nn_indices, nn_dists, shot_);
1376 
1377  // Copy into the resultant cloud
1378  for (int d = 0; d < shot_.size (); ++d)
1379  output.points[idx].descriptor[d] = shot_[d];
1380  for (int d = 0; d < 9; ++d)
1381  output.points[idx].rf[d] = frames_->points[idx].rf[ (4*(d/3) + (d%3)) ];
1382  }
1383 }
1384 
1388 /*
1389 template <typename PointInT, typename PointNT, typename PointRFT> void
1390 pcl::SHOTEstimationBase<PointInT, PointNT, Eigen::MatrixXf, PointRFT>::computeFeatureEigen (pcl::PointCloud<Eigen::MatrixXf> &output)
1391 {
1392  descLength_ = nr_grid_sector_ * (nr_shape_bins_+1);
1393 
1394  // Set up the output channels
1395  output.channels["shot"].name = "shot";
1396  output.channels["shot"].offset = 0;
1397  output.channels["shot"].size = 4;
1398  output.channels["shot"].count = descLength_ + 9;
1399  output.channels["shot"].datatype = sensor_msgs::PointField::FLOAT32;
1400 
1401  sqradius_ = search_radius_ * search_radius_;
1402  radius3_4_ = (search_radius_*3) / 4;
1403  radius1_4_ = search_radius_ / 4;
1404  radius1_2_ = search_radius_ / 2;
1405 
1406  shot_.setZero (descLength_);
1407 
1408  output.points.resize (indices_->size (), descLength_ + 9);
1409 
1410  // Allocate enough space to hold the results
1411  // \note This resize is irrelevant for a radiusSearch ().
1412  std::vector<int> nn_indices (k_);
1413  std::vector<float> nn_dists (k_);
1414 
1415  output.is_dense = true;
1416  // Iterating over the entire index vector
1417  for (size_t idx = 0; idx < indices_->size (); ++idx)
1418  {
1419  bool lrf_is_nan = false;
1420  const PointRFT& current_frame = (*frames_)[idx];
1421  if (!pcl_isfinite (current_frame.rf[0]) ||
1422  !pcl_isfinite (current_frame.rf[4]) ||
1423  !pcl_isfinite (current_frame.rf[11]))
1424  {
1425  PCL_WARN ("[pcl::%s::computeFeature] The local reference frame is not valid! Aborting description of point with index %d\n",
1426  getClassName ().c_str (), (*indices_)[idx]);
1427  lrf_is_nan = true;
1428  }
1429 
1430  if (!isFinite ((*input_)[(*indices_)[idx]]) ||
1431  lrf_is_nan ||
1432  this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
1433  {
1434  output.points.row (idx).setConstant (std::numeric_limits<float>::quiet_NaN ());
1435 
1436  output.is_dense = false;
1437  continue;
1438  }
1439 
1440  // Estimate the SHOT at each patch
1441  this->computePointSHOT (static_cast<int> (idx), nn_indices, nn_dists, shot_);
1442 
1443  // Copy into the resultant cloud
1444  for (int d = 0; d < shot_.size (); ++d)
1445  output.points (idx, d) = shot_[d];
1446  for (int d = 0; d < 9; ++d)
1447  output.points (idx, shot_.size () + d) = frames_->points[idx].rf[ (4*(d/3) + (d%3)) ];
1448  }
1449 }
1450 */
1454 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
1455 pcl::SHOTEstimation<PointInT, PointNT, PointOutT, PointRFT>::computeFeature (pcl::PointCloud<PointOutT> &output)
1456 {
1457  descLength_ = nr_grid_sector_ * (nr_shape_bins_+1);
1458 
1459  sqradius_ = search_radius_ * search_radius_;
1460  radius3_4_ = (search_radius_*3) / 4;
1461  radius1_4_ = search_radius_ / 4;
1462  radius1_2_ = search_radius_ / 2;
1463 
1464  assert(descLength_ == 352);
1465 
1466  shot_.setZero (descLength_);
1467 
1468  // Allocate enough space to hold the results
1469  // \note This resize is irrelevant for a radiusSearch ().
1470  std::vector<int> nn_indices (k_);
1471  std::vector<float> nn_dists (k_);
1472 
1473  output.is_dense = true;
1474  // Iterating over the entire index vector
1475  for (size_t idx = 0; idx < indices_->size (); ++idx)
1476  {
1477  bool lrf_is_nan = false;
1478  const PointRFT& current_frame = (*frames_)[idx];
1479  if (!pcl_isfinite (current_frame.rf[0]) ||
1480  !pcl_isfinite (current_frame.rf[4]) ||
1481  !pcl_isfinite (current_frame.rf[11]))
1482  {
1483  PCL_WARN ("[pcl::%s::computeFeature] The local reference frame is not valid! Aborting description of point with index %d\n",
1484  getClassName ().c_str (), (*indices_)[idx]);
1485  lrf_is_nan = true;
1486  }
1487 
1488  if (!isFinite ((*input_)[(*indices_)[idx]]) ||
1489  lrf_is_nan ||
1490  this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
1491  {
1492  // Copy into the resultant cloud
1493  for (int d = 0; d < descLength_; ++d)
1494  output.points[idx].descriptor[d] = std::numeric_limits<float>::quiet_NaN ();
1495  for (int d = 0; d < 9; ++d)
1496  output.points[idx].rf[d] = std::numeric_limits<float>::quiet_NaN ();
1497 
1498  output.is_dense = false;
1499  continue;
1500  }
1501 
1502  // Estimate the SHOT descriptor at each patch
1503  computePointSHOT (static_cast<int> (idx), nn_indices, nn_dists, shot_);
1504 
1505  // Copy into the resultant cloud
1506  for (int d = 0; d < descLength_; ++d)
1507  output.points[idx].descriptor[d] = shot_[d];
1508  for (int d = 0; d < 9; ++d)
1509  output.points[idx].rf[d] = frames_->points[idx].rf[ (4*(d/3) + (d%3)) ];
1510  }
1511 }
1512 
1516 template <typename PointInT, typename PointNT, typename PointRFT> void
1517 pcl::SHOTEstimation<PointInT, PointNT, Eigen::MatrixXf, PointRFT>::computeFeatureEigen (pcl::PointCloud<Eigen::MatrixXf> &output)
1518 {
1519  descLength_ = nr_grid_sector_ * (nr_shape_bins_+1);
1520 
1521  // Set up the output channels
1522  output.channels["shot"].name = "shot";
1523  output.channels["shot"].offset = 0;
1524  output.channels["shot"].size = 4;
1525  output.channels["shot"].count = descLength_ + 9;
1526  output.channels["shot"].datatype = sensor_msgs::PointField::FLOAT32;
1527 
1528  sqradius_ = search_radius_ * search_radius_;
1529  radius3_4_ = (search_radius_*3) / 4;
1530  radius1_4_ = search_radius_ / 4;
1531  radius1_2_ = search_radius_ / 2;
1532 
1533  shot_.setZero (descLength_);
1534 
1535  output.points.resize (indices_->size (), descLength_ + 9);
1536 
1537  // Allocate enough space to hold the results
1538  // \note This resize is irrelevant for a radiusSearch ().
1539  std::vector<int> nn_indices (k_);
1540  std::vector<float> nn_dists (k_);
1541 
1542  output.is_dense = true;
1543  // Iterating over the entire index vector
1544  for (size_t idx = 0; idx < indices_->size (); ++idx)
1545  {
1546  bool lrf_is_nan = false;
1547  const PointRFT& current_frame = (*frames_)[idx];
1548  if (!pcl_isfinite (current_frame.rf[0]) ||
1549  !pcl_isfinite (current_frame.rf[4]) ||
1550  !pcl_isfinite (current_frame.rf[11]))
1551  {
1552  PCL_WARN ("[pcl::%s::computeFeature] The local reference frame is not valid! Aborting description of point with index %d\n",
1553  getClassName ().c_str (), (*indices_)[idx]);
1554  lrf_is_nan = true;
1555  }
1556 
1557  if (!isFinite ((*input_)[(*indices_)[idx]]) ||
1558  lrf_is_nan ||
1559  this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
1560  {
1561  output.points.row (idx).setConstant (std::numeric_limits<float>::quiet_NaN ());
1562 
1563  output.is_dense = false;
1564  continue;
1565  }
1566 
1567  // Estimate the SHOT at each patch
1568  this->computePointSHOT (static_cast<int> (idx), nn_indices, nn_dists, shot_);
1569 
1570  // Copy into the resultant cloud
1571  for (int d = 0; d < descLength_; ++d)
1572  output.points (idx, d) = shot_[d];
1573  for (int d = 0; d < 9; ++d)
1574  output.points (idx, shot_.size () + d) = frames_->points[idx].rf[ (4*(d/3) + (d%3)) ];
1575  }
1576 }
1577 
1581 template <typename PointInT, typename PointNT, typename PointOutT, typename PointRFT> void
1582 pcl::SHOTColorEstimation<PointInT, PointNT, PointOutT, PointRFT>::computeFeature (pcl::PointCloud<PointOutT> &output)
1583 {
1584  // Compute the current length of the descriptor
1585  descLength_ = (b_describe_shape_) ? nr_grid_sector_*(nr_shape_bins_+1) : 0;
1586  descLength_ += (b_describe_color_) ? nr_grid_sector_*(nr_color_bins_+1) : 0;
1587 
1588  assert( (!b_describe_color_ && b_describe_shape_ && descLength_ == 352) ||
1589  (b_describe_color_ && !b_describe_shape_ && descLength_ == 992) ||
1590  (b_describe_color_ && b_describe_shape_ && descLength_ == 1344)
1591  );
1592 
1593  // Useful values
1594  sqradius_ = search_radius_*search_radius_;
1595  radius3_4_ = (search_radius_*3) / 4;
1596  radius1_4_ = search_radius_ / 4;
1597  radius1_2_ = search_radius_ / 2;
1598 
1599  shot_.setZero (descLength_);
1600 
1601  // Allocate enough space to hold the results
1602  // \note This resize is irrelevant for a radiusSearch ().
1603  std::vector<int> nn_indices (k_);
1604  std::vector<float> nn_dists (k_);
1605 
1606  output.is_dense = true;
1607  // Iterating over the entire index vector
1608  for (size_t idx = 0; idx < indices_->size (); ++idx)
1609  {
1610  bool lrf_is_nan = false;
1611  const PointRFT& current_frame = (*frames_)[idx];
1612  if (!pcl_isfinite (current_frame.rf[0]) ||
1613  !pcl_isfinite (current_frame.rf[4]) ||
1614  !pcl_isfinite (current_frame.rf[11]))
1615  {
1616  PCL_WARN ("[pcl::%s::computeFeature] The local reference frame is not valid! Aborting description of point with index %d\n",
1617  getClassName ().c_str (), (*indices_)[idx]);
1618  lrf_is_nan = true;
1619  }
1620 
1621  if (!isFinite ((*input_)[(*indices_)[idx]]) ||
1622  lrf_is_nan ||
1623  this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
1624  {
1625  // Copy into the resultant cloud
1626  for (int d = 0; d < descLength_; ++d)
1627  output.points[idx].descriptor[d] = std::numeric_limits<float>::quiet_NaN ();
1628  for (int d = 0; d < 9; ++d)
1629  output.points[idx].rf[d] = std::numeric_limits<float>::quiet_NaN ();
1630 
1631  output.is_dense = false;
1632  continue;
1633  }
1634 
1635  // Compute the SHOT descriptor for the current 3D feature
1636  computePointSHOT (static_cast<int> (idx), nn_indices, nn_dists, shot_);
1637 
1638  // Copy into the resultant cloud
1639  for (int d = 0; d < descLength_; ++d)
1640  output.points[idx].descriptor[d] = shot_[d];
1641  for (int d = 0; d < 9; ++d)
1642  output.points[idx].rf[d] = frames_->points[idx].rf[ (4*(d/3) + (d%3)) ];
1643  }
1644 }
1645 
1649 template <typename PointInT, typename PointNT, typename PointRFT> void
1650 pcl::SHOTColorEstimation<PointInT, PointNT, Eigen::MatrixXf, PointRFT>::computeFeatureEigen (pcl::PointCloud<Eigen::MatrixXf> &output)
1651 {
1652  // Compute the current length of the descriptor
1653  descLength_ = (b_describe_shape_) ? nr_grid_sector_*(nr_shape_bins_+1) : 0;
1654  descLength_ += (b_describe_color_) ? nr_grid_sector_*(nr_color_bins_+1) : 0;
1655 
1656  // Set up the output channels
1657  output.channels["shot"].name = "shot";
1658  output.channels["shot"].offset = 0;
1659  output.channels["shot"].size = 4;
1660  output.channels["shot"].count = descLength_ + 9;
1661  output.channels["shot"].datatype = sensor_msgs::PointField::FLOAT32;
1662 
1663  // Useful values
1664  sqradius_ = search_radius_*search_radius_;
1665  radius3_4_ = (search_radius_*3) / 4;
1666  radius1_4_ = search_radius_ / 4;
1667  radius1_2_ = search_radius_ / 2;
1668 
1669  shot_.setZero (descLength_);
1670 
1671  output.points.resize (indices_->size (), descLength_ + 9);
1672 
1673  // Allocate enough space to hold the results
1674  // \note This resize is irrelevant for a radiusSearch ().
1675  std::vector<int> nn_indices (k_);
1676  std::vector<float> nn_dists (k_);
1677 
1678  output.is_dense = true;
1679  // Iterating over the entire index vector
1680  for (size_t idx = 0; idx < indices_->size (); ++idx)
1681  {
1682  bool lrf_is_nan = false;
1683  const PointRFT& current_frame = (*frames_)[idx];
1684  if (!pcl_isfinite (current_frame.rf[0]) ||
1685  !pcl_isfinite (current_frame.rf[4]) ||
1686  !pcl_isfinite (current_frame.rf[11]))
1687  {
1688  PCL_WARN ("[pcl::%s::computeFeature] The local reference frame is not valid! Aborting description of point with index %d\n",
1689  getClassName ().c_str (), (*indices_)[idx]);
1690  lrf_is_nan = true;
1691  }
1692 
1693  if (!isFinite ((*input_)[(*indices_)[idx]]) ||
1694  lrf_is_nan ||
1695  this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
1696  {
1697  output.points.row (idx).setConstant (std::numeric_limits<float>::quiet_NaN ());
1698 
1699  output.is_dense = false;
1700  continue;
1701  }
1702 
1703  // Compute the SHOT descriptor for the current 3D feature
1704  this->computePointSHOT (static_cast<int> (idx), nn_indices, nn_dists, shot_);
1705 
1706  // Copy into the resultant cloud
1707  for (int d = 0; d < descLength_; ++d)
1708  output.points (idx, d) = shot_[d];
1709  for (int d = 0; d < 9; ++d)
1710  output.points (idx, shot_.size () + d) = frames_->points[idx].rf[ (4*(d/3) + (d%3)) ];
1711  }
1712 }
1713 
1714 #define PCL_INSTANTIATE_SHOTEstimationBase(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTEstimationBase<T,NT,OutT,RFT>;
1715 #define PCL_INSTANTIATE_SHOTEstimation(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTEstimation<T,NT,OutT,RFT>;
1716 #define PCL_INSTANTIATE_SHOTColorEstimation(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTColorEstimation<T,NT,OutT,RFT>;
1717 
1718 #endif // PCL_FEATURES_IMPL_SHOT_H_
1719 
Generated on Sat Aug 24 2013 17:28:29 for Point Cloud Library (PCL) by   doxygen 1.8.4