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