41 #include <pcl/features/3dsc.h>
45 #include <pcl/common/point_tests.h>
46 #include <pcl/common/utils.h>
52 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
bool
57 PCL_ERROR (
"[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
61 if (search_radius_< min_radius_)
63 PCL_ERROR (
"[pcl::%s::initCompute] search_radius_ must be GREATER than min_radius_.\n", getClassName ().c_str ());
68 descriptor_length_ = elevation_bins_ * azimuth_bins_ * radius_bins_;
71 float azimuth_interval = 360.0f /
static_cast<float> (azimuth_bins_);
72 float elevation_interval = 180.0f /
static_cast<float> (elevation_bins_);
75 radii_interval_.clear ();
76 phi_divisions_.clear ();
77 theta_divisions_.clear ();
81 radii_interval_.resize (radius_bins_ + 1);
82 for (std::size_t j = 0; j < radius_bins_ + 1; j++)
83 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_))));
86 theta_divisions_.resize (elevation_bins_ + 1, elevation_interval);
87 theta_divisions_[0] = 0.f;
88 std::partial_sum(theta_divisions_.begin (), theta_divisions_.end (), theta_divisions_.begin ());
91 phi_divisions_.resize (azimuth_bins_ + 1, azimuth_interval);
92 phi_divisions_[0] = 0.f;
93 std::partial_sum(phi_divisions_.begin (), phi_divisions_.end (), phi_divisions_.begin ());
100 float e = 1.0f / 3.0f;
102 volume_lut_.resize (radius_bins_ * elevation_bins_ * azimuth_bins_);
104 for (std::size_t j = 0; j < radius_bins_; j++)
107 float integr_r = (radii_interval_[j+1] * radii_interval_[j+1] * radii_interval_[j+1] / 3.0f) - (radii_interval_[j] * radii_interval_[j] * radii_interval_[j] / 3.0f);
109 for (std::size_t k = 0; k < elevation_bins_; k++)
112 float integr_theta = std::cos (
pcl::deg2rad (theta_divisions_[k])) - std::cos (
pcl::deg2rad (theta_divisions_[k+1]));
114 float V = integr_phi * integr_theta * integr_r;
120 for (std::size_t l = 0; l < azimuth_bins_; l++)
124 volume_lut_[(l*elevation_bins_*radius_bins_) + k*radius_bins_ + j] = 1.0f / powf (V, e);
132 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
bool
137 Eigen::Map<Eigen::Vector3f> x_axis (rf);
138 Eigen::Map<Eigen::Vector3f> y_axis (rf + 3);
139 Eigen::Map<Eigen::Vector3f> normal (rf + 6);
143 std::vector<float> nn_dists;
144 const std::size_t neighb_cnt = searchForNeighbors ((*indices_)[index], search_radius_, nn_indices, nn_dists);
147 std::fill (desc.begin (), desc.end (), std::numeric_limits<float>::quiet_NaN ());
148 std::fill (rf, rf + 9, 0.f);
152 const auto minDistanceIt = std::min_element(nn_dists.begin (), nn_dists.end ());
153 const auto minIndex = nn_indices[
std::distance (nn_dists.begin (), minDistanceIt)];
159 normal = normals[minIndex].getNormalVector3fMap ();
166 x_axis[2] = - (normal[0]*x_axis[0] + normal[1]*x_axis[1]) / normal[2];
168 x_axis[1] = - (normal[0]*x_axis[0] + normal[2]*x_axis[2]) / normal[1];
170 x_axis[0] = - (normal[1]*x_axis[1] + normal[2]*x_axis[2]) / normal[0];
175 assert (
pcl::utils::equal (x_axis[0]*normal[0] + x_axis[1]*normal[1] + x_axis[2]*normal[2], 0.0f, 1E-6f));
178 y_axis.matrix () = normal.cross (x_axis);
181 for (std::size_t ne = 0; ne < neighb_cnt; ne++)
186 Eigen::Vector3f neighbour = (*surface_)[nn_indices[ne]].getVector3fMap ();
190 float r = std::sqrt (nn_dists[ne]);
193 Eigen::Vector3f proj;
201 Eigen::Vector3f cross = x_axis.cross (proj);
202 float phi =
pcl::rad2deg (std::atan2 (cross.norm (), x_axis.dot (proj)));
203 phi = cross.dot (normal) < 0.f ? (360.0f - phi) : phi;
205 Eigen::Vector3f no = neighbour - origin;
207 float theta = normal.dot (no);
208 theta =
pcl::rad2deg (std::acos (std::min (1.0f, std::max (-1.0f, theta))));
211 const auto rad_min = std::lower_bound(std::next (radii_interval_.cbegin ()), radii_interval_.cend (), r);
212 const auto theta_min = std::lower_bound(std::next (theta_divisions_.cbegin ()), theta_divisions_.cend (), theta);
213 const auto phi_min = std::lower_bound(std::next (phi_divisions_.cbegin ()), phi_divisions_.cend (), phi);
216 const auto j =
std::distance(radii_interval_.cbegin (), std::prev(rad_min));
217 const auto k =
std::distance(theta_divisions_.cbegin (), std::prev(theta_min));
218 const auto l =
std::distance(phi_divisions_.cbegin (), std::prev(phi_min));
222 std::vector<float> neighbour_distances;
223 int point_density = searchForNeighbors (*surface_, nn_indices[ne], point_density_radius_, neighbour_indices, neighbour_distances);
225 if (point_density == 0)
228 float w = (1.0f /
static_cast<float> (point_density)) *
229 volume_lut_[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j];
232 if (w == std::numeric_limits<float>::infinity ())
233 PCL_ERROR (
"Shape Context Error INF!\n");
235 PCL_ERROR (
"Shape Context Error IND!\n");
237 desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] += w;
239 assert (desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] >= 0);
243 std::fill (rf, rf + 9, 0);
248 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
251 assert (descriptor_length_ == 1980);
253 output.is_dense =
true;
255 for (std::size_t point_index = 0; point_index < indices_->size (); point_index++)
260 if (!
isFinite ((*input_)[(*indices_)[point_index]]))
262 std::fill (output[point_index].descriptor, output[point_index].descriptor + descriptor_length_,
263 std::numeric_limits<float>::quiet_NaN ());
264 std::fill (output[point_index].rf, output[point_index].rf + 9, 0);
265 output.is_dense =
false;
269 std::vector<float> descriptor (descriptor_length_);
270 if (!computePoint (point_index, *normals_, output[point_index].rf, descriptor))
271 output.is_dense =
false;
272 std::copy (descriptor.begin (), descriptor.end (), output[point_index].descriptor);
276 #define PCL_INSTANTIATE_ShapeContext3DEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::ShapeContext3DEstimation<T,NT,OutT>;
Define standard C methods to do angle calculations.
bool computePoint(std::size_t index, const pcl::PointCloud< PointNT > &normals, float rf[9], std::vector< float > &desc)
Estimate a descriptor for a given point.
bool initCompute() override
Initialize computation by allocating all the intervals and the volume lookup table.
typename Feature< PointInT, PointOutT >::PointCloudOut PointCloudOut
void computeFeature(PointCloudOut &output) override
Estimate the actual feature.
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
IndicesAllocator<> Indices
Type used for indices in PCL.