41 #ifndef PCL_FEATURES_IMPL_USC_HPP_
42 #define PCL_FEATURES_IMPL_USC_HPP_
44 #include <pcl/features/usc.h>
45 #include <pcl/features/shot_lrf.h>
46 #include <pcl/common/geometry.h>
47 #include <pcl/common/angles.h>
48 #include <pcl/common/utils.h>
51 template <
typename Po
intInT,
typename Po
intOutT,
typename Po
intRFT>
bool
56 PCL_ERROR (
"[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
70 PCL_ERROR (
"[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
74 if (search_radius_< min_radius_)
76 PCL_ERROR (
"[pcl::%s::initCompute] search_radius_ must be GREATER than min_radius_.\n", getClassName ().c_str ());
81 descriptor_length_ = elevation_bins_ * azimuth_bins_ * radius_bins_;
84 float azimuth_interval = 360.0f /
static_cast<float> (azimuth_bins_);
85 float elevation_interval = 180.0f /
static_cast<float> (elevation_bins_);
88 radii_interval_.clear ();
89 phi_divisions_.clear ();
90 theta_divisions_.clear ();
94 radii_interval_.resize (radius_bins_ + 1);
95 for (
size_t j = 0; j < radius_bins_ + 1; j++)
96 radii_interval_[j] = static_cast<float> (exp (log (min_radius_) + ((static_cast<float> (j) / static_cast<float> (radius_bins_)) * log (search_radius_/min_radius_))));
99 theta_divisions_.resize (elevation_bins_+1);
100 for (
size_t k = 0; k < elevation_bins_+1; k++)
101 theta_divisions_[k] = static_cast<float> (k) * elevation_interval;
104 phi_divisions_.resize (azimuth_bins_+1);
105 for (
size_t l = 0; l < azimuth_bins_+1; l++)
106 phi_divisions_[l] = static_cast<float> (l) * azimuth_interval;
113 float e = 1.0f / 3.0f;
115 volume_lut_.resize (radius_bins_ * elevation_bins_ * azimuth_bins_);
117 for (
size_t j = 0; j < radius_bins_; j++)
120 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);
122 for (
size_t k = 0; k < elevation_bins_; k++)
125 float integr_theta = cosf (
deg2rad (theta_divisions_[k])) - cosf (
deg2rad (theta_divisions_[k+1]));
127 float V = integr_phi * integr_theta * integr_r;
133 for (
size_t l = 0; l < azimuth_bins_; l++)
136 volume_lut_[(l*elevation_bins_*radius_bins_) + k*radius_bins_ + j] = 1.0f / powf (V, e);
143 template <
typename Po
intInT,
typename Po
intOutT,
typename Po
intRFT>
void
148 const Eigen::Vector3f x_axis (frames_->points[index].x_axis[0],
149 frames_->points[index].x_axis[1],
150 frames_->points[index].x_axis[2]);
152 const Eigen::Vector3f normal (frames_->points[index].z_axis[0],
153 frames_->points[index].z_axis[1],
154 frames_->points[index].z_axis[2]);
157 std::vector<int> nn_indices;
158 std::vector<float> nn_dists;
159 const size_t neighb_cnt = searchForNeighbors ((*indices_)[index], search_radius_, nn_indices, nn_dists);
161 for (
size_t ne = 0; ne < neighb_cnt; ne++)
166 Eigen::Vector3f neighbour = surface_->points[nn_indices[ne]].getVector3fMap ();
171 float r = std::sqrt (nn_dists[ne]);
174 Eigen::Vector3f proj;
182 Eigen::Vector3f cross = x_axis.cross (proj);
183 float phi =
rad2deg (std::atan2 (cross.norm (), x_axis.dot (proj)));
184 phi = cross.dot (normal) < 0.f ? (360.0f - phi) : phi;
186 Eigen::Vector3f no = neighbour - origin;
188 float theta = normal.dot (no);
189 theta =
pcl::rad2deg (acosf (std::min (1.0f, std::max (-1.0f, theta))));
197 for (
size_t rad = 1; rad < radius_bins_ + 1; rad++)
199 if (r <= radii_interval_[rad])
206 for (
size_t ang = 1; ang < elevation_bins_ + 1; ang++)
208 if (theta <= theta_divisions_[ang])
215 for (
size_t ang = 1; ang < azimuth_bins_ + 1; ang++)
217 if (phi <= phi_divisions_[ang])
225 std::vector<int> neighbour_indices;
226 std::vector<float> neighbour_didtances;
227 float point_density =
static_cast<float> (searchForNeighbors (*surface_, nn_indices[ne], point_density_radius_, neighbour_indices, neighbour_didtances));
229 float w = (1.0f / point_density) * volume_lut_[(l*elevation_bins_*radius_bins_) +
234 if (w == std::numeric_limits<float>::infinity ())
235 PCL_ERROR (
"Shape Context Error INF!\n");
237 PCL_ERROR (
"Shape Context Error IND!\n");
239 desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] += w;
241 assert (desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] >= 0);
246 template <
typename Po
intInT,
typename Po
intOutT,
typename Po
intRFT>
void
249 assert (descriptor_length_ == 1960);
253 for (
size_t point_index = 0; point_index < indices_->size (); ++point_index)
258 const PointRFT& current_frame = (*frames_)[point_index];
259 if (!
isFinite ((*input_)[(*indices_)[point_index]]) ||
260 !pcl_isfinite (current_frame.x_axis[0]) ||
261 !pcl_isfinite (current_frame.y_axis[0]) ||
262 !pcl_isfinite (current_frame.z_axis[0]) )
264 for (
size_t i = 0; i < descriptor_length_; ++i)
265 output[point_index].descriptor[i] = std::numeric_limits<float>::quiet_NaN ();
267 memset (output[point_index].rf, 0,
sizeof (output[point_index].rf[0]) * 9);
272 for (
int d = 0; d < 3; ++d)
274 output.
points[point_index].rf[0 + d] = current_frame.x_axis[d];
275 output.
points[point_index].rf[3 + d] = current_frame.y_axis[d];
276 output.
points[point_index].rf[6 + d] = current_frame.z_axis[d];
279 std::vector<float> descriptor (descriptor_length_);
280 computePointDescriptor (point_index, descriptor);
281 for (
size_t j = 0; j < descriptor_length_; ++j)
282 output [point_index].descriptor[j] = descriptor[j];
286 #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 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 computePointDescriptor(size_t index, std::vector< float > &desc)
Compute 3D shape context feature descriptor.
virtual void computeFeature(PointCloudOut &output)
The actual feature computation.
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...
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
virtual bool initCompute()
Initialize computation by allocating all the intervals and the volume lookup table.
const Eigen::Map< const Eigen::Vector3f > Vector3fMapConst
bool equal(T val1, T val2, T eps=std::numeric_limits< T >::min())
Check if val1 and val2 are equal to an epsilon extent.
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
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.
Feature represents the base feature class.
FeatureWithLocalReferenceFrames provides a public interface for descriptor extractor classes which ne...
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...
boost::shared_ptr< SHOTLocalReferenceFrameEstimation< PointInT, PointOutT > > Ptr