40 #ifndef PCL_FEATURES_IMPL_SHOT_H_
41 #define PCL_FEATURES_IMPL_SHOT_H_
43 #include <pcl/features/shot.h>
44 #include <pcl/features/shot_lrf.h>
48 #define PST_PI 3.1415926535897932384626433832795
49 #define PST_RAD_45 0.78539816339744830961566084581988
50 #define PST_RAD_90 1.5707963267948966192313216916398
51 #define PST_RAD_135 2.3561944901923449288469825374596
52 #define PST_RAD_180 PST_PI
53 #define PST_RAD_360 6.283185307179586476925286766558
54 #define PST_RAD_PI_7_8 2.7488935718910690836548129603691
56 const double zeroDoubleEps15 = 1E-15;
57 const float zeroFloatEps8 = 1E-8f;
68 areEquals (
double val1,
double val2,
double zeroDoubleEps = zeroDoubleEps15)
70 return (std::abs (val1 - val2)<zeroDoubleEps);
82 areEquals (
float val1,
float val2,
float zeroFloatEps = zeroFloatEps8)
84 return (std::fabs (val1 - val2)<zeroFloatEps);
88 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
float
92 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
float
96 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
void
98 unsigned char B,
float &L,
float &A,
104 for (
int i = 0; i < 256; i++)
106 float f =
static_cast<float> (i) / 255.0f;
108 sRGB_LUT[i] = powf ((f + 0.055f) / 1.055f, 2.4f);
110 sRGB_LUT[i] = f / 12.92f;
113 for (
int i = 0; i < 4000; i++)
115 float f =
static_cast<float> (i) / 4000.0f;
117 sXYZ_LUT[i] =
static_cast<float> (powf (f, 0.3333f));
119 sXYZ_LUT[i] =
static_cast<float>((7.787 * f) + (16.0 / 116.0));
123 float fr = sRGB_LUT[R];
124 float fg = sRGB_LUT[G];
125 float fb = sRGB_LUT[
B];
128 const float x = fr * 0.412453f + fg * 0.357580f + fb * 0.180423f;
129 const float y = fr * 0.212671f + fg * 0.715160f + fb * 0.072169f;
130 const float z = fr * 0.019334f + fg * 0.119193f + fb * 0.950227f;
132 float vx = x / 0.95047f;
134 float vz = z / 1.08883f;
136 vx = sXYZ_LUT[int(vx*4000)];
137 vy = sXYZ_LUT[int(vy*4000)];
138 vz = sXYZ_LUT[int(vz*4000)];
140 L = 116.0f * vy - 16.0f;
144 A = 500.0f * (vx - vy);
150 B2 = 200.0f * (vy - vz);
158 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
bool
163 PCL_ERROR (
"[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
168 if (this->getKSearch () != 0)
171 "[pcl::%s::initCompute] Error! Search method set to k-neighborhood. Call setKSearch(0) and setRadiusSearch( radius ) to use this class.\n",
172 getClassName().c_str ());
178 lrf_estimator->
setRadiusSearch ((lrf_radius_ > 0 ? lrf_radius_ : search_radius_));
186 PCL_ERROR (
"[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
194 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
void
197 const std::vector<int> &indices,
198 std::vector<double> &bin_distance_shape)
200 bin_distance_shape.resize (indices.size ());
202 const PointRFT& current_frame = frames_->points[index];
206 Eigen::Vector4f current_frame_z (current_frame.z_axis[0], current_frame.z_axis[1], current_frame.z_axis[2], 0);
208 unsigned nan_counter = 0;
209 for (std::size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
212 const Eigen::Vector4f& normal_vec = normals_->points[indices[i_idx]].getNormalVector4fMap ();
213 if (!std::isfinite (normal_vec[0]) ||
214 !std::isfinite (normal_vec[1]) ||
215 !std::isfinite (normal_vec[2]))
217 bin_distance_shape[i_idx] = std::numeric_limits<double>::quiet_NaN ();
222 double cosineDesc = normal_vec.dot (current_frame_z);
224 if (cosineDesc > 1.0)
226 if (cosineDesc < - 1.0)
229 bin_distance_shape[i_idx] = ((1.0 + cosineDesc) * nr_shape_bins_) / 2;
233 PCL_WARN (
"[pcl::%s::createBinDistanceShape] Point %d has %d (%f%%) NaN normals in its neighbourhood\n",
234 getClassName ().c_str (), index, nan_counter, (static_cast<float>(nan_counter)*100.f/static_cast<float>(indices.size ())));
238 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
void
240 Eigen::VectorXf &shot,
int desc_length)
247 for (
int j = 0; j < desc_length; j++)
248 acc_norm += shot[j] * shot[j];
249 acc_norm = sqrt (acc_norm);
250 for (
int j = 0; j < desc_length; j++)
251 shot[j] /= static_cast<float> (acc_norm);
255 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
void
257 const std::vector<int> &indices,
258 const std::vector<float> &sqr_dists,
260 std::vector<double> &binDistance,
262 Eigen::VectorXf &shot)
264 const Eigen::Vector4f& central_point = (*input_)[(*indices_)[index]].getVector4fMap ();
265 const PointRFT& current_frame = (*frames_)[index];
267 Eigen::Vector4f current_frame_x (current_frame.x_axis[0], current_frame.x_axis[1], current_frame.x_axis[2], 0);
268 Eigen::Vector4f current_frame_y (current_frame.y_axis[0], current_frame.y_axis[1], current_frame.y_axis[2], 0);
269 Eigen::Vector4f current_frame_z (current_frame.z_axis[0], current_frame.z_axis[1], current_frame.z_axis[2], 0);
271 for (std::size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
273 if (!std::isfinite(binDistance[i_idx]))
276 Eigen::Vector4f delta = surface_->points[indices[i_idx]].getVector4fMap () - central_point;
280 double distance = sqrt (sqr_dists[i_idx]);
282 if (areEquals (distance, 0.0))
285 double xInFeatRef = delta.dot (current_frame_x);
286 double yInFeatRef = delta.dot (current_frame_y);
287 double zInFeatRef = delta.dot (current_frame_z);
290 if (std::abs (yInFeatRef) < 1E-30)
292 if (std::abs (xInFeatRef) < 1E-30)
294 if (std::abs (zInFeatRef) < 1E-30)
298 unsigned char bit4 = ((yInFeatRef > 0) || ((yInFeatRef == 0.0) && (xInFeatRef < 0))) ? 1 : 0;
299 unsigned char bit3 =
static_cast<unsigned char> (((xInFeatRef > 0) || ((xInFeatRef == 0.0) && (yInFeatRef > 0))) ? !bit4 : bit4);
301 assert (bit3 == 0 || bit3 == 1);
303 int desc_index = (bit4<<3) + (bit3<<2);
305 desc_index = desc_index << 1;
307 if ((xInFeatRef * yInFeatRef > 0) || (xInFeatRef == 0.0))
308 desc_index += (std::abs (xInFeatRef) >= std::abs (yInFeatRef)) ? 0 : 4;
310 desc_index += (std::abs (xInFeatRef) > std::abs (yInFeatRef)) ? 4 : 0;
312 desc_index += zInFeatRef > 0 ? 1 : 0;
315 desc_index += (distance > radius1_2_) ? 2 : 0;
317 int step_index =
static_cast<int>(std::floor (binDistance[i_idx] +0.5));
318 int volume_index = desc_index * (nr_bins+1);
321 binDistance[i_idx] -= step_index;
322 double intWeight = (1- std::abs (binDistance[i_idx]));
324 if (binDistance[i_idx] > 0)
325 shot[volume_index + ((step_index+1) % nr_bins)] +=
static_cast<float> (binDistance[i_idx]);
327 shot[volume_index + ((step_index - 1 + nr_bins) % nr_bins)] += -
static_cast<float> (binDistance[i_idx]);
331 if (distance > radius1_2_)
333 double radiusDistance = (distance - radius3_4_) / radius1_2_;
335 if (distance > radius3_4_)
336 intWeight += 1 - radiusDistance;
339 intWeight += 1 + radiusDistance;
340 shot[(desc_index - 2) * (nr_bins+1) + step_index] -=
static_cast<float> (radiusDistance);
345 double radiusDistance = (distance - radius1_4_) / radius1_2_;
347 if (distance < radius1_4_)
348 intWeight += 1 + radiusDistance;
351 intWeight += 1 - radiusDistance;
352 shot[(desc_index + 2) * (nr_bins+1) + step_index] +=
static_cast<float> (radiusDistance);
357 double inclinationCos = zInFeatRef / distance;
358 if (inclinationCos < - 1.0)
359 inclinationCos = - 1.0;
360 if (inclinationCos > 1.0)
361 inclinationCos = 1.0;
363 double inclination = std::acos (inclinationCos);
365 assert (inclination >= 0.0 && inclination <= PST_RAD_180);
367 if (inclination > PST_RAD_90 || (std::abs (inclination - PST_RAD_90) < 1e-30 && zInFeatRef <= 0))
369 double inclinationDistance = (inclination - PST_RAD_135) / PST_RAD_90;
370 if (inclination > PST_RAD_135)
371 intWeight += 1 - inclinationDistance;
374 intWeight += 1 + inclinationDistance;
375 assert ((desc_index + 1) * (nr_bins+1) + step_index >= 0 && (desc_index + 1) * (nr_bins+1) + step_index < descLength_);
376 shot[(desc_index + 1) * (nr_bins+1) + step_index] -=
static_cast<float> (inclinationDistance);
381 double inclinationDistance = (inclination - PST_RAD_45) / PST_RAD_90;
382 if (inclination < PST_RAD_45)
383 intWeight += 1 + inclinationDistance;
386 intWeight += 1 - inclinationDistance;
387 assert ((desc_index - 1) * (nr_bins+1) + step_index >= 0 && (desc_index - 1) * (nr_bins+1) + step_index < descLength_);
388 shot[(desc_index - 1) * (nr_bins+1) + step_index] +=
static_cast<float> (inclinationDistance);
392 if (yInFeatRef != 0.0 || xInFeatRef != 0.0)
395 double azimuth = std::atan2 (yInFeatRef, xInFeatRef);
397 int sel = desc_index >> 2;
398 double angularSectorSpan = PST_RAD_45;
399 double angularSectorStart = - PST_RAD_PI_7_8;
401 double azimuthDistance = (azimuth - (angularSectorStart + angularSectorSpan*sel)) / angularSectorSpan;
403 assert ((azimuthDistance < 0.5 || areEquals (azimuthDistance, 0.5)) && (azimuthDistance > - 0.5 || areEquals (azimuthDistance, - 0.5)));
405 azimuthDistance = (std::max)(- 0.5, std::min (azimuthDistance, 0.5));
407 if (azimuthDistance > 0)
409 intWeight += 1 - azimuthDistance;
410 int interp_index = (desc_index + 4) % maxAngularSectors_;
411 assert (interp_index * (nr_bins+1) + step_index >= 0 && interp_index * (nr_bins+1) + step_index < descLength_);
412 shot[interp_index * (nr_bins+1) + step_index] += static_cast<float> (azimuthDistance);
416 int interp_index = (desc_index - 4 + maxAngularSectors_) % maxAngularSectors_;
417 assert (interp_index * (nr_bins+1) + step_index >= 0 && interp_index * (nr_bins+1) + step_index < descLength_);
418 intWeight += 1 + azimuthDistance;
419 shot[interp_index * (nr_bins+1) + step_index] -= static_cast<float> (azimuthDistance);
424 assert (volume_index + step_index >= 0 && volume_index + step_index < descLength_);
425 shot[volume_index + step_index] +=
static_cast<float> (intWeight);
430 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
void
432 const std::vector<int> &indices,
433 const std::vector<float> &sqr_dists,
435 std::vector<double> &binDistanceShape,
436 std::vector<double> &binDistanceColor,
437 const int nr_bins_shape,
438 const int nr_bins_color,
439 Eigen::VectorXf &shot)
441 const Eigen::Vector4f ¢ral_point = (*input_)[(*indices_)[index]].getVector4fMap ();
442 const PointRFT& current_frame = (*frames_)[index];
444 int shapeToColorStride = nr_grid_sector_*(nr_bins_shape+1);
446 Eigen::Vector4f current_frame_x (current_frame.x_axis[0], current_frame.x_axis[1], current_frame.x_axis[2], 0);
447 Eigen::Vector4f current_frame_y (current_frame.y_axis[0], current_frame.y_axis[1], current_frame.y_axis[2], 0);
448 Eigen::Vector4f current_frame_z (current_frame.z_axis[0], current_frame.z_axis[1], current_frame.z_axis[2], 0);
450 for (std::size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
452 if (!std::isfinite(binDistanceShape[i_idx]))
455 Eigen::Vector4f delta = surface_->points[indices[i_idx]].getVector4fMap () - central_point;
459 double distance = sqrt (sqr_dists[i_idx]);
461 if (areEquals (distance, 0.0))
464 double xInFeatRef = delta.dot (current_frame_x);
465 double yInFeatRef = delta.dot (current_frame_y);
466 double zInFeatRef = delta.dot (current_frame_z);
469 if (std::abs (yInFeatRef) < 1E-30)
471 if (std::abs (xInFeatRef) < 1E-30)
473 if (std::abs (zInFeatRef) < 1E-30)
476 unsigned char bit4 = ((yInFeatRef > 0) || ((yInFeatRef == 0.0) && (xInFeatRef < 0))) ? 1 : 0;
477 unsigned char bit3 =
static_cast<unsigned char> (((xInFeatRef > 0) || ((xInFeatRef == 0.0) && (yInFeatRef > 0))) ? !bit4 : bit4);
479 assert (bit3 == 0 || bit3 == 1);
481 int desc_index = (bit4<<3) + (bit3<<2);
483 desc_index = desc_index << 1;
485 if ((xInFeatRef * yInFeatRef > 0) || (xInFeatRef == 0.0))
486 desc_index += (std::abs (xInFeatRef) >= std::abs (yInFeatRef)) ? 0 : 4;
488 desc_index += (std::abs (xInFeatRef) > std::abs (yInFeatRef)) ? 4 : 0;
490 desc_index += zInFeatRef > 0 ? 1 : 0;
493 desc_index += (distance > radius1_2_) ? 2 : 0;
495 int step_index_shape =
static_cast<int>(std::floor (binDistanceShape[i_idx] +0.5));
496 int step_index_color =
static_cast<int>(std::floor (binDistanceColor[i_idx] +0.5));
498 int volume_index_shape = desc_index * (nr_bins_shape+1);
499 int volume_index_color = shapeToColorStride + desc_index * (nr_bins_color+1);
502 binDistanceShape[i_idx] -= step_index_shape;
503 binDistanceColor[i_idx] -= step_index_color;
505 double intWeightShape = (1- std::abs (binDistanceShape[i_idx]));
506 double intWeightColor = (1- std::abs (binDistanceColor[i_idx]));
508 if (binDistanceShape[i_idx] > 0)
509 shot[volume_index_shape + ((step_index_shape + 1) % nr_bins_shape)] +=
static_cast<float> (binDistanceShape[i_idx]);
511 shot[volume_index_shape + ((step_index_shape - 1 + nr_bins_shape) % nr_bins_shape)] -=
static_cast<float> (binDistanceShape[i_idx]);
513 if (binDistanceColor[i_idx] > 0)
514 shot[volume_index_color + ((step_index_color+1) % nr_bins_color)] +=
static_cast<float> (binDistanceColor[i_idx]);
516 shot[volume_index_color + ((step_index_color - 1 + nr_bins_color) % nr_bins_color)] -=
static_cast<float> (binDistanceColor[i_idx]);
520 if (distance > radius1_2_)
522 double radiusDistance = (distance - radius3_4_) / radius1_2_;
524 if (distance > radius3_4_)
526 intWeightShape += 1 - radiusDistance;
527 intWeightColor += 1 - radiusDistance;
531 intWeightShape += 1 + radiusDistance;
532 intWeightColor += 1 + radiusDistance;
533 shot[(desc_index - 2) * (nr_bins_shape+1) + step_index_shape] -=
static_cast<float> (radiusDistance);
534 shot[shapeToColorStride + (desc_index - 2) * (nr_bins_color+1) + step_index_color] -=
static_cast<float> (radiusDistance);
539 double radiusDistance = (distance - radius1_4_) / radius1_2_;
541 if (distance < radius1_4_)
543 intWeightShape += 1 + radiusDistance;
544 intWeightColor += 1 + radiusDistance;
548 intWeightShape += 1 - radiusDistance;
549 intWeightColor += 1 - radiusDistance;
550 shot[(desc_index + 2) * (nr_bins_shape+1) + step_index_shape] +=
static_cast<float> (radiusDistance);
551 shot[shapeToColorStride + (desc_index + 2) * (nr_bins_color+1) + step_index_color] +=
static_cast<float> (radiusDistance);
556 double inclinationCos = zInFeatRef / distance;
557 if (inclinationCos < - 1.0)
558 inclinationCos = - 1.0;
559 if (inclinationCos > 1.0)
560 inclinationCos = 1.0;
562 double inclination = std::acos (inclinationCos);
564 assert (inclination >= 0.0 && inclination <= PST_RAD_180);
566 if (inclination > PST_RAD_90 || (std::abs (inclination - PST_RAD_90) < 1e-30 && zInFeatRef <= 0))
568 double inclinationDistance = (inclination - PST_RAD_135) / PST_RAD_90;
569 if (inclination > PST_RAD_135)
571 intWeightShape += 1 - inclinationDistance;
572 intWeightColor += 1 - inclinationDistance;
576 intWeightShape += 1 + inclinationDistance;
577 intWeightColor += 1 + inclinationDistance;
578 assert ((desc_index + 1) * (nr_bins_shape+1) + step_index_shape >= 0 && (desc_index + 1) * (nr_bins_shape+1) + step_index_shape < descLength_);
579 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_);
580 shot[(desc_index + 1) * (nr_bins_shape+1) + step_index_shape] -=
static_cast<float> (inclinationDistance);
581 shot[shapeToColorStride + (desc_index + 1) * (nr_bins_color+1) + step_index_color] -=
static_cast<float> (inclinationDistance);
586 double inclinationDistance = (inclination - PST_RAD_45) / PST_RAD_90;
587 if (inclination < PST_RAD_45)
589 intWeightShape += 1 + inclinationDistance;
590 intWeightColor += 1 + inclinationDistance;
594 intWeightShape += 1 - inclinationDistance;
595 intWeightColor += 1 - inclinationDistance;
596 assert ((desc_index - 1) * (nr_bins_shape+1) + step_index_shape >= 0 && (desc_index - 1) * (nr_bins_shape+1) + step_index_shape < descLength_);
597 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_);
598 shot[(desc_index - 1) * (nr_bins_shape+1) + step_index_shape] +=
static_cast<float> (inclinationDistance);
599 shot[shapeToColorStride + (desc_index - 1) * (nr_bins_color+1) + step_index_color] +=
static_cast<float> (inclinationDistance);
603 if (yInFeatRef != 0.0 || xInFeatRef != 0.0)
606 double azimuth = std::atan2 (yInFeatRef, xInFeatRef);
608 int sel = desc_index >> 2;
609 double angularSectorSpan = PST_RAD_45;
610 double angularSectorStart = - PST_RAD_PI_7_8;
612 double azimuthDistance = (azimuth - (angularSectorStart + angularSectorSpan*sel)) / angularSectorSpan;
613 assert ((azimuthDistance < 0.5 || areEquals (azimuthDistance, 0.5)) && (azimuthDistance > - 0.5 || areEquals (azimuthDistance, - 0.5)));
614 azimuthDistance = (std::max)(- 0.5, std::min (azimuthDistance, 0.5));
616 if (azimuthDistance > 0)
618 intWeightShape += 1 - azimuthDistance;
619 intWeightColor += 1 - azimuthDistance;
620 int interp_index = (desc_index + 4) % maxAngularSectors_;
621 assert (interp_index * (nr_bins_shape+1) + step_index_shape >= 0 && interp_index * (nr_bins_shape+1) + step_index_shape < descLength_);
622 assert (shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color >= 0 && shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color < descLength_);
623 shot[interp_index * (nr_bins_shape+1) + step_index_shape] += static_cast<float> (azimuthDistance);
624 shot[shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color] += static_cast<float> (azimuthDistance);
628 int interp_index = (desc_index - 4 + maxAngularSectors_) % maxAngularSectors_;
629 intWeightShape += 1 + azimuthDistance;
630 intWeightColor += 1 + azimuthDistance;
631 assert (interp_index * (nr_bins_shape+1) + step_index_shape >= 0 && interp_index * (nr_bins_shape+1) + step_index_shape < descLength_);
632 assert (shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color >= 0 && shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color < descLength_);
633 shot[interp_index * (nr_bins_shape+1) + step_index_shape] -= static_cast<float> (azimuthDistance);
634 shot[shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color] -= static_cast<float> (azimuthDistance);
638 assert (volume_index_shape + step_index_shape >= 0 && volume_index_shape + step_index_shape < descLength_);
639 assert (volume_index_color + step_index_color >= 0 && volume_index_color + step_index_color < descLength_);
640 shot[volume_index_shape + step_index_shape] +=
static_cast<float> (intWeightShape);
641 shot[volume_index_color + step_index_color] +=
static_cast<float> (intWeightColor);
646 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
void
648 const int index,
const std::vector<int> &indices,
const std::vector<float> &sqr_dists, Eigen::VectorXf &shot)
652 const auto nNeighbors = indices.size ();
656 PCL_WARN (
"[pcl::%s::computePointSHOT] Warning! Neighborhood has less than 5 vertexes. Aborting description of point with index %d\n",
657 getClassName ().c_str (), (*indices_)[index]);
659 shot.setConstant(descLength_, 1, std::numeric_limits<float>::quiet_NaN () );
665 std::vector<double> binDistanceShape;
666 if (b_describe_shape_)
668 this->createBinDistanceShape (index, indices, binDistanceShape);
672 std::vector<double> binDistanceColor;
673 if (b_describe_color_)
675 binDistanceColor.reserve (nNeighbors);
680 unsigned char redRef = input_->points[(*indices_)[index]].r;
681 unsigned char greenRef = input_->points[(*indices_)[index]].g;
682 unsigned char blueRef = input_->points[(*indices_)[index]].b;
684 float LRef, aRef, bRef;
686 RGB2CIELAB (redRef, greenRef, blueRef, LRef, aRef, bRef);
691 for (
const auto& idx: indices)
693 unsigned char red = surface_->points[idx].r;
694 unsigned char green = surface_->points[idx].g;
695 unsigned char blue = surface_->points[idx].b;
699 RGB2CIELAB (red, green, blue, L, a, b);
704 double colorDistance = (std::fabs (LRef - L) + ((std::fabs (aRef - a) + std::fabs (bRef - b)) / 2)) /3;
706 if (colorDistance > 1.0)
708 if (colorDistance < 0.0)
711 binDistanceColor.push_back (colorDistance * nr_color_bins_);
717 if (b_describe_shape_ && b_describe_color_)
718 interpolateDoubleChannel (indices, sqr_dists, index, binDistanceShape, binDistanceColor,
719 nr_shape_bins_, nr_color_bins_,
721 else if (b_describe_color_)
722 interpolateSingleChannel (indices, sqr_dists, index, binDistanceColor, nr_color_bins_, shot);
724 interpolateSingleChannel (indices, sqr_dists, index, binDistanceShape, nr_shape_bins_, shot);
727 this->normalizeHistogram (shot, descLength_);
731 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
void
733 const int index,
const std::vector<int> &indices,
const std::vector<float> &sqr_dists, Eigen::VectorXf &shot)
736 if (indices.size () < 5)
738 PCL_WARN (
"[pcl::%s::computePointSHOT] Warning! Neighborhood has less than 5 vertexes. Aborting description of point with index %d\n",
739 getClassName ().c_str (), (*indices_)[index]);
741 shot.setConstant(descLength_, 1, std::numeric_limits<float>::quiet_NaN () );
747 std::vector<double> binDistanceShape;
748 this->createBinDistanceShape (index, indices, binDistanceShape);
752 interpolateSingleChannel (indices, sqr_dists, index, binDistanceShape, nr_shape_bins_, shot);
755 this->normalizeHistogram (shot, descLength_);
761 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
void
764 descLength_ = nr_grid_sector_ * (nr_shape_bins_+1);
766 sqradius_ = search_radius_ * search_radius_;
767 radius3_4_ = (search_radius_*3) / 4;
768 radius1_4_ = search_radius_ / 4;
769 radius1_2_ = search_radius_ / 2;
771 assert(descLength_ == 352);
773 shot_.setZero (descLength_);
777 std::vector<int> nn_indices (k_);
778 std::vector<float> nn_dists (k_);
782 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
784 bool lrf_is_nan =
false;
785 const PointRFT& current_frame = (*frames_)[idx];
786 if (!std::isfinite (current_frame.x_axis[0]) ||
787 !std::isfinite (current_frame.y_axis[0]) ||
788 !std::isfinite (current_frame.z_axis[0]))
790 PCL_WARN (
"[pcl::%s::computeFeature] The local reference frame is not valid! Aborting description of point with index %d\n",
791 getClassName ().c_str (), (*indices_)[idx]);
795 if (!
isFinite ((*input_)[(*indices_)[idx]]) ||
797 this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
800 for (
int d = 0; d < descLength_; ++d)
801 output.
points[idx].descriptor[d] = std::numeric_limits<float>::quiet_NaN ();
802 for (
int d = 0; d < 9; ++d)
803 output.
points[idx].rf[d] = std::numeric_limits<float>::quiet_NaN ();
810 computePointSHOT (static_cast<int> (idx), nn_indices, nn_dists, shot_);
813 for (
int d = 0; d < descLength_; ++d)
814 output.
points[idx].descriptor[d] = shot_[d];
815 for (
int d = 0; d < 3; ++d)
817 output.
points[idx].rf[d + 0] = frames_->points[idx].x_axis[d];
818 output.
points[idx].rf[d + 3] = frames_->points[idx].y_axis[d];
819 output.
points[idx].rf[d + 6] = frames_->points[idx].z_axis[d];
827 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
void
831 descLength_ = (b_describe_shape_) ? nr_grid_sector_*(nr_shape_bins_+1) : 0;
832 descLength_ += (b_describe_color_) ? nr_grid_sector_*(nr_color_bins_+1) : 0;
834 assert( (!b_describe_color_ && b_describe_shape_ && descLength_ == 352) ||
835 (b_describe_color_ && !b_describe_shape_ && descLength_ == 992) ||
836 (b_describe_color_ && b_describe_shape_ && descLength_ == 1344)
840 sqradius_ = search_radius_*search_radius_;
841 radius3_4_ = (search_radius_*3) / 4;
842 radius1_4_ = search_radius_ / 4;
843 radius1_2_ = search_radius_ / 2;
845 shot_.setZero (descLength_);
849 std::vector<int> nn_indices (k_);
850 std::vector<float> nn_dists (k_);
854 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
856 bool lrf_is_nan =
false;
857 const PointRFT& current_frame = (*frames_)[idx];
858 if (!std::isfinite (current_frame.x_axis[0]) ||
859 !std::isfinite (current_frame.y_axis[0]) ||
860 !std::isfinite (current_frame.z_axis[0]))
862 PCL_WARN (
"[pcl::%s::computeFeature] The local reference frame is not valid! Aborting description of point with index %d\n",
863 getClassName ().c_str (), (*indices_)[idx]);
867 if (!
isFinite ((*input_)[(*indices_)[idx]]) ||
869 this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
872 for (
int d = 0; d < descLength_; ++d)
873 output.
points[idx].descriptor[d] = std::numeric_limits<float>::quiet_NaN ();
874 for (
int d = 0; d < 9; ++d)
875 output.
points[idx].rf[d] = std::numeric_limits<float>::quiet_NaN ();
882 computePointSHOT (static_cast<int> (idx), nn_indices, nn_dists, shot_);
885 for (
int d = 0; d < descLength_; ++d)
886 output.
points[idx].descriptor[d] = shot_[d];
887 for (
int d = 0; d < 3; ++d)
889 output.
points[idx].rf[d + 0] = frames_->points[idx].x_axis[d];
890 output.
points[idx].rf[d + 3] = frames_->points[idx].y_axis[d];
891 output.
points[idx].rf[d + 6] = frames_->points[idx].z_axis[d];
896 #define PCL_INSTANTIATE_SHOTEstimationBase(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTEstimationBase<T,NT,OutT,RFT>;
897 #define PCL_INSTANTIATE_SHOTEstimation(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTEstimation<T,NT,OutT,RFT>;
898 #define PCL_INSTANTIATE_SHOTColorEstimation(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTColorEstimation<T,NT,OutT,RFT>;
900 #endif // PCL_FEATURES_IMPL_SHOT_H_
void setSearchSurface(const PointCloudInConstPtr &cloud)
Provide a pointer to a dataset to add additional information to estimate the features for every point...
void computePointSHOT(const int index, const std::vector< int > &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...
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...
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
SHOTColorEstimation estimates the Signature of Histograms of OrienTations (SHOT) descriptor for a giv...
bool initCompute() override
This method should get called before starting the actual computation.
void interpolateSingleChannel(const std::vector< int > &indices, const std::vector< float > &sqr_dists, const int index, std::vector< double > &binDistance, const int nr_bins, Eigen::VectorXf &shot)
Quadrilinear interpolation used when color and shape descriptions are NOT activated simultaneously...
static void RGB2CIELAB(unsigned char R, unsigned char G, unsigned char B, float &L, float &A, float &B2)
Converts RGB triplets to CIELab space.
void setRadiusSearch(double radius)
Set the sphere radius that is to be used for determining the nearest neighbors used for the feature e...
shared_ptr< SHOTLocalReferenceFrameEstimation< PointInT, PointOutT > > Ptr
void computeFeature(pcl::PointCloud< PointOutT > &output) override
Estimate the Signatures of Histograms of OrienTations (SHOT) descriptors at a set of points given by ...
void createBinDistanceShape(int index, const std::vector< int > &indices, std::vector< double > &bin_distance_shape)
Create a binned distance shape histogram.
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
void normalizeHistogram(Eigen::VectorXf &shot, int desc_length)
Normalize the SHOT histogram.
void computeFeature(pcl::PointCloud< PointOutT > &output) override
Estimate the Signatures of Histograms of OrienTations (SHOT) descriptors at a set of points given by ...
SHOTLocalReferenceFrameEstimation estimates the Local Reference Frame used in the calculation of the ...
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...
FeatureWithLocalReferenceFrames provides a public interface for descriptor extractor classes which ne...
void computePointSHOT(const int index, const std::vector< int > &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...
void interpolateDoubleChannel(const std::vector< int > &indices, const std::vector< float > &sqr_dists, const int index, std::vector< double > &binDistanceShape, std::vector< double > &binDistanceColor, const int nr_bins_shape, const int nr_bins_color, Eigen::VectorXf &shot)
Quadrilinear interpolation; used when color and shape descriptions are both activated.