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