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_SHOTEstimationBase(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTEstimationBase<T,NT,OutT,RFT>; 899 #define PCL_INSTANTIATE_SHOTEstimation(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTEstimation<T,NT,OutT,RFT>; 900 #define PCL_INSTANTIATE_SHOTColorEstimation(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTColorEstimation<T,NT,OutT,RFT>; 902 #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...
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
SHOTColorEstimation estimates the Signature of Histograms of OrienTations (SHOT) descriptor for a giv...
void computeFeature(pcl::PointCloud< PointOutT > &output)
Estimate the Signatures of Histograms of OrienTations (SHOT) descriptors at a set of points given by ...
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 computeFeature(pcl::PointCloud< PointOutT > &output)
Estimate the Signatures of Histograms of OrienTations (SHOT) descriptors at a set of points given by ...
void setRadiusSearch(double radius)
Set the sphere radius that is to be used for determining the nearest neighbors used for the feature e...
virtual bool initCompute()
This method should get called before starting the actual computation.
void createBinDistanceShape(int index, const std::vector< int > &indices, std::vector< double > &bin_distance_shape)
Create a binned distance shape histogram.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
virtual void computePointSHOT(const int index, const std::vector< int > &indices, const std::vector< float > &sqr_dists, Eigen::VectorXf &shot)
Estimate the SHOT descriptor for a given point based on its spatial neighborhood of 3D points with no...
void normalizeHistogram(Eigen::VectorXf &shot, int desc_length)
Normalize the SHOT histogram.
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).
FeatureWithLocalReferenceFrames provides a public interface for descriptor extractor classes which ne...
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.
virtual void computePointSHOT(const int index, const std::vector< int > &indices, const std::vector< float > &sqr_dists, Eigen::VectorXf &shot)
Estimate the SHOT descriptor for a given point based on its spatial neighborhood of 3D points with no...
boost::shared_ptr< SHOTLocalReferenceFrameEstimation< PointInT, PointOutT > > Ptr