43 #include <pcl/features/usc.h> 44 #include <pcl/features/shot_lrf.h> 47 #include <pcl/common/point_tests.h> 48 #include <pcl/common/utils.h> 52 template <
typename Po
intInT,
typename Po
intOutT,
typename Po
intRFT>
bool 57 PCL_ERROR (
"[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
71 PCL_ERROR (
"[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
75 if (search_radius_< min_radius_)
77 PCL_ERROR (
"[pcl::%s::initCompute] search_radius_ must be GREATER than min_radius_.\n", getClassName ().c_str ());
82 descriptor_length_ = elevation_bins_ * azimuth_bins_ * radius_bins_;
85 float azimuth_interval = 360.0f /
static_cast<float> (azimuth_bins_);
86 float elevation_interval = 180.0f /
static_cast<float> (elevation_bins_);
89 radii_interval_.clear ();
90 phi_divisions_.clear ();
91 theta_divisions_.clear ();
95 radii_interval_.resize (radius_bins_ + 1);
96 for (std::size_t j = 0; j < radius_bins_ + 1; j++)
97 radii_interval_[j] = static_cast<float> (std::exp (std::log (min_radius_) + ((static_cast<float> (j) / static_cast<float> (radius_bins_)) * std::log (search_radius_/min_radius_))));
100 theta_divisions_.resize (elevation_bins_ + 1, elevation_interval);
101 theta_divisions_[0] = 0;
102 std::partial_sum(theta_divisions_.begin (), theta_divisions_.end (), theta_divisions_.begin ());
105 phi_divisions_.resize (azimuth_bins_ + 1, azimuth_interval);
106 phi_divisions_[0] = 0;
107 std::partial_sum(phi_divisions_.begin (), phi_divisions_.end (), phi_divisions_.begin ());
114 float e = 1.0f / 3.0f;
116 volume_lut_.resize (radius_bins_ * elevation_bins_ * azimuth_bins_);
118 for (std::size_t j = 0; j < radius_bins_; j++)
121 float integr_r = (radii_interval_[j+1]*radii_interval_[j+1]*radii_interval_[j+1] / 3) - (radii_interval_[j]*radii_interval_[j]*radii_interval_[j]/ 3);
123 for (std::size_t k = 0; k < elevation_bins_; k++)
126 float integr_theta = std::cos (
deg2rad (theta_divisions_[k])) - std::cos (
deg2rad (theta_divisions_[k+1]));
128 float V = integr_phi * integr_theta * integr_r;
134 for (std::size_t l = 0; l < azimuth_bins_; l++)
137 volume_lut_[(l*elevation_bins_*radius_bins_) + k*radius_bins_ + j] = 1.0f / powf (V, e);
144 template <
typename Po
intInT,
typename Po
intOutT,
typename Po
intRFT>
void 149 const Eigen::Vector3f x_axis ((*frames_)[index].x_axis[0],
150 (*frames_)[index].x_axis[1],
151 (*frames_)[index].x_axis[2]);
153 const Eigen::Vector3f normal ((*frames_)[index].z_axis[0],
154 (*frames_)[index].z_axis[1],
155 (*frames_)[index].z_axis[2]);
158 std::vector<int> nn_indices;
159 std::vector<float> nn_dists;
160 const std::size_t neighb_cnt = searchForNeighbors ((*indices_)[index], search_radius_, nn_indices, nn_dists);
162 for (std::size_t ne = 0; ne < neighb_cnt; ne++)
167 Eigen::Vector3f neighbour = (*surface_)[nn_indices[ne]].getVector3fMap ();
172 float r = std::sqrt (nn_dists[ne]);
175 Eigen::Vector3f proj;
183 Eigen::Vector3f cross = x_axis.cross (proj);
184 float phi =
rad2deg (std::atan2 (cross.norm (), x_axis.dot (proj)));
185 phi = cross.dot (normal) < 0.f ? (360.0f - phi) : phi;
187 Eigen::Vector3f no = neighbour - origin;
189 float theta = normal.dot (no);
190 theta =
pcl::rad2deg (std::acos (std::min (1.0f, std::max (-1.0f, theta))));
193 const auto rad_min = std::lower_bound(std::next (radii_interval_.cbegin ()), radii_interval_.cend (), r);
194 const auto theta_min = std::lower_bound(std::next (theta_divisions_.cbegin ()), theta_divisions_.cend (), theta);
195 const auto phi_min = std::lower_bound(std::next (phi_divisions_.cbegin ()), phi_divisions_.cend (), phi);
198 const auto j =
std::distance(radii_interval_.cbegin (), std::prev(rad_min));
199 const auto k =
std::distance(theta_divisions_.cbegin (), std::prev(theta_min));
200 const auto l =
std::distance(phi_divisions_.cbegin (), std::prev(phi_min));
203 std::vector<int> neighbour_indices;
204 std::vector<float> neighbour_didtances;
205 float point_density =
static_cast<float> (searchForNeighbors (*surface_, nn_indices[ne], point_density_radius_, neighbour_indices, neighbour_didtances));
207 float w = (1.0f / point_density) * volume_lut_[(l*elevation_bins_*radius_bins_) +
212 if (w == std::numeric_limits<float>::infinity ())
213 PCL_ERROR (
"Shape Context Error INF!\n");
215 PCL_ERROR (
"Shape Context Error IND!\n");
217 desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] += w;
219 assert (desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] >= 0);
224 template <
typename Po
intInT,
typename Po
intOutT,
typename Po
intRFT>
void 227 assert (descriptor_length_ == 1960);
231 for (std::size_t point_index = 0; point_index < indices_->size (); ++point_index)
236 const PointRFT& current_frame = (*frames_)[point_index];
237 if (!
isFinite ((*input_)[(*indices_)[point_index]]) ||
238 !std::isfinite (current_frame.x_axis[0]) ||
239 !std::isfinite (current_frame.y_axis[0]) ||
240 !std::isfinite (current_frame.z_axis[0]) )
242 std::fill (output[point_index].descriptor, output[point_index].descriptor + descriptor_length_,
243 std::numeric_limits<float>::quiet_NaN ());
244 std::fill (output[point_index].rf, output[point_index].rf + 9, 0);
249 for (
int d = 0; d < 3; ++d)
251 output[point_index].rf[0 + d] = current_frame.x_axis[d];
252 output[point_index].rf[3 + d] = current_frame.y_axis[d];
253 output[point_index].rf[6 + d] = current_frame.z_axis[d];
256 std::vector<float> descriptor (descriptor_length_);
257 computePointDescriptor (point_index, descriptor);
258 std::copy (descriptor.begin (), descriptor.end (), output[point_index].descriptor);
262 #define PCL_INSTANTIATE_UniqueShapeContext(T,OutT,RFT) template class PCL_EXPORTS pcl::UniqueShapeContext<T,OutT,RFT>; void setSearchSurface(const PointCloudInConstPtr &cloud)
Provide a pointer to a dataset to add additional information to estimate the features for every point...
void project(const PointT &point, const PointT &plane_origin, const NormalT &plane_normal, PointT &projected)
bool initCompute() override
Initialize computation by allocating all the intervals and the volume lookup table.
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...
float deg2rad(float alpha)
Convert an angle from degrees to radians.
void computeFeature(PointCloudOut &output) override
The actual feature computation.
Defines some geometrical functions and utility functions.
float rad2deg(float alpha)
Convert an angle from radians to degrees.
void setRadiusSearch(double radius)
Set the sphere radius that is to be used for determining the nearest neighbors used for the feature e...
shared_ptr< SHOTLocalReferenceFrameEstimation< PointInT, PointOutT > > Ptr
bool equal(T val1, T val2, T eps=std::numeric_limits< T >::min())
Check if val1 and val2 are equal to an epsilon extent.
float distance(const PointT &p1, const PointT &p2)
const Eigen::Map< const Eigen::Vector3f > Vector3fMapConst
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
void computePointDescriptor(std::size_t index, std::vector< float > &desc)
Compute 3D shape context feature descriptor.
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 in any of their floating point fields)...
Feature represents the base feature class.
FeatureWithLocalReferenceFrames provides a public interface for descriptor extractor classes which ne...
Define standard C methods to do angle calculations.