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 = sqrtf (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_ == 1980);
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 computePointDescriptor(size_t index, std::vector< float > &desc)
Compute 3D shape context feature descriptor.
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
bool equal(T val1, T val2, T eps=std::numeric_limits< T >::min())
Check if val1 and val2 are equals to an epsilon extent.
void setRadiusSearch(double radius)
Set the sphere radius that is to be used for determining the nearest neighbors used for the feature e...
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...
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested.
virtual bool initCompute()
Initialize computation by allocating all the intervals and the volume lookup table.
boost::shared_ptr< SHOTLocalReferenceFrameEstimation< PointInT, PointOutT > > Ptr
virtual void computeFeature(PointCloudOut &output)
The actual feature computation.
void setSearchSurface(const PointCloudInConstPtr &cloud)
Provide a pointer to a dataset to add additional information to estimate the features for every point...
float deg2rad(float alpha)
Convert an angle from degrees to radians.
void project(const PointT &point, const PointT &plane_origin, const NormalT &plane_normal, PointT &projected)
Feature represents the base feature class.
const Eigen::Map< const Eigen::Vector3f > Vector3fMapConst
SHOTLocalReferenceFrameEstimation estimates the Local Reference Frame used in the calculation of the ...
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
float rad2deg(float alpha)
Convert an angle from radians to degrees.