58 PCL_ERROR (
"[pcl::%s::initCompute] Init failed.\n", getClassName ().
c_str ());
72 PCL_ERROR (
"[pcl::%s::initCompute] Init failed.\n", getClassName ().
c_str ());
76 if (search_radius_< min_radius_)
78 PCL_ERROR (
"[pcl::%s::initCompute] search_radius_ must be GREATER than min_radius_.\n", getClassName ().
c_str ());
83 descriptor_length_ = elevation_bins_ * azimuth_bins_ * radius_bins_;
90 radii_interval_.clear ();
91 phi_divisions_.clear ();
92 theta_divisions_.clear ();
96 radii_interval_.resize (radius_bins_ + 1);
97 for (std::size_t j = 0; j < radius_bins_ + 1; j++)
98 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_))));
102 theta_divisions_[0] = 0;
103 std::partial_sum(theta_divisions_.begin (), theta_divisions_.end (), theta_divisions_.begin ());
107 phi_divisions_[0] = 0;
108 std::partial_sum(phi_divisions_.begin (), phi_divisions_.end (), phi_divisions_.begin ());
115 float e = 1.0f / 3.0f;
117 volume_lut_.resize (radius_bins_ * elevation_bins_ * azimuth_bins_);
119 for (std::size_t j = 0; j < radius_bins_; j++)
122 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);
124 for (std::size_t k = 0; k < elevation_bins_; k++)
135 for (std::size_t l = 0; l < azimuth_bins_; l++)
138 volume_lut_[(l*elevation_bins_*radius_bins_) + k*radius_bins_ + j] = 1.0f /
powf (
V,
e);
150 const Eigen::Vector3f x_axis ((*frames_)[index].x_axis[0],
151 (*frames_)[index].x_axis[1],
152 (*frames_)[index].x_axis[2]);
154 const Eigen::Vector3f normal ((*frames_)[index].z_axis[0],
155 (*frames_)[index].z_axis[1],
156 (*frames_)[index].z_axis[2]);
176 Eigen::Vector3f
proj;
184 Eigen::Vector3f cross = x_axis.cross (
proj);
185 float phi =
rad2deg (std::atan2 (cross.norm (), x_axis.dot (
proj)));
186 phi = cross.dot (normal) < 0.f ? (360.0f -
phi) :
phi;
194 const auto rad_min = std::lower_bound(std::next (radii_interval_.cbegin ()), radii_interval_.cend (), r);
195 const auto theta_min = std::lower_bound(std::next (theta_divisions_.cbegin ()), theta_divisions_.cend (),
theta);
196 const auto phi_min = std::lower_bound(std::next (phi_divisions_.cbegin ()), phi_divisions_.cend (),
phi);
199 const auto j = std::distance(radii_interval_.cbegin (), std::prev(
rad_min));
200 const auto k = std::distance(theta_divisions_.cbegin (), std::prev(
theta_min));
201 const auto l = std::distance(phi_divisions_.cbegin (), std::prev(
phi_min));
208 float w = (1.0f /
point_density) * volume_lut_[(l*elevation_bins_*radius_bins_) +
213 if (w == std::numeric_limits<float>::infinity ())
214 PCL_ERROR (
"Shape Context Error INF!\n");
216 PCL_ERROR (
"Shape Context Error IND!\n");
218 desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] += w;
220 assert (desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] >= 0);
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
bool equal(T val1, T val2, T eps=std::numeric_limits< T >::min())
Check if val1 and val2 are equal to an epsilon extent.