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 (fabs (val1 - val2)<zeroDoubleEps);
82 areEquals (
float val1,
float val2,
float zeroFloatEps = zeroFloatEps8)
84 return (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,
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
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_->points[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 (
size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
211 const Eigen::Vector4f& normal_vec = normals_->points[indices[i_idx]].getNormalVector4fMap ();
212 if (!pcl_isfinite (normal_vec[0]) ||
213 !pcl_isfinite (normal_vec[1]) ||
214 !pcl_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 (
size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
272 if (!pcl_isfinite(binDistance[i_idx]))
275 Eigen::Vector4f delta = surface_->points[indices[i_idx]].getVector4fMap () - central_point;
279 double distance = sqrt (sqr_dists[i_idx]);
281 if (areEquals (distance, 0.0))
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 (fabs (yInFeatRef) < 1E-30)
291 if (fabs (xInFeatRef) < 1E-30)
293 if (fabs (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 += (fabs (xInFeatRef) >= fabs (yInFeatRef)) ? 0 : 4;
309 desc_index += (fabs (xInFeatRef) > fabs (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>(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- fabs (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]);
330 if (distance > radius1_2_)
332 double radiusDistance = (distance - radius3_4_) / radius1_2_;
334 if (distance > radius3_4_)
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_;
346 if (distance < radius1_4_)
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 = acos (inclinationCos);
364 assert (inclination >= 0.0 && inclination <= PST_RAD_180);
366 if (inclination > PST_RAD_90 || (fabs (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 = 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 (
size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
451 if (!pcl_isfinite(binDistanceShape[i_idx]))
454 Eigen::Vector4f delta = surface_->points[indices[i_idx]].getVector4fMap () - central_point;
458 double distance = sqrt (sqr_dists[i_idx]);
460 if (areEquals (distance, 0.0))
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 (fabs (yInFeatRef) < 1E-30)
470 if (fabs (xInFeatRef) < 1E-30)
472 if (fabs (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 += (fabs (xInFeatRef) >= fabs (yInFeatRef)) ? 0 : 4;
487 desc_index += (fabs (xInFeatRef) > fabs (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>(floor (binDistanceShape[i_idx] +0.5));
495 int step_index_color =
static_cast<int>(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- fabs (binDistanceShape[i_idx]));
505 double intWeightColor = (1- fabs (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]);
519 if (distance > radius1_2_)
521 double radiusDistance = (distance - radius3_4_) / radius1_2_;
523 if (distance > radius3_4_)
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_;
540 if (distance < radius1_4_)
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 = acos (inclinationCos);
563 assert (inclination >= 0.0 && inclination <= PST_RAD_180);
565 if (inclination > PST_RAD_90 || (fabs (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 = 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 std::vector<double> binDistanceShape;
652 std::vector<double> binDistanceColor;
653 size_t nNeighbors = indices.size ();
657 PCL_WARN (
"[pcl::%s::computePointSHOT] Warning! Neighborhood has less than 5 vertexes. Aborting description of point with index %d\n",
658 getClassName ().c_str (), (*indices_)[index]);
660 shot.setConstant(descLength_, 1, std::numeric_limits<float>::quiet_NaN () );
666 if (b_describe_shape_)
668 this->createBinDistanceShape (index, indices, binDistanceShape);
672 if (b_describe_color_)
674 binDistanceColor.resize (nNeighbors);
679 unsigned char redRef = input_->points[(*indices_)[index]].r;
680 unsigned char greenRef = input_->points[(*indices_)[index]].g;
681 unsigned char blueRef = input_->points[(*indices_)[index]].b;
683 float LRef, aRef, bRef;
685 RGB2CIELAB (redRef, greenRef, blueRef, LRef, aRef, bRef);
690 for (
size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
695 unsigned char red = surface_->points[indices[i_idx]].r;
696 unsigned char green = surface_->points[indices[i_idx]].g;
697 unsigned char blue = surface_->points[indices[i_idx]].b;
701 RGB2CIELAB (red, green, blue, L, a, b);
706 double colorDistance = (fabs (LRef - L) + ((fabs (aRef - a) + fabs (bRef - b)) / 2)) /3;
708 if (colorDistance > 1.0)
710 if (colorDistance < 0.0)
713 binDistanceColor[i_idx] = colorDistance * nr_color_bins_;
719 if (b_describe_shape_ && b_describe_color_)
720 interpolateDoubleChannel (indices, sqr_dists, index, binDistanceShape, binDistanceColor,
721 nr_shape_bins_, nr_color_bins_,
723 else if (b_describe_color_)
724 interpolateSingleChannel (indices, sqr_dists, index, binDistanceColor, nr_color_bins_, shot);
726 interpolateSingleChannel (indices, sqr_dists, index, binDistanceShape, nr_shape_bins_, shot);
729 this->normalizeHistogram (shot, descLength_);
733 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
void
735 const int index,
const std::vector<int> &indices,
const std::vector<float> &sqr_dists, Eigen::VectorXf &shot)
738 if (indices.size () < 5)
740 PCL_WARN (
"[pcl::%s::computePointSHOT] Warning! Neighborhood has less than 5 vertexes. Aborting description of point with index %d\n",
741 getClassName ().c_str (), (*indices_)[index]);
743 shot.setConstant(descLength_, 1, std::numeric_limits<float>::quiet_NaN () );
749 std::vector<double> binDistanceShape;
750 this->createBinDistanceShape (index, indices, binDistanceShape);
754 interpolateSingleChannel (indices, sqr_dists, index, binDistanceShape, nr_shape_bins_, shot);
757 this->normalizeHistogram (shot, descLength_);
763 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
void
766 descLength_ = nr_grid_sector_ * (nr_shape_bins_+1);
768 sqradius_ = search_radius_ * search_radius_;
769 radius3_4_ = (search_radius_*3) / 4;
770 radius1_4_ = search_radius_ / 4;
771 radius1_2_ = search_radius_ / 2;
773 assert(descLength_ == 352);
775 shot_.setZero (descLength_);
779 std::vector<int> nn_indices (k_);
780 std::vector<float> nn_dists (k_);
784 for (
size_t idx = 0; idx < indices_->size (); ++idx)
786 bool lrf_is_nan =
false;
787 const PointRFT& current_frame = (*frames_)[idx];
788 if (!pcl_isfinite (current_frame.x_axis[0]) ||
789 !pcl_isfinite (current_frame.y_axis[0]) ||
790 !pcl_isfinite (current_frame.z_axis[0]))
792 PCL_WARN (
"[pcl::%s::computeFeature] The local reference frame is not valid! Aborting description of point with index %d\n",
793 getClassName ().c_str (), (*indices_)[idx]);
797 if (!
isFinite ((*input_)[(*indices_)[idx]]) ||
799 this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
802 for (
int d = 0; d < descLength_; ++d)
803 output.
points[idx].descriptor[d] = std::numeric_limits<float>::quiet_NaN ();
804 for (
int d = 0; d < 9; ++d)
805 output.
points[idx].rf[d] = std::numeric_limits<float>::quiet_NaN ();
812 computePointSHOT (static_cast<int> (idx), nn_indices, nn_dists, shot_);
815 for (
int d = 0; d < descLength_; ++d)
816 output.
points[idx].descriptor[d] = shot_[d];
817 for (
int d = 0; d < 3; ++d)
819 output.
points[idx].rf[d + 0] = frames_->points[idx].x_axis[d];
820 output.
points[idx].rf[d + 3] = frames_->points[idx].y_axis[d];
821 output.
points[idx].rf[d + 6] = frames_->points[idx].z_axis[d];
829 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
void
833 descLength_ = (b_describe_shape_) ? nr_grid_sector_*(nr_shape_bins_+1) : 0;
834 descLength_ += (b_describe_color_) ? nr_grid_sector_*(nr_color_bins_+1) : 0;
836 assert( (!b_describe_color_ && b_describe_shape_ && descLength_ == 352) ||
837 (b_describe_color_ && !b_describe_shape_ && descLength_ == 992) ||
838 (b_describe_color_ && b_describe_shape_ && descLength_ == 1344)
842 sqradius_ = search_radius_*search_radius_;
843 radius3_4_ = (search_radius_*3) / 4;
844 radius1_4_ = search_radius_ / 4;
845 radius1_2_ = search_radius_ / 2;
847 shot_.setZero (descLength_);
851 std::vector<int> nn_indices (k_);
852 std::vector<float> nn_dists (k_);
856 for (
size_t idx = 0; idx < indices_->size (); ++idx)
858 bool lrf_is_nan =
false;
859 const PointRFT& current_frame = (*frames_)[idx];
860 if (!pcl_isfinite (current_frame.x_axis[0]) ||
861 !pcl_isfinite (current_frame.y_axis[0]) ||
862 !pcl_isfinite (current_frame.z_axis[0]))
864 PCL_WARN (
"[pcl::%s::computeFeature] The local reference frame is not valid! Aborting description of point with index %d\n",
865 getClassName ().c_str (), (*indices_)[idx]);
869 if (!
isFinite ((*input_)[(*indices_)[idx]]) ||
871 this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
874 for (
int d = 0; d < descLength_; ++d)
875 output.
points[idx].descriptor[d] = std::numeric_limits<float>::quiet_NaN ();
876 for (
int d = 0; d < 9; ++d)
877 output.
points[idx].rf[d] = std::numeric_limits<float>::quiet_NaN ();
884 computePointSHOT (static_cast<int> (idx), nn_indices, nn_dists, shot_);
887 for (
int d = 0; d < descLength_; ++d)
888 output.
points[idx].descriptor[d] = shot_[d];
889 for (
int d = 0; d < 3; ++d)
891 output.
points[idx].rf[d + 0] = frames_->points[idx].x_axis[d];
892 output.
points[idx].rf[d + 3] = frames_->points[idx].y_axis[d];
893 output.
points[idx].rf[d + 6] = frames_->points[idx].z_axis[d];
898 #define PCL_INSTANTIATE_SHOTEstimation(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTEstimation<T,NT,OutT,RFT>;
899 #define PCL_INSTANTIATE_SHOTColorEstimation(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTColorEstimation<T,NT,OutT,RFT>;
901 #endif // PCL_FEATURES_IMPL_SHOT_H_