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);
229 output.is_dense =
true;
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);
245 output.is_dense =
false;
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>;
Define standard C methods to do angle calculations.
Feature represents the base feature class.
void setSearchSurface(const PointCloudInConstPtr &cloud)
Provide a pointer to a dataset to add additional information to estimate the features for every point...
void setRadiusSearch(double radius)
Set the sphere radius that is to be used for determining the nearest neighbors used for the feature e...
FeatureWithLocalReferenceFrames provides a public interface for descriptor extractor classes which ne...
virtual void setInputCloud(const PointCloudConstPtr &cloud)
Provide a pointer to the input dataset.
virtual void setIndices(const IndicesPtr &indices)
Provide a pointer to the vector of indices that represents the input data.
SHOTLocalReferenceFrameEstimation estimates the Local Reference Frame used in the calculation of the ...
shared_ptr< SHOTLocalReferenceFrameEstimation< PointInT, PointOutT > > Ptr
void computeFeature(PointCloudOut &output) override
The actual feature computation.
bool initCompute() override
Initialize computation by allocating all the intervals and the volume lookup table.
void computePointDescriptor(std::size_t index, std::vector< float > &desc)
Compute 3D shape context feature descriptor.
typename Feature< PointInT, PointOutT >::PointCloudOut PointCloudOut
Defines some geometrical functions and utility functions.
float deg2rad(float alpha)
Convert an angle from degrees to radians.
float rad2deg(float alpha)
Convert an angle from radians to degrees.
float distance(const PointT &p1, const PointT &p2)
void project(const PointT &point, const PointT &plane_origin, const NormalT &plane_normal, PointT &projected)
bool equal(T val1, T val2, T eps=std::numeric_limits< T >::min())
Check if val1 and val2 are equal to an epsilon extent.
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...
const Eigen::Map< const Eigen::Vector3f > Vector3fMapConst