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 (std::size_t j = 0; j < radius_bins_ + 1; j++)
80 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_))));
83 theta_divisions_.resize (elevation_bins_ + 1, elevation_interval);
84 theta_divisions_[0] = 0.f;
85 std::partial_sum(theta_divisions_.begin (), theta_divisions_.end (), theta_divisions_.begin ());
88 phi_divisions_.resize (azimuth_bins_ + 1, azimuth_interval);
89 phi_divisions_[0] = 0.f;
90 std::partial_sum(phi_divisions_.begin (), phi_divisions_.end (), phi_divisions_.begin ());
97 float e = 1.0f / 3.0f;
99 volume_lut_.resize (radius_bins_ * elevation_bins_ * azimuth_bins_);
101 for (std::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 (std::size_t k = 0; k < elevation_bins_; k++)
109 float integr_theta = std::cos (
pcl::deg2rad (theta_divisions_[k])) - std::cos (
pcl::deg2rad (theta_divisions_[k+1]));
111 float V = integr_phi * integr_theta * integr_r;
117 for (std::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 std::size_t neighb_cnt = searchForNeighbors ((*indices_)[index], search_radius_, nn_indices, nn_dists);
144 std::fill (desc.begin (), desc.end (), std::numeric_limits<float>::quiet_NaN ());
145 std::fill (rf, rf + 9, 0.f);
149 const auto minDistanceIt = std::min_element(nn_dists.begin (), nn_dists.end ());
150 const auto minIndex = nn_indices[
std::distance (nn_dists.begin (), minDistanceIt)];
153 Vector3fMapConst origin = input_->points[(*indices_)[index]].getVector3fMap ();
156 normal = normals[minIndex].getNormalVector3fMap ();
163 x_axis[2] = - (normal[0]*x_axis[0] + normal[1]*x_axis[1]) / normal[2];
165 x_axis[1] = - (normal[0]*x_axis[0] + normal[2]*x_axis[2]) / normal[1];
167 x_axis[0] = - (normal[1]*x_axis[1] + normal[2]*x_axis[2]) / normal[0];
172 assert (
pcl::utils::equal (x_axis[0]*normal[0] + x_axis[1]*normal[1] + x_axis[2]*normal[2], 0.0f, 1E-6f));
175 y_axis.matrix () = normal.cross (x_axis);
178 for (std::size_t ne = 0; ne < neighb_cnt; ne++)
183 Eigen::Vector3f neighbour = surface_->points[nn_indices[ne]].getVector3fMap ();
187 float r = std::sqrt (nn_dists[ne]);
190 Eigen::Vector3f proj;
198 Eigen::Vector3f cross = x_axis.cross (proj);
199 float phi =
pcl::rad2deg (std::atan2 (cross.norm (), x_axis.dot (proj)));
200 phi = cross.dot (normal) < 0.f ? (360.0f - phi) : phi;
202 Eigen::Vector3f no = neighbour - origin;
204 float theta = normal.dot (no);
205 theta =
pcl::rad2deg (std::acos (std::min (1.0f, std::max (-1.0f, theta))));
208 const auto rad_min = std::lower_bound(std::next (radii_interval_.cbegin ()), radii_interval_.cend (), r);
209 const auto theta_min = std::lower_bound(std::next (theta_divisions_.cbegin ()), theta_divisions_.cend (), theta);
210 const auto phi_min = std::lower_bound(std::next (phi_divisions_.cbegin ()), phi_divisions_.cend (), phi);
213 const auto j =
std::distance(radii_interval_.cbegin (), std::prev(rad_min));
214 const auto k =
std::distance(theta_divisions_.cbegin (), std::prev(theta_min));
215 const auto l =
std::distance(phi_divisions_.cbegin (), std::prev(phi_min));
218 std::vector<int> neighbour_indices;
219 std::vector<float> neighbour_distances;
220 int point_density = searchForNeighbors (*surface_, nn_indices[ne], point_density_radius_, neighbour_indices, neighbour_distances);
222 if (point_density == 0)
225 float w = (1.0f / static_cast<float> (point_density)) *
226 volume_lut_[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j];
229 if (w == std::numeric_limits<float>::infinity ())
230 PCL_ERROR (
"Shape Context Error INF!\n");
232 PCL_ERROR (
"Shape Context Error IND!\n");
234 desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] += w;
236 assert (desc[(l*elevation_bins_*radius_bins_) + (k*radius_bins_) + j] >= 0);
240 std::fill (rf, rf + 9, 0);
245 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
248 assert (descriptor_length_ == 1980);
250 output.is_dense =
true;
252 for (std::size_t point_index = 0; point_index < indices_->size (); point_index++)
257 if (!
isFinite ((*input_)[(*indices_)[point_index]]))
259 std::fill (output[point_index].descriptor, output[point_index].descriptor + descriptor_length_,
260 std::numeric_limits<float>::quiet_NaN ());
261 std::fill (output[point_index].rf, output[point_index].rf + 9, 0);
262 output.is_dense =
false;
266 std::vector<float> descriptor (descriptor_length_);
267 if (!computePoint (point_index, *normals_, output[point_index].rf, descriptor))
268 output.is_dense =
false;
269 std::copy (descriptor.begin (), descriptor.end (), output[point_index].descriptor);
273 #define PCL_INSTANTIATE_ShapeContext3DEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::ShapeContext3DEstimation<T,NT,OutT>;