39 #ifndef PCL_FEATURES_IMPL_3DSC_HPP_ 40 #define PCL_FEATURES_IMPL_3DSC_HPP_ 43 #include <pcl/features/3dsc.h> 44 #include <pcl/common/utils.h> 49 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
bool 54 PCL_ERROR (
"[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
58 if (search_radius_< min_radius_)
60 PCL_ERROR (
"[pcl::%s::initCompute] search_radius_ must be GREATER than min_radius_.\n", getClassName ().c_str ());
65 descriptor_length_ = elevation_bins_ * azimuth_bins_ * radius_bins_;
68 float azimuth_interval = 360.0f /
static_cast<float> (azimuth_bins_);
69 float elevation_interval = 180.0f /
static_cast<float> (elevation_bins_);
72 radii_interval_.clear ();
73 phi_divisions_.clear ();
74 theta_divisions_.clear ();
78 radii_interval_.resize (radius_bins_ + 1);
79 for (
size_t j = 0; j < radius_bins_ + 1; j++)
80 radii_interval_[j] = static_cast<float> (exp (log (min_radius_) + ((static_cast<float> (j) / static_cast<float> (radius_bins_)) * log (search_radius_ / min_radius_))));
83 theta_divisions_.resize (elevation_bins_ + 1);
84 for (
size_t k = 0; k < elevation_bins_ + 1; k++)
85 theta_divisions_[k] = static_cast<float> (k) * elevation_interval;
88 phi_divisions_.resize (azimuth_bins_ + 1);
89 for (
size_t l = 0; l < azimuth_bins_ + 1; l++)
90 phi_divisions_[l] = static_cast<float> (l) * azimuth_interval;
97 float e = 1.0f / 3.0f;
99 volume_lut_.resize (radius_bins_ * elevation_bins_ * azimuth_bins_);
101 for (
size_t j = 0; j < radius_bins_; j++)
104 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);
106 for (
size_t k = 0; k < elevation_bins_; k++)
111 float V = integr_phi * integr_theta * integr_r;
117 for (
size_t l = 0; l < azimuth_bins_; l++)
121 volume_lut_[(l*elevation_bins_*radius_bins_) + k*radius_bins_ + j] = 1.0f / powf (V, e);
129 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
bool 134 Eigen::Map<Eigen::Vector3f> x_axis (rf);
135 Eigen::Map<Eigen::Vector3f> y_axis (rf + 3);
136 Eigen::Map<Eigen::Vector3f> normal (rf + 6);
139 std::vector<int> nn_indices;
140 std::vector<float> nn_dists;
141 const size_t neighb_cnt = searchForNeighbors ((*indices_)[index], search_radius_, nn_indices, nn_dists);
144 for (
size_t i = 0; i < desc.size (); ++i)
145 desc[i] = std::numeric_limits<float>::quiet_NaN ();
147 memset (rf, 0,
sizeof (rf[0]) * 9);
151 float minDist = std::numeric_limits<float>::max ();
153 for (
size_t i = 0; i < nn_indices.size (); i++)
155 if (nn_dists[i] < minDist)
157 minDist = nn_dists[i];
158 minIndex = nn_indices[i];
163 Vector3fMapConst origin = input_->points[(*indices_)[index]].getVector3fMap ();
166 normal = normals[minIndex].getNormalVector3fMap ();
169 x_axis[0] =
static_cast<float> (rnd ());
170 x_axis[1] =
static_cast<float> (rnd ());
171 x_axis[2] =
static_cast<float> (rnd ());
173 x_axis[2] = - (normal[0]*x_axis[0] + normal[1]*x_axis[1]) / normal[2];
175 x_axis[1] = - (normal[0]*x_axis[0] + normal[2]*x_axis[2]) / normal[1];
177 x_axis[0] = - (normal[1]*x_axis[1] + normal[2]*x_axis[2]) / normal[0];
182 assert (
pcl::utils::equal (x_axis[0]*normal[0] + x_axis[1]*normal[1] + x_axis[2]*normal[2], 0.0f, 1E-6f));
185 y_axis.matrix () = normal.cross (x_axis);
188 for (
size_t ne = 0; ne < neighb_cnt; ne++)
193 Eigen::Vector3f neighbour = surface_->points[nn_indices[ne]].getVector3fMap ();
197 float r = std::sqrt (nn_dists[ne]);
200 Eigen::Vector3f proj;
208 Eigen::Vector3f cross = x_axis.cross (proj);
209 float phi =
pcl::rad2deg (std::atan2 (cross.norm (), x_axis.dot (proj)));
210 phi = cross.dot (normal) < 0.f ? (360.0f - phi) : phi;
212 Eigen::Vector3f no = neighbour - origin;
214 float theta = normal.dot (no);
215 theta =
pcl::rad2deg (acosf (std::min (1.0f, std::max (-1.0f, theta))));
223 for (
size_t rad = 1; rad < radius_bins_+1; rad++)
225 if (r <= radii_interval_[rad])
232 for (
size_t ang = 1; ang < elevation_bins_+1; ang++)
234 if (theta <= theta_divisions_[ang])
241 for (
size_t ang = 1; ang < azimuth_bins_+1; ang++)
243 if (phi <= phi_divisions_[ang])
251 std::vector<int> neighbour_indices;
252 std::vector<float> neighbour_distances;
253 int point_density = searchForNeighbors (*surface_, nn_indices[ne], point_density_radius_, neighbour_indices, neighbour_distances);
255 if (point_density == 0)
258 float w = (1.0f /
static_cast<float> (point_density)) *
259 volume_lut_[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j];
262 if (w == std::numeric_limits<float>::infinity ())
263 PCL_ERROR (
"Shape Context Error INF!\n");
265 PCL_ERROR (
"Shape Context Error IND!\n");
267 desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] += w;
269 assert (desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] >= 0);
273 memset (rf, 0,
sizeof (rf[0]) * 9);
278 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void 281 assert (descriptor_length_ == 1980);
285 for (
size_t point_index = 0; point_index < indices_->size (); point_index++)
290 if (!
isFinite ((*input_)[(*indices_)[point_index]]))
292 for (
size_t i = 0; i < descriptor_length_; ++i)
293 output[point_index].descriptor[i] = std::numeric_limits<float>::quiet_NaN ();
295 memset (output[point_index].rf, 0,
sizeof (output[point_index].rf[0]) * 9);
300 std::vector<float> descriptor (descriptor_length_);
301 if (!computePoint (point_index, *normals_, output[point_index].rf, descriptor))
303 for (
size_t j = 0; j < descriptor_length_; ++j)
304 output[point_index].descriptor[j] = descriptor[j];
308 #define PCL_INSTANTIATE_ShapeContext3DEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::ShapeContext3DEstimation<T,NT,OutT>; bool initCompute()
Initialize computation by allocating all the intervals and the volume lookup table.
void project(const PointT &point, const PointT &plane_origin, const NormalT &plane_normal, PointT &projected)
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...
float deg2rad(float alpha)
Convert an angle from degrees to radians.
Defines some geometrical functions and utility functions.
float rad2deg(float alpha)
Convert an angle from radians to degrees.
void computeFeature(PointCloudOut &output)
Estimate the actual feature.
bool computePoint(size_t index, const pcl::PointCloud< PointNT > &normals, float rf[9], std::vector< float > &desc)
Estimate a descriptor for a given point.
const Eigen::Map< const Eigen::Vector3f > Vector3fMapConst
bool equal(T val1, T val2, T eps=std::numeric_limits< T >::min())
Check if val1 and val2 are equals to an epsilon extent.
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values).
Define standard C methods to do angle calculations.