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>
47 #define PST_PI 3.1415926535897932384626433832795
48 #define PST_RAD_45 0.78539816339744830961566084581988
49 #define PST_RAD_90 1.5707963267948966192313216916398
50 #define PST_RAD_135 2.3561944901923449288469825374596
51 #define PST_RAD_180 PST_PI
52 #define PST_RAD_360 6.283185307179586476925286766558
53 #define PST_RAD_PI_7_8 2.7488935718910690836548129603691
55 const double zeroDoubleEps15 = 1E-15;
56 const float zeroFloatEps8 = 1E-8f;
67 areEquals (
double val1,
double val2,
double zeroDoubleEps = zeroDoubleEps15)
69 return (std::abs (val1 - val2)<zeroDoubleEps);
81 areEquals (
float val1,
float val2,
float zeroFloatEps = zeroFloatEps8)
83 return (std::fabs (val1 - val2)<zeroFloatEps);
87 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
float
91 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
float
95 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
void
97 unsigned char B,
float &L,
float &A,
103 for (
int i = 0; i < 256; i++)
105 float f =
static_cast<float> (i) / 255.0f;
107 sRGB_LUT[i] = powf ((f + 0.055f) / 1.055f, 2.4f);
109 sRGB_LUT[i] = f / 12.92f;
112 for (
int i = 0; i < 4000; i++)
114 float f =
static_cast<float> (i) / 4000.0f;
116 sXYZ_LUT[i] =
static_cast<float> (powf (f, 0.3333f));
118 sXYZ_LUT[i] =
static_cast<float>((7.787 * f) + (16.0 / 116.0));
122 float fr = sRGB_LUT[R];
123 float fg = sRGB_LUT[G];
124 float fb = sRGB_LUT[
B];
127 const float x = fr * 0.412453f + fg * 0.357580f + fb * 0.180423f;
128 const float y = fr * 0.212671f + fg * 0.715160f + fb * 0.072169f;
129 const float z = fr * 0.019334f + fg * 0.119193f + fb * 0.950227f;
131 float vx = x / 0.95047f;
133 float vz = z / 1.08883f;
135 vx = sXYZ_LUT[int(vx*4000)];
136 vy = sXYZ_LUT[int(vy*4000)];
137 vz = sXYZ_LUT[int(vz*4000)];
139 L = 116.0f * vy - 16.0f;
143 A = 500.0f * (vx - vy);
149 B2 = 200.0f * (vy - vz);
157 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
bool
162 PCL_ERROR (
"[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
167 if (this->getKSearch () != 0)
170 "[pcl::%s::initCompute] Error! Search method set to k-neighborhood. Call setKSearch(0) and setRadiusSearch( radius ) to use this class.\n",
171 getClassName().c_str ());
177 lrf_estimator->
setRadiusSearch ((lrf_radius_ > 0 ? lrf_radius_ : search_radius_));
185 PCL_ERROR (
"[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
193 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
void
197 std::vector<double> &bin_distance_shape)
199 bin_distance_shape.resize (indices.size ());
201 const PointRFT& current_frame = (*frames_)[index];
205 Eigen::Vector4f current_frame_z (current_frame.z_axis[0], current_frame.z_axis[1], current_frame.z_axis[2], 0);
207 unsigned nan_counter = 0;
208 for (std::size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
211 const Eigen::Vector4f& normal_vec = (*normals_)[indices[i_idx]].getNormalVector4fMap ();
212 if (!std::isfinite (normal_vec[0]) ||
213 !std::isfinite (normal_vec[1]) ||
214 !std::isfinite (normal_vec[2]))
216 bin_distance_shape[i_idx] = std::numeric_limits<double>::quiet_NaN ();
221 double cosineDesc = normal_vec.dot (current_frame_z);
223 if (cosineDesc > 1.0)
225 if (cosineDesc < - 1.0)
228 bin_distance_shape[i_idx] = ((1.0 + cosineDesc) * nr_shape_bins_) / 2;
232 PCL_WARN (
"[pcl::%s::createBinDistanceShape] Point %d has %d (%f%%) NaN normals in its neighbourhood\n",
233 getClassName ().c_str (), index, nan_counter, (
static_cast<float>(nan_counter)*100.f/
static_cast<float>(indices.size ())));
237 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
void
239 Eigen::VectorXf &shot,
int desc_length)
246 for (
int j = 0; j < desc_length; j++)
247 acc_norm += shot[j] * shot[j];
248 acc_norm = sqrt (acc_norm);
249 for (
int j = 0; j < desc_length; j++)
250 shot[j] /=
static_cast<float> (acc_norm);
254 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
void
257 const std::vector<float> &sqr_dists,
259 std::vector<double> &binDistance,
261 Eigen::VectorXf &shot)
263 const Eigen::Vector4f& central_point = (*input_)[(*indices_)[index]].getVector4fMap ();
264 const PointRFT& current_frame = (*frames_)[index];
266 Eigen::Vector4f current_frame_x (current_frame.x_axis[0], current_frame.x_axis[1], current_frame.x_axis[2], 0);
267 Eigen::Vector4f current_frame_y (current_frame.y_axis[0], current_frame.y_axis[1], current_frame.y_axis[2], 0);
268 Eigen::Vector4f current_frame_z (current_frame.z_axis[0], current_frame.z_axis[1], current_frame.z_axis[2], 0);
270 for (std::size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
272 if (!std::isfinite(binDistance[i_idx]))
275 Eigen::Vector4f delta = (*surface_)[indices[i_idx]].getVector4fMap () - central_point;
279 double distance = sqrt (sqr_dists[i_idx]);
284 double xInFeatRef = delta.dot (current_frame_x);
285 double yInFeatRef = delta.dot (current_frame_y);
286 double zInFeatRef = delta.dot (current_frame_z);
289 if (std::abs (yInFeatRef) < 1E-30)
291 if (std::abs (xInFeatRef) < 1E-30)
293 if (std::abs (zInFeatRef) < 1E-30)
297 unsigned char bit4 = ((yInFeatRef > 0) || ((yInFeatRef == 0.0) && (xInFeatRef < 0))) ? 1 : 0;
298 unsigned char bit3 =
static_cast<unsigned char> (((xInFeatRef > 0) || ((xInFeatRef == 0.0) && (yInFeatRef > 0))) ? !bit4 : bit4);
300 assert (bit3 == 0 || bit3 == 1);
302 int desc_index = (bit4<<3) + (bit3<<2);
304 desc_index = desc_index << 1;
306 if ((xInFeatRef * yInFeatRef > 0) || (xInFeatRef == 0.0))
307 desc_index += (std::abs (xInFeatRef) >= std::abs (yInFeatRef)) ? 0 : 4;
309 desc_index += (std::abs (xInFeatRef) > std::abs (yInFeatRef)) ? 4 : 0;
311 desc_index += zInFeatRef > 0 ? 1 : 0;
314 desc_index += (
distance > radius1_2_) ? 2 : 0;
316 int step_index =
static_cast<int>(std::floor (binDistance[i_idx] +0.5));
317 int volume_index = desc_index * (nr_bins+1);
320 binDistance[i_idx] -= step_index;
321 double intWeight = (1- std::abs (binDistance[i_idx]));
323 if (binDistance[i_idx] > 0)
324 shot[volume_index + ((step_index+1) % nr_bins)] +=
static_cast<float> (binDistance[i_idx]);
326 shot[volume_index + ((step_index - 1 + nr_bins) % nr_bins)] += -
static_cast<float> (binDistance[i_idx]);
332 double radiusDistance = (
distance - radius3_4_) / radius1_2_;
335 intWeight += 1 - radiusDistance;
338 intWeight += 1 + radiusDistance;
339 shot[(desc_index - 2) * (nr_bins+1) + step_index] -=
static_cast<float> (radiusDistance);
344 double radiusDistance = (
distance - radius1_4_) / radius1_2_;
347 intWeight += 1 + radiusDistance;
350 intWeight += 1 - radiusDistance;
351 shot[(desc_index + 2) * (nr_bins+1) + step_index] +=
static_cast<float> (radiusDistance);
356 double inclinationCos = zInFeatRef /
distance;
357 if (inclinationCos < - 1.0)
358 inclinationCos = - 1.0;
359 if (inclinationCos > 1.0)
360 inclinationCos = 1.0;
362 double inclination = std::acos (inclinationCos);
364 assert (inclination >= 0.0 && inclination <= PST_RAD_180);
366 if (inclination > PST_RAD_90 || (std::abs (inclination - PST_RAD_90) < 1e-30 && zInFeatRef <= 0))
368 double inclinationDistance = (inclination - PST_RAD_135) / PST_RAD_90;
369 if (inclination > PST_RAD_135)
370 intWeight += 1 - inclinationDistance;
373 intWeight += 1 + inclinationDistance;
374 assert ((desc_index + 1) * (nr_bins+1) + step_index >= 0 && (desc_index + 1) * (nr_bins+1) + step_index < descLength_);
375 shot[(desc_index + 1) * (nr_bins+1) + step_index] -=
static_cast<float> (inclinationDistance);
380 double inclinationDistance = (inclination - PST_RAD_45) / PST_RAD_90;
381 if (inclination < PST_RAD_45)
382 intWeight += 1 + inclinationDistance;
385 intWeight += 1 - inclinationDistance;
386 assert ((desc_index - 1) * (nr_bins+1) + step_index >= 0 && (desc_index - 1) * (nr_bins+1) + step_index < descLength_);
387 shot[(desc_index - 1) * (nr_bins+1) + step_index] +=
static_cast<float> (inclinationDistance);
391 if (yInFeatRef != 0.0 || xInFeatRef != 0.0)
394 double azimuth = std::atan2 (yInFeatRef, xInFeatRef);
396 int sel = desc_index >> 2;
397 double angularSectorSpan = PST_RAD_45;
398 double angularSectorStart = - PST_RAD_PI_7_8;
400 double azimuthDistance = (azimuth - (angularSectorStart + angularSectorSpan*sel)) / angularSectorSpan;
402 assert ((azimuthDistance < 0.5 || areEquals (azimuthDistance, 0.5)) && (azimuthDistance > - 0.5 || areEquals (azimuthDistance, - 0.5)));
404 azimuthDistance = (std::max)(- 0.5, std::min (azimuthDistance, 0.5));
406 if (azimuthDistance > 0)
408 intWeight += 1 - azimuthDistance;
409 int interp_index = (desc_index + 4) % maxAngularSectors_;
410 assert (interp_index * (nr_bins+1) + step_index >= 0 && interp_index * (nr_bins+1) + step_index < descLength_);
411 shot[interp_index * (nr_bins+1) + step_index] +=
static_cast<float> (azimuthDistance);
415 int interp_index = (desc_index - 4 + maxAngularSectors_) % maxAngularSectors_;
416 assert (interp_index * (nr_bins+1) + step_index >= 0 && interp_index * (nr_bins+1) + step_index < descLength_);
417 intWeight += 1 + azimuthDistance;
418 shot[interp_index * (nr_bins+1) + step_index] -=
static_cast<float> (azimuthDistance);
423 assert (volume_index + step_index >= 0 && volume_index + step_index < descLength_);
424 shot[volume_index + step_index] +=
static_cast<float> (intWeight);
429 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
void
432 const std::vector<float> &sqr_dists,
434 std::vector<double> &binDistanceShape,
435 std::vector<double> &binDistanceColor,
436 const int nr_bins_shape,
437 const int nr_bins_color,
438 Eigen::VectorXf &shot)
440 const Eigen::Vector4f ¢ral_point = (*input_)[(*indices_)[index]].getVector4fMap ();
441 const PointRFT& current_frame = (*frames_)[index];
443 int shapeToColorStride = nr_grid_sector_*(nr_bins_shape+1);
445 Eigen::Vector4f current_frame_x (current_frame.x_axis[0], current_frame.x_axis[1], current_frame.x_axis[2], 0);
446 Eigen::Vector4f current_frame_y (current_frame.y_axis[0], current_frame.y_axis[1], current_frame.y_axis[2], 0);
447 Eigen::Vector4f current_frame_z (current_frame.z_axis[0], current_frame.z_axis[1], current_frame.z_axis[2], 0);
449 for (std::size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
451 if (!std::isfinite(binDistanceShape[i_idx]))
454 Eigen::Vector4f delta = (*surface_)[indices[i_idx]].getVector4fMap () - central_point;
458 double distance = sqrt (sqr_dists[i_idx]);
463 double xInFeatRef = delta.dot (current_frame_x);
464 double yInFeatRef = delta.dot (current_frame_y);
465 double zInFeatRef = delta.dot (current_frame_z);
468 if (std::abs (yInFeatRef) < 1E-30)
470 if (std::abs (xInFeatRef) < 1E-30)
472 if (std::abs (zInFeatRef) < 1E-30)
475 unsigned char bit4 = ((yInFeatRef > 0) || ((yInFeatRef == 0.0) && (xInFeatRef < 0))) ? 1 : 0;
476 unsigned char bit3 =
static_cast<unsigned char> (((xInFeatRef > 0) || ((xInFeatRef == 0.0) && (yInFeatRef > 0))) ? !bit4 : bit4);
478 assert (bit3 == 0 || bit3 == 1);
480 int desc_index = (bit4<<3) + (bit3<<2);
482 desc_index = desc_index << 1;
484 if ((xInFeatRef * yInFeatRef > 0) || (xInFeatRef == 0.0))
485 desc_index += (std::abs (xInFeatRef) >= std::abs (yInFeatRef)) ? 0 : 4;
487 desc_index += (std::abs (xInFeatRef) > std::abs (yInFeatRef)) ? 4 : 0;
489 desc_index += zInFeatRef > 0 ? 1 : 0;
492 desc_index += (
distance > radius1_2_) ? 2 : 0;
494 int step_index_shape =
static_cast<int>(std::floor (binDistanceShape[i_idx] +0.5));
495 int step_index_color =
static_cast<int>(std::floor (binDistanceColor[i_idx] +0.5));
497 int volume_index_shape = desc_index * (nr_bins_shape+1);
498 int volume_index_color = shapeToColorStride + desc_index * (nr_bins_color+1);
501 binDistanceShape[i_idx] -= step_index_shape;
502 binDistanceColor[i_idx] -= step_index_color;
504 double intWeightShape = (1- std::abs (binDistanceShape[i_idx]));
505 double intWeightColor = (1- std::abs (binDistanceColor[i_idx]));
507 if (binDistanceShape[i_idx] > 0)
508 shot[volume_index_shape + ((step_index_shape + 1) % nr_bins_shape)] +=
static_cast<float> (binDistanceShape[i_idx]);
510 shot[volume_index_shape + ((step_index_shape - 1 + nr_bins_shape) % nr_bins_shape)] -=
static_cast<float> (binDistanceShape[i_idx]);
512 if (binDistanceColor[i_idx] > 0)
513 shot[volume_index_color + ((step_index_color+1) % nr_bins_color)] +=
static_cast<float> (binDistanceColor[i_idx]);
515 shot[volume_index_color + ((step_index_color - 1 + nr_bins_color) % nr_bins_color)] -=
static_cast<float> (binDistanceColor[i_idx]);
521 double radiusDistance = (
distance - radius3_4_) / radius1_2_;
525 intWeightShape += 1 - radiusDistance;
526 intWeightColor += 1 - radiusDistance;
530 intWeightShape += 1 + radiusDistance;
531 intWeightColor += 1 + radiusDistance;
532 shot[(desc_index - 2) * (nr_bins_shape+1) + step_index_shape] -=
static_cast<float> (radiusDistance);
533 shot[shapeToColorStride + (desc_index - 2) * (nr_bins_color+1) + step_index_color] -=
static_cast<float> (radiusDistance);
538 double radiusDistance = (
distance - radius1_4_) / radius1_2_;
542 intWeightShape += 1 + radiusDistance;
543 intWeightColor += 1 + radiusDistance;
547 intWeightShape += 1 - radiusDistance;
548 intWeightColor += 1 - radiusDistance;
549 shot[(desc_index + 2) * (nr_bins_shape+1) + step_index_shape] +=
static_cast<float> (radiusDistance);
550 shot[shapeToColorStride + (desc_index + 2) * (nr_bins_color+1) + step_index_color] +=
static_cast<float> (radiusDistance);
555 double inclinationCos = zInFeatRef /
distance;
556 if (inclinationCos < - 1.0)
557 inclinationCos = - 1.0;
558 if (inclinationCos > 1.0)
559 inclinationCos = 1.0;
561 double inclination = std::acos (inclinationCos);
563 assert (inclination >= 0.0 && inclination <= PST_RAD_180);
565 if (inclination > PST_RAD_90 || (std::abs (inclination - PST_RAD_90) < 1e-30 && zInFeatRef <= 0))
567 double inclinationDistance = (inclination - PST_RAD_135) / PST_RAD_90;
568 if (inclination > PST_RAD_135)
570 intWeightShape += 1 - inclinationDistance;
571 intWeightColor += 1 - inclinationDistance;
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);
585 double inclinationDistance = (inclination - PST_RAD_45) / PST_RAD_90;
586 if (inclination < PST_RAD_45)
588 intWeightShape += 1 + inclinationDistance;
589 intWeightColor += 1 + inclinationDistance;
593 intWeightShape += 1 - inclinationDistance;
594 intWeightColor += 1 - inclinationDistance;
595 assert ((desc_index - 1) * (nr_bins_shape+1) + step_index_shape >= 0 && (desc_index - 1) * (nr_bins_shape+1) + step_index_shape < descLength_);
596 assert (shapeToColorStride + (desc_index - 1) * (nr_bins_color+ 1) + step_index_color >= 0 && shapeToColorStride + (desc_index - 1) * (nr_bins_color+1) + step_index_color < descLength_);
597 shot[(desc_index - 1) * (nr_bins_shape+1) + step_index_shape] +=
static_cast<float> (inclinationDistance);
598 shot[shapeToColorStride + (desc_index - 1) * (nr_bins_color+1) + step_index_color] +=
static_cast<float> (inclinationDistance);
602 if (yInFeatRef != 0.0 || xInFeatRef != 0.0)
605 double azimuth = std::atan2 (yInFeatRef, xInFeatRef);
607 int sel = desc_index >> 2;
608 double angularSectorSpan = PST_RAD_45;
609 double angularSectorStart = - PST_RAD_PI_7_8;
611 double azimuthDistance = (azimuth - (angularSectorStart + angularSectorSpan*sel)) / angularSectorSpan;
612 assert ((azimuthDistance < 0.5 || areEquals (azimuthDistance, 0.5)) && (azimuthDistance > - 0.5 || areEquals (azimuthDistance, - 0.5)));
613 azimuthDistance = (std::max)(- 0.5, std::min (azimuthDistance, 0.5));
615 if (azimuthDistance > 0)
617 intWeightShape += 1 - azimuthDistance;
618 intWeightColor += 1 - azimuthDistance;
619 int interp_index = (desc_index + 4) % maxAngularSectors_;
620 assert (interp_index * (nr_bins_shape+1) + step_index_shape >= 0 && interp_index * (nr_bins_shape+1) + step_index_shape < descLength_);
621 assert (shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color >= 0 && shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color < descLength_);
622 shot[interp_index * (nr_bins_shape+1) + step_index_shape] +=
static_cast<float> (azimuthDistance);
623 shot[shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color] +=
static_cast<float> (azimuthDistance);
627 int interp_index = (desc_index - 4 + maxAngularSectors_) % maxAngularSectors_;
628 intWeightShape += 1 + azimuthDistance;
629 intWeightColor += 1 + azimuthDistance;
630 assert (interp_index * (nr_bins_shape+1) + step_index_shape >= 0 && interp_index * (nr_bins_shape+1) + step_index_shape < descLength_);
631 assert (shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color >= 0 && shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color < descLength_);
632 shot[interp_index * (nr_bins_shape+1) + step_index_shape] -=
static_cast<float> (azimuthDistance);
633 shot[shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color] -=
static_cast<float> (azimuthDistance);
637 assert (volume_index_shape + step_index_shape >= 0 && volume_index_shape + step_index_shape < descLength_);
638 assert (volume_index_color + step_index_color >= 0 && volume_index_color + step_index_color < descLength_);
639 shot[volume_index_shape + step_index_shape] +=
static_cast<float> (intWeightShape);
640 shot[volume_index_color + step_index_color] +=
static_cast<float> (intWeightColor);
645 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
void
647 const int index,
const pcl::Indices &indices,
const std::vector<float> &sqr_dists, Eigen::VectorXf &shot)
651 const auto nNeighbors = indices.size ();
655 PCL_WARN (
"[pcl::%s::computePointSHOT] Warning! Neighborhood has less than 5 vertexes. Aborting description of point with index %d\n",
656 getClassName ().c_str (), (*indices_)[index]);
658 shot.setConstant(descLength_, 1, std::numeric_limits<float>::quiet_NaN () );
664 std::vector<double> binDistanceShape;
665 if (b_describe_shape_)
667 this->createBinDistanceShape (index, indices, binDistanceShape);
671 std::vector<double> binDistanceColor;
672 if (b_describe_color_)
674 binDistanceColor.reserve (nNeighbors);
679 unsigned char redRef = (*input_)[(*indices_)[index]].r;
680 unsigned char greenRef = (*input_)[(*indices_)[index]].g;
681 unsigned char blueRef = (*input_)[(*indices_)[index]].b;
683 float LRef, aRef, bRef;
685 RGB2CIELAB (redRef, greenRef, blueRef, LRef, aRef, bRef);
690 for (
const auto& idx: indices)
692 unsigned char red = (*surface_)[idx].r;
693 unsigned char green = (*surface_)[idx].g;
694 unsigned char blue = (*surface_)[idx].b;
698 RGB2CIELAB (red, green, blue, L, a, b);
703 double colorDistance = (std::fabs (LRef - L) + ((std::fabs (aRef - a) + std::fabs (bRef - b)) / 2)) /3;
705 if (colorDistance > 1.0)
707 if (colorDistance < 0.0)
710 binDistanceColor.push_back (colorDistance * nr_color_bins_);
716 if (b_describe_shape_ && b_describe_color_)
717 interpolateDoubleChannel (indices, sqr_dists, index, binDistanceShape, binDistanceColor,
718 nr_shape_bins_, nr_color_bins_,
720 else if (b_describe_color_)
721 interpolateSingleChannel (indices, sqr_dists, index, binDistanceColor, nr_color_bins_, shot);
723 interpolateSingleChannel (indices, sqr_dists, index, binDistanceShape, nr_shape_bins_, shot);
726 this->normalizeHistogram (shot, descLength_);
730 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
void
732 const int index,
const pcl::Indices &indices,
const std::vector<float> &sqr_dists, Eigen::VectorXf &shot)
735 if (indices.size () < 5)
737 PCL_WARN (
"[pcl::%s::computePointSHOT] Warning! Neighborhood has less than 5 vertexes. Aborting description of point with index %d\n",
738 getClassName ().c_str (), (*indices_)[index]);
740 shot.setConstant(descLength_, 1, std::numeric_limits<float>::quiet_NaN () );
746 std::vector<double> binDistanceShape;
747 this->createBinDistanceShape (index, indices, binDistanceShape);
751 interpolateSingleChannel (indices, sqr_dists, index, binDistanceShape, nr_shape_bins_, shot);
754 this->normalizeHistogram (shot, descLength_);
760 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
void
763 descLength_ = nr_grid_sector_ * (nr_shape_bins_+1);
765 sqradius_ = search_radius_ * search_radius_;
766 radius3_4_ = (search_radius_*3) / 4;
767 radius1_4_ = search_radius_ / 4;
768 radius1_2_ = search_radius_ / 2;
770 assert(descLength_ == 352);
772 shot_.setZero (descLength_);
777 std::vector<float> nn_dists (k_);
781 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
783 bool lrf_is_nan =
false;
784 const PointRFT& current_frame = (*frames_)[idx];
785 if (!std::isfinite (current_frame.x_axis[0]) ||
786 !std::isfinite (current_frame.y_axis[0]) ||
787 !std::isfinite (current_frame.z_axis[0]))
789 PCL_WARN (
"[pcl::%s::computeFeature] The local reference frame is not valid! Aborting description of point with index %d\n",
790 getClassName ().c_str (), (*indices_)[idx]);
794 if (!
isFinite ((*input_)[(*indices_)[idx]]) ||
796 this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
799 for (
int d = 0; d < descLength_; ++d)
800 output[idx].descriptor[d] = std::numeric_limits<float>::quiet_NaN ();
801 for (
int d = 0; d < 9; ++d)
802 output[idx].rf[d] = std::numeric_limits<float>::quiet_NaN ();
809 computePointSHOT (
static_cast<int> (idx), nn_indices, nn_dists, shot_);
812 for (
int d = 0; d < descLength_; ++d)
813 output[idx].descriptor[d] = shot_[d];
814 for (
int d = 0; d < 3; ++d)
816 output[idx].rf[d + 0] = (*frames_)[idx].x_axis[d];
817 output[idx].rf[d + 3] = (*frames_)[idx].y_axis[d];
818 output[idx].rf[d + 6] = (*frames_)[idx].z_axis[d];
826 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
void
830 descLength_ = (b_describe_shape_) ? nr_grid_sector_*(nr_shape_bins_+1) : 0;
831 descLength_ += (b_describe_color_) ? nr_grid_sector_*(nr_color_bins_+1) : 0;
833 assert( (!b_describe_color_ && b_describe_shape_ && descLength_ == 352) ||
834 (b_describe_color_ && !b_describe_shape_ && descLength_ == 992) ||
835 (b_describe_color_ && b_describe_shape_ && descLength_ == 1344)
839 sqradius_ = search_radius_*search_radius_;
840 radius3_4_ = (search_radius_*3) / 4;
841 radius1_4_ = search_radius_ / 4;
842 radius1_2_ = search_radius_ / 2;
844 shot_.setZero (descLength_);
849 std::vector<float> nn_dists (k_);
853 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
855 bool lrf_is_nan =
false;
856 const PointRFT& current_frame = (*frames_)[idx];
857 if (!std::isfinite (current_frame.x_axis[0]) ||
858 !std::isfinite (current_frame.y_axis[0]) ||
859 !std::isfinite (current_frame.z_axis[0]))
861 PCL_WARN (
"[pcl::%s::computeFeature] The local reference frame is not valid! Aborting description of point with index %d\n",
862 getClassName ().c_str (), (*indices_)[idx]);
866 if (!
isFinite ((*input_)[(*indices_)[idx]]) ||
868 this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
871 for (
int d = 0; d < descLength_; ++d)
872 output[idx].descriptor[d] = std::numeric_limits<float>::quiet_NaN ();
873 for (
int d = 0; d < 9; ++d)
874 output[idx].rf[d] = std::numeric_limits<float>::quiet_NaN ();
881 computePointSHOT (
static_cast<int> (idx), nn_indices, nn_dists, shot_);
884 for (
int d = 0; d < descLength_; ++d)
885 output[idx].descriptor[d] = shot_[d];
886 for (
int d = 0; d < 3; ++d)
888 output[idx].rf[d + 0] = (*frames_)[idx].x_axis[d];
889 output[idx].rf[d + 3] = (*frames_)[idx].y_axis[d];
890 output[idx].rf[d + 6] = (*frames_)[idx].z_axis[d];
895 #define PCL_INSTANTIATE_SHOTEstimationBase(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTEstimationBase<T,NT,OutT,RFT>;
896 #define PCL_INSTANTIATE_SHOTEstimation(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTEstimation<T,NT,OutT,RFT>;
897 #define PCL_INSTANTIATE_SHOTColorEstimation(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTColorEstimation<T,NT,OutT,RFT>;
899 #endif // PCL_FEATURES_IMPL_SHOT_H_