414 const std::vector<float> &sqr_dists,
416 std::vector<double> &binDistanceShape,
417 std::vector<double> &binDistanceColor,
418 const int nr_bins_shape,
419 const int nr_bins_color,
420 Eigen::VectorXf &shot)
422 const Eigen::Vector4f ¢ral_point = (*input_)[(*indices_)[index]].getVector4fMap ();
423 const PointRFT& current_frame = (*frames_)[index];
425 int shapeToColorStride = nr_grid_sector_*(nr_bins_shape+1);
427 Eigen::Vector4f current_frame_x (current_frame.x_axis[0], current_frame.x_axis[1], current_frame.x_axis[2], 0);
428 Eigen::Vector4f current_frame_y (current_frame.y_axis[0], current_frame.y_axis[1], current_frame.y_axis[2], 0);
429 Eigen::Vector4f current_frame_z (current_frame.z_axis[0], current_frame.z_axis[1], current_frame.z_axis[2], 0);
431 for (std::size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
433 if (!std::isfinite(binDistanceShape[i_idx]))
436 Eigen::Vector4f delta = (*surface_)[indices[i_idx]].getVector4fMap () - central_point;
440 double distance = sqrt (sqr_dists[i_idx]);
442 if (areEquals (distance, 0.0))
445 double xInFeatRef = delta.dot (current_frame_x);
446 double yInFeatRef = delta.dot (current_frame_y);
447 double zInFeatRef = delta.dot (current_frame_z);
450 if (std::abs (yInFeatRef) < 1E-30)
452 if (std::abs (xInFeatRef) < 1E-30)
454 if (std::abs (zInFeatRef) < 1E-30)
457 unsigned char bit4 = ((yInFeatRef > 0) || ((yInFeatRef == 0.0) && (xInFeatRef < 0))) ? 1 : 0;
458 auto bit3 =
static_cast<unsigned char> (((xInFeatRef > 0) || ((xInFeatRef == 0.0) && (yInFeatRef > 0))) ? !bit4 : bit4);
460 assert (bit3 == 0 || bit3 == 1);
462 int desc_index = (bit4<<3) + (bit3<<2);
464 desc_index = desc_index << 1;
466 if ((xInFeatRef * yInFeatRef > 0) || (xInFeatRef == 0.0))
467 desc_index += (std::abs (xInFeatRef) >= std::abs (yInFeatRef)) ? 0 : 4;
469 desc_index += (std::abs (xInFeatRef) > std::abs (yInFeatRef)) ? 4 : 0;
471 desc_index += zInFeatRef > 0 ? 1 : 0;
474 desc_index += (distance > radius1_2_) ? 2 : 0;
476 int step_index_shape =
static_cast<int>(std::floor (binDistanceShape[i_idx] +0.5));
477 int step_index_color =
static_cast<int>(std::floor (binDistanceColor[i_idx] +0.5));
479 int volume_index_shape = desc_index * (nr_bins_shape+1);
480 int volume_index_color = shapeToColorStride + desc_index * (nr_bins_color+1);
483 binDistanceShape[i_idx] -= step_index_shape;
484 binDistanceColor[i_idx] -= step_index_color;
486 double intWeightShape = (1- std::abs (binDistanceShape[i_idx]));
487 double intWeightColor = (1- std::abs (binDistanceColor[i_idx]));
489 if (binDistanceShape[i_idx] > 0)
490 shot[volume_index_shape + ((step_index_shape + 1) % nr_bins_shape)] +=
static_cast<float> (binDistanceShape[i_idx]);
492 shot[volume_index_shape + ((step_index_shape - 1 + nr_bins_shape) % nr_bins_shape)] -=
static_cast<float> (binDistanceShape[i_idx]);
494 if (binDistanceColor[i_idx] > 0)
495 shot[volume_index_color + ((step_index_color+1) % nr_bins_color)] +=
static_cast<float> (binDistanceColor[i_idx]);
497 shot[volume_index_color + ((step_index_color - 1 + nr_bins_color) % nr_bins_color)] -=
static_cast<float> (binDistanceColor[i_idx]);
501 if (distance > radius1_2_)
503 double radiusDistance = (distance - radius3_4_) / radius1_2_;
505 if (distance > radius3_4_)
507 intWeightShape += 1 - radiusDistance;
508 intWeightColor += 1 - radiusDistance;
512 intWeightShape += 1 + radiusDistance;
513 intWeightColor += 1 + radiusDistance;
514 shot[(desc_index - 2) * (nr_bins_shape+1) + step_index_shape] -=
static_cast<float> (radiusDistance);
515 shot[shapeToColorStride + (desc_index - 2) * (nr_bins_color+1) + step_index_color] -=
static_cast<float> (radiusDistance);
520 double radiusDistance = (distance - radius1_4_) / radius1_2_;
522 if (distance < radius1_4_)
524 intWeightShape += 1 + radiusDistance;
525 intWeightColor += 1 + radiusDistance;
529 intWeightShape += 1 - radiusDistance;
530 intWeightColor += 1 - radiusDistance;
531 shot[(desc_index + 2) * (nr_bins_shape+1) + step_index_shape] +=
static_cast<float> (radiusDistance);
532 shot[shapeToColorStride + (desc_index + 2) * (nr_bins_color+1) + step_index_color] +=
static_cast<float> (radiusDistance);
537 double inclinationCos = zInFeatRef / distance;
538 if (inclinationCos < - 1.0)
539 inclinationCos = - 1.0;
540 if (inclinationCos > 1.0)
541 inclinationCos = 1.0;
543 double inclination = std::acos (inclinationCos);
545 assert (inclination >= 0.0 && inclination <= PST_RAD_180);
547 if (inclination > PST_RAD_90 || (std::abs (inclination - PST_RAD_90) < 1e-30 && zInFeatRef <= 0))
549 double inclinationDistance = (inclination - PST_RAD_135) / PST_RAD_90;
550 if (inclination > PST_RAD_135)
552 intWeightShape += 1 - inclinationDistance;
553 intWeightColor += 1 - inclinationDistance;
557 intWeightShape += 1 + inclinationDistance;
558 intWeightColor += 1 + inclinationDistance;
559 assert ((desc_index + 1) * (nr_bins_shape+1) + step_index_shape >= 0 && (desc_index + 1) * (nr_bins_shape+1) + step_index_shape < descLength_);
560 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_);
561 shot[(desc_index + 1) * (nr_bins_shape+1) + step_index_shape] -=
static_cast<float> (inclinationDistance);
562 shot[shapeToColorStride + (desc_index + 1) * (nr_bins_color+1) + step_index_color] -=
static_cast<float> (inclinationDistance);
567 double inclinationDistance = (inclination - PST_RAD_45) / PST_RAD_90;
568 if (inclination < PST_RAD_45)
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);
584 if (yInFeatRef != 0.0 || xInFeatRef != 0.0)
587 double azimuth = std::atan2 (yInFeatRef, xInFeatRef);
589 int sel = desc_index >> 2;
590 double angularSectorSpan = PST_RAD_45;
591 double angularSectorStart = - PST_RAD_PI_7_8;
593 double azimuthDistance = (azimuth - (angularSectorStart + angularSectorSpan*sel)) / angularSectorSpan;
594 assert ((azimuthDistance < 0.5 || areEquals (azimuthDistance, 0.5)) && (azimuthDistance > - 0.5 || areEquals (azimuthDistance, - 0.5)));
595 azimuthDistance = (std::max)(- 0.5, std::min (azimuthDistance, 0.5));
597 if (azimuthDistance > 0)
599 intWeightShape += 1 - azimuthDistance;
600 intWeightColor += 1 - azimuthDistance;
601 int interp_index = (desc_index + 4) % maxAngularSectors_;
602 assert (interp_index * (nr_bins_shape+1) + step_index_shape >= 0 && interp_index * (nr_bins_shape+1) + step_index_shape < descLength_);
603 assert (shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color >= 0 && shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color < descLength_);
604 shot[interp_index * (nr_bins_shape+1) + step_index_shape] +=
static_cast<float> (azimuthDistance);
605 shot[shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color] +=
static_cast<float> (azimuthDistance);
609 int interp_index = (desc_index - 4 + maxAngularSectors_) % maxAngularSectors_;
610 intWeightShape += 1 + azimuthDistance;
611 intWeightColor += 1 + azimuthDistance;
612 assert (interp_index * (nr_bins_shape+1) + step_index_shape >= 0 && interp_index * (nr_bins_shape+1) + step_index_shape < descLength_);
613 assert (shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color >= 0 && shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color < descLength_);
614 shot[interp_index * (nr_bins_shape+1) + step_index_shape] -=
static_cast<float> (azimuthDistance);
615 shot[shapeToColorStride + interp_index * (nr_bins_color+1) + step_index_color] -=
static_cast<float> (azimuthDistance);
619 assert (volume_index_shape + step_index_shape >= 0 && volume_index_shape + step_index_shape < descLength_);
620 assert (volume_index_color + step_index_color >= 0 && volume_index_color + step_index_color < descLength_);
621 shot[volume_index_shape + step_index_shape] +=
static_cast<float> (intWeightShape);
622 shot[volume_index_color + step_index_color] +=
static_cast<float> (intWeightColor);
629 const int index,
const pcl::Indices &indices,
const std::vector<float> &sqr_dists, Eigen::VectorXf &shot)
633 const auto nNeighbors = indices.size ();
637 PCL_WARN (
"[pcl::%s::computePointSHOT] Warning! Neighborhood has less than 5 vertexes. Aborting description of point with index %d\n",
638 getClassName ().c_str (), (*indices_)[index]);
640 shot.setConstant(descLength_, 1, std::numeric_limits<float>::quiet_NaN () );
646 std::vector<double> binDistanceShape;
647 if (b_describe_shape_)
649 this->createBinDistanceShape (index, indices, binDistanceShape);
653 std::vector<double> binDistanceColor;
654 if (b_describe_color_)
656 binDistanceColor.reserve (nNeighbors);
661 unsigned char redRef = (*input_)[(*indices_)[index]].r;
662 unsigned char greenRef = (*input_)[(*indices_)[index]].g;
663 unsigned char blueRef = (*input_)[(*indices_)[index]].b;
665 float LRef, aRef, bRef;
667 RGB2CIELAB (redRef, greenRef, blueRef, LRef, aRef, bRef);
672 for (
const auto& idx: indices)
674 unsigned char red = (*surface_)[idx].r;
675 unsigned char green = (*surface_)[idx].g;
676 unsigned char blue = (*surface_)[idx].b;
680 RGB2CIELAB (red, green, blue, L, a, b);
685 double colorDistance = (std::fabs (LRef - L) + ((std::fabs (aRef - a) + std::fabs (bRef - b)) / 2)) /3;
687 if (colorDistance > 1.0)
689 if (colorDistance < 0.0)
692 binDistanceColor.push_back (colorDistance * nr_color_bins_);
698 if (b_describe_shape_ && b_describe_color_)
699 interpolateDoubleChannel (indices, sqr_dists, index, binDistanceShape, binDistanceColor,
700 nr_shape_bins_, nr_color_bins_,
702 else if (b_describe_color_)
703 interpolateSingleChannel (indices, sqr_dists, index, binDistanceColor, nr_color_bins_, shot);
705 interpolateSingleChannel (indices, sqr_dists, index, binDistanceShape, nr_shape_bins_, shot);
708 this->normalizeHistogram (shot, descLength_);
745 descLength_ = nr_grid_sector_ * (nr_shape_bins_+1);
747 sqradius_ = search_radius_ * search_radius_;
748 radius3_4_ = (search_radius_*3) / 4;
749 radius1_4_ = search_radius_ / 4;
750 radius1_2_ = search_radius_ / 2;
752 assert(descLength_ == 352);
754 Eigen::VectorXf shot;
755 shot.setZero (descLength_);
760 std::vector<float> nn_dists (k_);
764 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
766 bool lrf_is_nan =
false;
767 const PointRFT& current_frame = (*frames_)[idx];
768 if (!std::isfinite (current_frame.x_axis[0]) ||
769 !std::isfinite (current_frame.y_axis[0]) ||
770 !std::isfinite (current_frame.z_axis[0]))
772 PCL_WARN (
"[pcl::%s::computeFeature] The local reference frame is not valid! Aborting description of point with index %d\n",
773 getClassName ().c_str (), (*indices_)[idx]);
777 if (!
isFinite ((*input_)[(*indices_)[idx]]) ||
779 this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
782 for (
int d = 0; d < descLength_; ++d)
783 output[idx].descriptor[d] = std::numeric_limits<float>::quiet_NaN ();
784 for (
int d = 0; d < 9; ++d)
785 output[idx].rf[d] = std::numeric_limits<float>::quiet_NaN ();
792 computePointSHOT (
static_cast<int> (idx), nn_indices, nn_dists, shot);
795 for (
int d = 0; d < descLength_; ++d)
796 output[idx].descriptor[d] = shot[d];
797 for (
int d = 0; d < 3; ++d)
799 output[idx].rf[d + 0] = (*frames_)[idx].x_axis[d];
800 output[idx].rf[d + 3] = (*frames_)[idx].y_axis[d];
801 output[idx].rf[d + 6] = (*frames_)[idx].z_axis[d];
813 descLength_ = (b_describe_shape_) ? nr_grid_sector_*(nr_shape_bins_+1) : 0;
814 descLength_ += (b_describe_color_) ? nr_grid_sector_*(nr_color_bins_+1) : 0;
816 assert( (!b_describe_color_ && b_describe_shape_ && descLength_ == 352) ||
817 (b_describe_color_ && !b_describe_shape_ && descLength_ == 992) ||
818 (b_describe_color_ && b_describe_shape_ && descLength_ == 1344)
822 sqradius_ = search_radius_*search_radius_;
823 radius3_4_ = (search_radius_*3) / 4;
824 radius1_4_ = search_radius_ / 4;
825 radius1_2_ = search_radius_ / 2;
827 Eigen::VectorXf shot;
828 shot.setZero (descLength_);
833 std::vector<float> nn_dists (k_);
837 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
839 bool lrf_is_nan =
false;
840 const PointRFT& current_frame = (*frames_)[idx];
841 if (!std::isfinite (current_frame.x_axis[0]) ||
842 !std::isfinite (current_frame.y_axis[0]) ||
843 !std::isfinite (current_frame.z_axis[0]))
845 PCL_WARN (
"[pcl::%s::computeFeature] The local reference frame is not valid! Aborting description of point with index %d\n",
846 getClassName ().c_str (), (*indices_)[idx]);
850 if (!
isFinite ((*input_)[(*indices_)[idx]]) ||
852 this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
855 for (
int d = 0; d < descLength_; ++d)
856 output[idx].descriptor[d] = std::numeric_limits<float>::quiet_NaN ();
857 for (
int d = 0; d < 9; ++d)
858 output[idx].rf[d] = std::numeric_limits<float>::quiet_NaN ();
865 computePointSHOT (
static_cast<int> (idx), nn_indices, nn_dists, shot);
868 for (
int d = 0; d < descLength_; ++d)
869 output[idx].descriptor[d] = shot[d];
870 for (
int d = 0; d < 3; ++d)
872 output[idx].rf[d + 0] = (*frames_)[idx].x_axis[d];
873 output[idx].rf[d + 3] = (*frames_)[idx].y_axis[d];
874 output[idx].rf[d + 6] = (*frames_)[idx].z_axis[d];