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];
203 double cosineDesc = normal_vec.dot (current_frame_z);
204
205 if (cosineDesc > 1.0)
206 cosineDesc = 1.0;
207 if (cosineDesc < - 1.0)
208 cosineDesc = - 1.0;
209
210 bin_distance_shape[i_idx] = ((1.0 + cosineDesc) * nr_shape_bins_) / 2;
211 }
212 }
213 if (nan_counter > 0)
214 PCL_WARN ("[pcl::%s::createBinDistanceShape] Point %d has %d (%f%%) NaN normals in its neighbourhood\n",
215 getClassName ().c_str (), index, nan_counter, (static_cast<float>(nan_counter)*100.f/static_cast<float>(indices.size ())));
216}
217
218//////////////////////////////////////////////////////////////////////////////////////////////
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];
230 acc_norm = sqrt (acc_norm);
231 for (int j = 0; j < desc_length; j++)
232 shot[j] /= static_cast<float> (acc_norm);
233}
234
235//////////////////////////////////////////////////////////////////////////////////////////////
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)
302 binDistance[i_idx] -= step_index;
303 double intWeight = (1- std::abs (binDistance[i_idx]));
304
305 if (binDistance[i_idx] > 0)
306 shot[volume_index + ((step_index+1) % nr_bins)] += static_cast<float> (binDistance[i_idx]);
307 else
308 shot[volume_index + ((step_index - 1 + nr_bins) % nr_bins)] += - static_cast<float> (binDistance[i_idx]);
309
310 //Interpolation on the distance (adjacent husks)
311
312 if (distance > radius1_2_) //external sphere
313 {
314 double radiusDistance = (distance - radius3_4_) / radius1_2_;
315
316 if (distance > radius3_4_) //most external sector, votes only for itself
317 intWeight += 1 - radiusDistance; //peso=1-d
318 else //3/4 of radius, votes also for the internal sphere
319 {
320 intWeight += 1 + radiusDistance;
321 shot[(desc_index - 2) * (nr_bins+1) + step_index] -= static_cast<float> (radiusDistance);
322 }
323 }
324 else //internal sphere
325 {
326 double radiusDistance = (distance - radius1_4_) / radius1_2_;
327
328 if (distance < radius1_4_) //most internal sector, votes only for itself
329 intWeight += 1 + radiusDistance; //weight=1-d
330 else //3/4 of radius, votes also for the external sphere
331 {
332 intWeight += 1 - radiusDistance;
333 shot[(desc_index + 2) * (nr_bins+1) + step_index] += static_cast<float> (radiusDistance);
334 }
335 }
336
337 //Interpolation on the inclination (adjacent vertical volumes)
338 double inclinationCos = zInFeatRef / distance;
339 if (inclinationCos < - 1.0)
340 inclinationCos = - 1.0;
341 if (inclinationCos > 1.0)
342 inclinationCos = 1.0;
343
344 double inclination = std::acos (inclinationCos);
345
346 assert (inclination >= 0.0 && inclination <= PST_RAD_180);
347
348 if (inclination > PST_RAD_90 || (std::abs (inclination - PST_RAD_90) < 1e-30 && zInFeatRef <= 0))
349 {
350 double inclinationDistance = (inclination - PST_RAD_135) / PST_RAD_90;
351 if (inclination > PST_RAD_135)
352 intWeight += 1 - inclinationDistance;
353 else
354 {
355 intWeight += 1 + inclinationDistance;
356 assert ((desc_index + 1) * (nr_bins+1) + step_index >= 0 && (desc_index + 1) * (nr_bins+1) + step_index < descLength_);
357 shot[(desc_index + 1) * (nr_bins+1) + step_index] -= static_cast<float> (inclinationDistance);
358 }
359 }
360 else
362 double inclinationDistance = (inclination - PST_RAD_45) / PST_RAD_90;
363 if (inclination < PST_RAD_45)
364 intWeight += 1 + inclinationDistance;
365 else
366 {
367 intWeight += 1 - inclinationDistance;
368 assert ((desc_index - 1) * (nr_bins+1) + step_index >= 0 && (desc_index - 1) * (nr_bins+1) + step_index < descLength_);
369 shot[(desc_index - 1) * (nr_bins+1) + step_index] += static_cast<float> (inclinationDistance);
370 }
371 }
372
373 if (yInFeatRef != 0.0 || xInFeatRef != 0.0)
375 //Interpolation on the azimuth (adjacent horizontal volumes)
376 double azimuth = std::atan2 (yInFeatRef, xInFeatRef);
377
378 int sel = desc_index >> 2;
379 double angularSectorSpan = PST_RAD_45;
380 double angularSectorStart = - PST_RAD_PI_7_8;
381
382 double azimuthDistance = (azimuth - (angularSectorStart + angularSectorSpan*sel)) / angularSectorSpan;
383
384 assert ((azimuthDistance < 0.5 || areEquals (azimuthDistance, 0.5)) && (azimuthDistance > - 0.5 || areEquals (azimuthDistance, - 0.5)));
385
386 azimuthDistance = (std::max)(- 0.5, std::min (azimuthDistance, 0.5));
387
388 if (azimuthDistance > 0)
389 {
390 intWeight += 1 - azimuthDistance;
391 int interp_index = (desc_index + 4) % maxAngularSectors_;
392 assert (interp_index * (nr_bins+1) + step_index >= 0 && interp_index * (nr_bins+1) + step_index < descLength_);
393 shot[interp_index * (nr_bins+1) + step_index] += static_cast<float> (azimuthDistance);
394 }
395 else
396 {
397 int interp_index = (desc_index - 4 + maxAngularSectors_) % maxAngularSectors_;
398 assert (interp_index * (nr_bins+1) + step_index >= 0 && interp_index * (nr_bins+1) + step_index < descLength_);
399 intWeight += 1 + azimuthDistance;
400 shot[interp_index * (nr_bins+1) + step_index] -= static_cast<float> (azimuthDistance);
401 }
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
479 int volume_index_shape = desc_index * (nr_bins_shape+1);
480 int volume_index_color = shapeToColorStride + desc_index * (nr_bins_color+1);
481
482 //Interpolation on the cosine (adjacent bins in the histrogram)
483 binDistanceShape[i_idx] -= step_index_shape;
484 binDistanceColor[i_idx] -= step_index_color;
485
486 double intWeightShape = (1- std::abs (binDistanceShape[i_idx]));
487 double intWeightColor = (1- std::abs (binDistanceColor[i_idx]));
488
489 if (binDistanceShape[i_idx] > 0)
490 shot[volume_index_shape + ((step_index_shape + 1) % nr_bins_shape)] += static_cast<float> (binDistanceShape[i_idx]);
491 else
492 shot[volume_index_shape + ((step_index_shape - 1 + nr_bins_shape) % nr_bins_shape)] -= static_cast<float> (binDistanceShape[i_idx]);
493
494 if (binDistanceColor[i_idx] > 0)
495 shot[volume_index_color + ((step_index_color+1) % nr_bins_color)] += static_cast<float> (binDistanceColor[i_idx]);
496 else
497 shot[volume_index_color + ((step_index_color - 1 + nr_bins_color) % nr_bins_color)] -= static_cast<float> (binDistanceColor[i_idx]);
498
499 //Interpolation on the distance (adjacent husks)
500
501 if (distance > radius1_2_) //external sphere
502 {
503 double radiusDistance = (distance - radius3_4_) / radius1_2_;
504
505 if (distance > radius3_4_) //most external sector, votes only for itself
506 {
507 intWeightShape += 1 - radiusDistance; //weight=1-d
508 intWeightColor += 1 - radiusDistance; //weight=1-d
509 }
510 else //3/4 of radius, votes also for the internal sphere
511 {
512 intWeightShape += 1 + radiusDistance;
513 intWeightColor += 1 + radiusDistance;
514 shot[(desc_index - 2) * (nr_bins_shape+1) + step_index_shape] -= static_cast<float> (radiusDistance);
515 shot[shapeToColorStride + (desc_index - 2) * (nr_bins_color+1) + step_index_color] -= static_cast<float> (radiusDistance);
516 }
517 }
518 else //internal sphere
519 {
520 double radiusDistance = (distance - radius1_4_) / radius1_2_;
521
522 if (distance < radius1_4_) //most internal sector, votes only for itself
523 {
524 intWeightShape += 1 + radiusDistance;
525 intWeightColor += 1 + radiusDistance; //weight=1-d
526 }
527 else //3/4 of radius, votes also for the external sphere
528 {
529 intWeightShape += 1 - radiusDistance; //weight=1-d
530 intWeightColor += 1 - radiusDistance; //weight=1-d
531 shot[(desc_index + 2) * (nr_bins_shape+1) + step_index_shape] += static_cast<float> (radiusDistance);
532 shot[shapeToColorStride + (desc_index + 2) * (nr_bins_color+1) + step_index_color] += static_cast<float> (radiusDistance);
533 }
534 }
535
536 //Interpolation on the inclination (adjacent vertical volumes)
537 double inclinationCos = zInFeatRef / distance;
538 if (inclinationCos < - 1.0)
539 inclinationCos = - 1.0;
540 if (inclinationCos > 1.0)
541 inclinationCos = 1.0;
542
543 double inclination = std::acos (inclinationCos);
544
545 assert (inclination >= 0.0 && inclination <= PST_RAD_180);
546
547 if (inclination > PST_RAD_90 || (std::abs (inclination - PST_RAD_90) < 1e-30 && zInFeatRef <= 0))
548 {
549 double inclinationDistance = (inclination - PST_RAD_135) / PST_RAD_90;
550 if (inclination > PST_RAD_135)
551 {
552 intWeightShape += 1 - inclinationDistance;
553 intWeightColor += 1 - inclinationDistance;
554 }
555 else
556 {
557 intWeightShape += 1 + inclinationDistance;
558 intWeightColor += 1 + inclinationDistance;
559 assert ((desc_index + 1) * (nr_bins_shape+1) + step_index_shape >= 0 && (desc_index + 1) * (nr_bins_shape+1) + step_index_shape < descLength_);
560 assert (shapeToColorStride + (desc_index + 1) * (nr_bins_color+ 1) + step_index_color >= 0 && shapeToColorStride + (desc_index + 1) * (nr_bins_color+1) + step_index_color < descLength_);
561 shot[(desc_index + 1) * (nr_bins_shape+1) + step_index_shape] -= static_cast<float> (inclinationDistance);
562 shot[shapeToColorStride + (desc_index + 1) * (nr_bins_color+1) + step_index_color] -= static_cast<float> (inclinationDistance);
563 }
564 }
565 else
566 {
567 double inclinationDistance = (inclination - PST_RAD_45) / PST_RAD_90;
568 if (inclination < PST_RAD_45)
569 {
570 intWeightShape += 1 + inclinationDistance;
571 intWeightColor += 1 + inclinationDistance;
572 }
573 else
574 {
575 intWeightShape += 1 - inclinationDistance;
576 intWeightColor += 1 - inclinationDistance;
577 assert ((desc_index - 1) * (nr_bins_shape+1) + step_index_shape >= 0 && (desc_index - 1) * (nr_bins_shape+1) + step_index_shape < descLength_);
578 assert (shapeToColorStride + (desc_index - 1) * (nr_bins_color+ 1) + step_index_color >= 0 && shapeToColorStride + (desc_index - 1) * (nr_bins_color+1) + step_index_color < descLength_);
579 shot[(desc_index - 1) * (nr_bins_shape+1) + step_index_shape] += static_cast<float> (inclinationDistance);
580 shot[shapeToColorStride + (desc_index - 1) * (nr_bins_color+1) + step_index_color] += static_cast<float> (inclinationDistance);
581 }
582 }
583
584 if (yInFeatRef != 0.0 || xInFeatRef != 0.0)
585 {
586 //Interpolation on the azimuth (adjacent horizontal volumes)
587 double azimuth = std::atan2 (yInFeatRef, xInFeatRef);
588
589 int sel = desc_index >> 2;
590 double angularSectorSpan = PST_RAD_45;
591 double angularSectorStart = - PST_RAD_PI_7_8;
592
593 double azimuthDistance = (azimuth - (angularSectorStart + angularSectorSpan*sel)) / angularSectorSpan;
594 assert ((azimuthDistance < 0.5 || areEquals (azimuthDistance, 0.5)) && (azimuthDistance > - 0.5 || areEquals (azimuthDistance, - 0.5)));
595 azimuthDistance = (std::max)(- 0.5, std::min (azimuthDistance, 0.5));
596
597 if (azimuthDistance > 0)
598 {
599 intWeightShape += 1 - azimuthDistance;
600 intWeightColor += 1 - azimuthDistance;
601 int interp_index = (desc_index + 4) % maxAngularSectors_;
602 assert (interp_index * (nr_bins_shape+1) + step_index_shape >= 0 && interp_index * (nr_bins_shape+1) + step_index_shape < descLength_);
603 assert (shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color >= 0 && shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color < descLength_);
604 shot[interp_index * (nr_bins_shape+1) + step_index_shape] += static_cast<float> (azimuthDistance);
605 shot[shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color] += static_cast<float> (azimuthDistance);
606 }
607 else
608 {
609 int interp_index = (desc_index - 4 + maxAngularSectors_) % maxAngularSectors_;
610 intWeightShape += 1 + azimuthDistance;
611 intWeightColor += 1 + azimuthDistance;
612 assert (interp_index * (nr_bins_shape+1) + step_index_shape >= 0 && interp_index * (nr_bins_shape+1) + step_index_shape < descLength_);
613 assert (shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color >= 0 && shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color < descLength_);
614 shot[interp_index * (nr_bins_shape+1) + step_index_shape] -= static_cast<float> (azimuthDistance);
615 shot[shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color] -= static_cast<float> (azimuthDistance);
616 }
617 }
618
619 assert (volume_index_shape + step_index_shape >= 0 && volume_index_shape + step_index_shape < descLength_);
620 assert (volume_index_color + step_index_color >= 0 && volume_index_color + step_index_color < descLength_);
621 shot[volume_index_shape + step_index_shape] += static_cast<float> (intWeightShape);
622 shot[volume_index_color + step_index_color] += static_cast<float> (intWeightColor);
623 }
624}
625
626//////////////////////////////////////////////////////////////////////////////////////////////
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 {
656 binDistanceColor.reserve (nNeighbors);
657
658 //unsigned char redRef = (*input_)[(*indices_)[index]].rgba >> 16 & 0xFF;
659 //unsigned char greenRef = (*input_)[(*indices_)[index]].rgba >> 8& 0xFF;
660 //unsigned char blueRef = (*input_)[(*indices_)[index]].rgba & 0xFF;
661 unsigned char redRef = (*input_)[(*indices_)[index]].r;
662 unsigned char greenRef = (*input_)[(*indices_)[index]].g;
663 unsigned char blueRef = (*input_)[(*indices_)[index]].b;
664
665 float LRef, aRef, bRef;
666
667 RGB2CIELAB (redRef, greenRef, blueRef, LRef, aRef, bRef);
668 LRef /= 100.0f;
669 aRef /= 120.0f;
670 bRef /= 120.0f; //normalized LAB components (0<L<1, -1<a<1, -1<b<1)
671
672 for (const auto& idx: indices)
673 {
674 unsigned char red = (*surface_)[idx].r;
675 unsigned char green = (*surface_)[idx].g;
676 unsigned char blue = (*surface_)[idx].b;
677
678 float L, a, b;
679
680 RGB2CIELAB (red, green, blue, L, a, b);
681 L /= 100.0f;
682 a /= 120.0f;
683 b /= 120.0f; //normalized LAB components (0<L<1, -1<a<1, -1<b<1)
684
685 double colorDistance = (std::fabs (LRef - L) + ((std::fabs (aRef - a) + std::fabs (bRef - b)) / 2)) /3;
686
687 if (colorDistance > 1.0)
688 colorDistance = 1.0;
689 if (colorDistance < 0.0)
690 colorDistance = 0.0;
691
692 binDistanceColor.push_back (colorDistance * nr_color_bins_);
693 }
694 }
695
696 //Apply quadrilinear interpolation on the activated bins in the shape and/or color histogram(s)
697
698 if (b_describe_shape_ && b_describe_color_)
699 interpolateDoubleChannel (indices, sqr_dists, index, binDistanceShape, binDistanceColor,
700 nr_shape_bins_, nr_color_bins_,
701 shot);
702 else if (b_describe_color_)
703 interpolateSingleChannel (indices, sqr_dists, index, binDistanceColor, nr_color_bins_, shot);
704 else
705 interpolateSingleChannel (indices, sqr_dists, index, binDistanceShape, nr_shape_bins_, shot);
706
707 // Normalize the final histogram
708 this->normalizeHistogram (shot, descLength_);
709}
710
711//////////////////////////////////////////////////////////////////////////////////////////////
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 ().
758 pcl::Indices nn_indices (k_);
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 ().
830 pcl::Indices nn_indices (k_);
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_
FeatureWithLocalReferenceFrames provides a public interface for descriptor extractor classes which ne...
Definition feature.h:449
PointCloud represents the base class in PCL for storing collections of 3D points.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields).
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
SHOTLocalReferenceFrameEstimation estimates the Local Reference Frame used in the calculation of the ...
Definition shot_lrf.h:66
shared_ptr< SHOTLocalReferenceFrameEstimation< PointInT, PointOutT > > Ptr
Definition shot_lrf.h:68
@ B
Definition norms.h:54
float distance(const PointT &p1, const PointT &p2)
Definition geometry.h:60
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
Definition point_tests.h:55
PCL_EXPORTS const std::array< T, discretizations > & XYZ2LAB_LUT() noexcept
Returns a Look-Up Table useful in converting scaled CIE XYZ into CIE L*a*b*.
Definition colors.h:148
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
PCL_EXPORTS std::array< T, 1<< bits > RGB2sRGB_LUT() noexcept
Returns a Look-Up Table useful in converting RGB to sRGB.
Definition colors.h:102