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> 45 #include <pcl/common/geometry.h> 46 #include <pcl/common/angles.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 = sqrtf (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.
float deg2rad(float alpha)
Convert an angle from degrees to radians.
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).