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_));
178 lrf_estimator->setInputCloud (input_);
179 lrf_estimator->setIndices (indices_);
181 lrf_estimator->setSearchSurface(surface_);
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
196 const std::vector<int> &indices,
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
256 const std::vector<int> &indices,
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
431 const std::vector<int> &indices,
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 std::vector<int> &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 std::vector<int> &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_);
776 std::vector<int> nn_indices (k_);
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_);
848 std::vector<int> nn_indices (k_);
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>;
FeatureWithLocalReferenceFrames provides a public interface for descriptor extractor classes which ne...
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...
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.
void computeFeature(pcl::PointCloud< PointOutT > &output) override
Estimate the Signatures of Histograms of OrienTations (SHOT) descriptors at a set of points given by ...
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 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 initCompute() override
This method should get called before starting the actual computation.
void normalizeHistogram(Eigen::VectorXf &shot, int desc_length)
Normalize the SHOT histogram.
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.
void createBinDistanceShape(int index, const std::vector< int > &indices, std::vector< double > &bin_distance_shape)
Create a binned distance shape histogram.
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 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 ...
shared_ptr< SHOTLocalReferenceFrameEstimation< PointInT, PointOutT > > Ptr
float distance(const PointT &p1, const PointT &p2)
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...