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>;