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>
46#include <pcl/common/colors.h>
49#define PST_PI 3.1415926535897932384626433832795
50#define PST_RAD_45 0.78539816339744830961566084581988
51#define PST_RAD_90 1.5707963267948966192313216916398
52#define PST_RAD_135 2.3561944901923449288469825374596
53#define PST_RAD_180 PST_PI
54#define PST_RAD_360 6.283185307179586476925286766558
55#define PST_RAD_PI_7_8 2.7488935718910690836548129603691
57const double zeroDoubleEps15 = 1E-15;
58const float zeroFloatEps8 = 1E-8f;
69areEquals (
double val1,
double val2,
double zeroDoubleEps = zeroDoubleEps15)
71 return (std::abs (val1 - val2)<zeroDoubleEps);
83areEquals (
float val1,
float val2,
float zeroFloatEps = zeroFloatEps8)
85 return (std::fabs (val1 - val2)<zeroFloatEps);
89template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
94template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
95std::array<float, 4000>
99template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
void
101 unsigned char B,
float &L,
float &
A,
104 float fr = sRGB_LUT[
R];
105 float fg = sRGB_LUT[
G];
106 float fb = sRGB_LUT[
B];
109 const float x =
fr * 0.412453f +
fg * 0.357580f +
fb * 0.180423f;
110 const float y =
fr * 0.212671f +
fg * 0.715160f +
fb * 0.072169f;
111 const float z =
fr * 0.019334f +
fg * 0.119193f +
fb * 0.950227f;
113 float vx = x / 0.95047f;
115 float vz = z / 1.08883f;
121 L = 116.0f *
vy - 16.0f;
125 A = 500.0f * (
vx -
vy);
139template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
bool
144 PCL_ERROR (
"[pcl::%s::initCompute] Init failed.\n", getClassName ().
c_str ());
149 if (this->getKSearch () != 0)
152 "[pcl::%s::initCompute] Error! Search method set to k-neighborhood. Call setKSearch(0) and setRadiusSearch( radius ) to use this class.\n",
153 getClassName().
c_str ());
159 lrf_estimator->setRadiusSearch ((lrf_radius_ > 0 ? lrf_radius_ : search_radius_));
167 PCL_ERROR (
"[pcl::%s::initCompute] Init failed.\n", getClassName ().
c_str ());
175template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
void
193 const Eigen::Vector4f&
normal_vec = (*normals_)[indices[
i_idx]].getNormalVector4fMap ();
214 PCL_WARN (
"[pcl::%s::createBinDistanceShape] Point %d has %d (%f%%) NaN normals in its neighbourhood\n",
219template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
void
236template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
void
243 Eigen::VectorXf &
shot)
245 const Eigen::Vector4f&
central_point = (*input_)[(*indices_)[index]].getVector4fMap ();
263 if (areEquals (distance, 0.0))
296 desc_index += (distance > radius1_2_) ? 2 : 0;
312 if (distance > radius1_2_)
316 if (distance > radius3_4_)
326 double radiusDistance = (distance - radius1_4_) / radius1_2_;
328 if (distance < radius1_4_)
329 intWeight += 1 + radiusDistance;
332 intWeight += 1 - radiusDistance;
333 shot[(desc_index + 2) * (nr_bins+1) + step_index] +=
static_cast<float> (radiusDistance);
338 double inclinationCos = zInFeatRef /
distance;
339 if (inclinationCos < - 1.0)
340 inclinationCos = - 1.0;
341 if (inclinationCos > 1.0)
342 inclinationCos = 1.0;
344 double inclination = std::acos (inclinationCos);
346 assert (inclination >= 0.0 && inclination <= PST_RAD_180);
348 if (inclination > PST_RAD_90 || (std::abs (inclination - PST_RAD_90) < 1e-30 && zInFeatRef <= 0))
411template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
void
420 Eigen::VectorXf &
shot)
422 const Eigen::Vector4f &
central_point = (*input_)[(*indices_)[index]].getVector4fMap ();
442 if (areEquals (distance, 0.0))
474 desc_index += (distance > radius1_2_) ? 2 : 0;
501 if (distance > radius1_2_)
505 if (distance > radius3_4_)
522 if (distance < radius1_4_)
627template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
void
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 () );
647 if (b_describe_shape_)
654 if (b_describe_color_)
661 unsigned char redRef = (*input_)[(*indices_)[index]].r;
662 unsigned char greenRef = (*input_)[(*indices_)[index]].g;
663 unsigned char blueRef = (*input_)[(*indices_)[index]].b;
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)
698 if (b_describe_shape_ && b_describe_color_)
700 nr_shape_bins_, nr_color_bins_,
702 else if (b_describe_color_)
708 this->normalizeHistogram (
shot, descLength_);
712template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
void
717 if (indices.size () < 5)
719 PCL_WARN (
"[pcl::%s::computePointSHOT] Warning! Neighborhood has less than 5 vertexes. Aborting description of point with index %d\n",
720 getClassName ().
c_str (), (*indices_)[index]);
722 shot.setConstant(descLength_, 1, std::numeric_limits<float>::quiet_NaN () );
736 this->normalizeHistogram (
shot, descLength_);
742template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
void
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 shot_.setZero (descLength_);
763 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
771 PCL_WARN (
"[pcl::%s::computeFeature] The local reference frame is not valid! Aborting description of point with index %d\n",
772 getClassName ().
c_str (), (*indices_)[idx]);
776 if (!
isFinite ((*input_)[(*indices_)[idx]]) ||
778 this->searchForNeighbors ((*indices_)[idx], search_parameter_,
nn_indices,
nn_dists) == 0)
781 for (
int d = 0; d < descLength_; ++d)
782 output[idx].descriptor[d] = std::numeric_limits<float>::quiet_NaN ();
783 for (
int d = 0; d < 9; ++d)
784 output[idx].rf[d] = std::numeric_limits<float>::quiet_NaN ();
794 for (
int d = 0; d < descLength_; ++d)
795 output[idx].descriptor[d] = shot_[d];
796 for (
int d = 0; d < 3; ++d)
798 output[idx].rf[d + 0] = (*frames_)[idx].x_axis[d];
799 output[idx].rf[d + 3] = (*frames_)[idx].y_axis[d];
800 output[idx].rf[d + 6] = (*frames_)[idx].z_axis[d];
808template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename Po
intRFT>
void
812 descLength_ = (b_describe_shape_) ? nr_grid_sector_*(nr_shape_bins_+1) : 0;
813 descLength_ += (b_describe_color_) ? nr_grid_sector_*(nr_color_bins_+1) : 0;
815 assert( (!b_describe_color_ && b_describe_shape_ && descLength_ == 352) ||
816 (b_describe_color_ && !b_describe_shape_ && descLength_ == 992) ||
817 (b_describe_color_ && b_describe_shape_ && descLength_ == 1344)
821 sqradius_ = search_radius_*search_radius_;
822 radius3_4_ = (search_radius_*3) / 4;
823 radius1_4_ = search_radius_ / 4;
824 radius1_2_ = search_radius_ / 2;
826 shot_.setZero (descLength_);
835 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
843 PCL_WARN (
"[pcl::%s::computeFeature] The local reference frame is not valid! Aborting description of point with index %d\n",
844 getClassName ().
c_str (), (*indices_)[idx]);
848 if (!
isFinite ((*input_)[(*indices_)[idx]]) ||
850 this->searchForNeighbors ((*indices_)[idx], search_parameter_,
nn_indices,
nn_dists) == 0)
853 for (
int d = 0; d < descLength_; ++d)
854 output[idx].descriptor[d] = std::numeric_limits<float>::quiet_NaN ();
855 for (
int d = 0; d < 9; ++d)
856 output[idx].rf[d] = std::numeric_limits<float>::quiet_NaN ();
866 for (
int d = 0; d < descLength_; ++d)
867 output[idx].descriptor[d] = shot_[d];
868 for (
int d = 0; d < 3; ++d)
870 output[idx].rf[d + 0] = (*frames_)[idx].x_axis[d];
871 output[idx].rf[d + 3] = (*frames_)[idx].y_axis[d];
872 output[idx].rf[d + 6] = (*frames_)[idx].z_axis[d];
877#define PCL_INSTANTIATE_SHOTEstimationBase(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTEstimationBase<T,NT,OutT,RFT>;
878#define PCL_INSTANTIATE_SHOTEstimation(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTEstimation<T,NT,OutT,RFT>;
879#define PCL_INSTANTIATE_SHOTColorEstimation(T,NT,OutT,RFT) template class PCL_EXPORTS pcl::SHOTColorEstimation<T,NT,OutT,RFT>;
Iterator class for point clouds with or without given indices.
ConstCloudIterator(const PointCloud< PointT > &cloud)
std::size_t size() const
Size of the range the iterator is going through.
FeatureWithLocalReferenceFrames provides a public interface for descriptor extractor classes which ne...
SHOTColorEstimation estimates the Signature of Histograms of OrienTations (SHOT) descriptor for a giv...
void interpolateDoubleChannel(const pcl::Indices &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 pcl::Indices &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 pcl::Indices &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 pcl::Indices &indices, std::vector< double > &bin_distance_shape)
Create a binned distance shape histogram.
void computeFeature(pcl::PointCloud< PointOutT > &output) override
Estimate the Signatures of Histograms of OrienTations (SHOT) descriptors at a set of points given by ...
void computePointSHOT(const int index, const pcl::Indices &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...
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...
IndicesAllocator<> Indices
Type used for indices in PCL.