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>
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 (std::size_t j = 0; j < radius_bins_ + 1; j++)
96 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_))));
99 theta_divisions_.resize (elevation_bins_ + 1, elevation_interval);
100 theta_divisions_[0] = 0;
101 std::partial_sum(theta_divisions_.begin (), theta_divisions_.end (), theta_divisions_.begin ());
104 phi_divisions_.resize (azimuth_bins_ + 1, azimuth_interval);
105 phi_divisions_[0] = 0;
106 std::partial_sum(phi_divisions_.begin (), phi_divisions_.end (), phi_divisions_.begin ());
113 float e = 1.0f / 3.0f;
115 volume_lut_.resize (radius_bins_ * elevation_bins_ * azimuth_bins_);
117 for (std::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 (std::size_t k = 0; k < elevation_bins_; k++)
125 float integr_theta = std::cos (
deg2rad (theta_divisions_[k])) - std::cos (
deg2rad (theta_divisions_[k+1]));
127 float V = integr_phi * integr_theta * integr_r;
133 for (std::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 std::size_t neighb_cnt = searchForNeighbors ((*indices_)[index], search_radius_, nn_indices, nn_dists);
161 for (std::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 (std::acos (std::min (1.0f, std::max (-1.0f, theta))));
192 const auto rad_min = std::lower_bound(std::next (radii_interval_.cbegin ()), radii_interval_.cend (), r);
193 const auto theta_min = std::lower_bound(std::next (theta_divisions_.cbegin ()), theta_divisions_.cend (), theta);
194 const auto phi_min = std::lower_bound(std::next (phi_divisions_.cbegin ()), phi_divisions_.cend (), phi);
197 const auto j =
std::distance(radii_interval_.cbegin (), std::prev(rad_min));
198 const auto k =
std::distance(theta_divisions_.cbegin (), std::prev(theta_min));
199 const auto l =
std::distance(phi_divisions_.cbegin (), std::prev(phi_min));
202 std::vector<int> neighbour_indices;
203 std::vector<float> neighbour_didtances;
204 float point_density = static_cast<float> (searchForNeighbors (*surface_, nn_indices[ne], point_density_radius_, neighbour_indices, neighbour_didtances));
206 float w = (1.0f / point_density) * volume_lut_[(l*elevation_bins_*radius_bins_) +
211 if (w == std::numeric_limits<float>::infinity ())
212 PCL_ERROR (
"Shape Context Error INF!\n");
214 PCL_ERROR (
"Shape Context Error IND!\n");
216 desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] += w;
218 assert (desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] >= 0);
223 template <
typename Po
intInT,
typename Po
intOutT,
typename Po
intRFT>
void
226 assert (descriptor_length_ == 1960);
230 for (std::size_t point_index = 0; point_index < indices_->size (); ++point_index)
235 const PointRFT& current_frame = (*frames_)[point_index];
236 if (!
isFinite ((*input_)[(*indices_)[point_index]]) ||
237 !std::isfinite (current_frame.x_axis[0]) ||
238 !std::isfinite (current_frame.y_axis[0]) ||
239 !std::isfinite (current_frame.z_axis[0]) )
241 std::fill (output.
points[point_index].descriptor, output.
points[point_index].descriptor + descriptor_length_,
242 std::numeric_limits<float>::quiet_NaN ());
243 std::fill (output.
points[point_index].rf, output.
points[point_index].rf + 9, 0);
248 for (
int d = 0; d < 3; ++d)
250 output.
points[point_index].rf[0 + d] = current_frame.x_axis[d];
251 output.
points[point_index].rf[3 + d] = current_frame.y_axis[d];
252 output.
points[point_index].rf[6 + d] = current_frame.z_axis[d];
255 std::vector<float> descriptor (descriptor_length_);
256 computePointDescriptor (point_index, descriptor);
257 std::copy (descriptor.begin (), descriptor.end (), output.
points[point_index].descriptor);
261 #define PCL_INSTANTIATE_UniqueShapeContext(T,OutT,RFT) template class PCL_EXPORTS pcl::UniqueShapeContext<T,OutT,RFT>;