41 #ifndef PCL_FEATURES_IMPL_RSD_H_
42 #define PCL_FEATURES_IMPL_RSD_H_
45 #include <pcl/features/rsd.h>
48 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT> Eigen::MatrixXf
50 const std::vector<int> &indices,
double max_dist,
51 int nr_subdiv,
double plane_radius, PointOutT &radii,
bool compute_histogram)
54 Eigen::MatrixXf histogram;
55 if (compute_histogram)
56 histogram = Eigen::MatrixXf::Zero (nr_subdiv, nr_subdiv);
59 if (indices.size () < 2)
67 std::vector<std::vector<double> > min_max_angle_by_dist (nr_subdiv);
68 min_max_angle_by_dist[0].resize (2);
69 min_max_angle_by_dist[0][0] = min_max_angle_by_dist[0][1] = 0.0;
70 for (
int di=1; di<nr_subdiv; di++)
72 min_max_angle_by_dist[di].resize (2);
73 min_max_angle_by_dist[di][0] = +DBL_MAX;
74 min_max_angle_by_dist[di][1] = -DBL_MAX;
78 std::vector<int>::const_iterator i, begin (indices.begin()), end (indices.end());
79 for(i = begin+1; i != end; ++i)
82 double cosine = normals->points[*i].normal[0] * normals->points[*begin].normal[0] +
83 normals->points[*i].normal[1] * normals->points[*begin].normal[1] +
84 normals->points[*i].normal[2] * normals->points[*begin].normal[2];
85 if (cosine > 1) cosine = 1;
86 if (cosine < -1) cosine = -1;
87 double angle = acos (cosine);
88 if (angle > M_PI/2) angle = M_PI - angle;
91 double dist = sqrt ((surface->points[*i].x - surface->points[*begin].x) * (surface->points[*i].x - surface->points[*begin].x) +
92 (surface->points[*i].y - surface->points[*begin].y) * (surface->points[*i].y - surface->points[*begin].y) +
93 (surface->points[*i].z - surface->points[*begin].z) * (surface->points[*i].z - surface->points[*begin].z));
99 int bin_d =
static_cast<int> (floor (nr_subdiv * dist / max_dist));
100 if (compute_histogram)
102 int bin_a = std::min (nr_subdiv-1, static_cast<int> (floor (nr_subdiv * angle / (M_PI/2))));
103 histogram(bin_a, bin_d)++;
107 if (min_max_angle_by_dist[bin_d][0] > angle) min_max_angle_by_dist[bin_d][0] = angle;
108 if (min_max_angle_by_dist[bin_d][1] < angle) min_max_angle_by_dist[bin_d][1] = angle;
112 double Amint_Amin = 0, Amint_d = 0;
113 double Amaxt_Amax = 0, Amaxt_d = 0;
114 for (
int di=0; di<nr_subdiv; di++)
117 if (min_max_angle_by_dist[di][1] >= 0)
119 double p_min = min_max_angle_by_dist[di][0];
120 double p_max = min_max_angle_by_dist[di][1];
121 double f = (di+0.5)*max_dist/nr_subdiv;
122 Amint_Amin += p_min * p_min;
123 Amint_d += p_min * f;
124 Amaxt_Amax += p_max * p_max;
125 Amaxt_d += p_max * f;
128 float min_radius = Amint_Amin == 0.0f ? float (plane_radius) : float (std::min (Amint_d/Amint_Amin, plane_radius));
129 float max_radius = Amaxt_Amax == 0.0f ? float (plane_radius) : float (std::min (Amaxt_d/Amaxt_Amax, plane_radius));
134 if (min_radius < max_radius)
136 radii.r_min = min_radius;
137 radii.r_max = max_radius;
141 radii.r_max = min_radius;
142 radii.r_min = max_radius;
149 template <
typename Po
intNT,
typename Po
intOutT> Eigen::MatrixXf
151 const std::vector<int> &indices,
const std::vector<float> &sqr_dists,
double max_dist,
152 int nr_subdiv,
double plane_radius, PointOutT &radii,
bool compute_histogram)
155 Eigen::MatrixXf histogram;
156 if (compute_histogram)
157 histogram = Eigen::MatrixXf::Zero (nr_subdiv, nr_subdiv);
160 if (indices.size () < 2)
168 std::vector<std::vector<double> > min_max_angle_by_dist (nr_subdiv);
169 min_max_angle_by_dist[0].resize (2);
170 min_max_angle_by_dist[0][0] = min_max_angle_by_dist[0][1] = 0.0;
171 for (
int di=1; di<nr_subdiv; di++)
173 min_max_angle_by_dist[di].resize (2);
174 min_max_angle_by_dist[di][0] = +DBL_MAX;
175 min_max_angle_by_dist[di][1] = -DBL_MAX;
179 std::vector<int>::const_iterator i, begin (indices.begin()), end (indices.end());
180 for(i = begin+1; i != end; ++i)
183 double cosine = normals->points[*i].normal[0] * normals->points[*begin].normal[0] +
184 normals->points[*i].normal[1] * normals->points[*begin].normal[1] +
185 normals->points[*i].normal[2] * normals->points[*begin].normal[2];
186 if (cosine > 1) cosine = 1;
187 if (cosine < -1) cosine = -1;
188 double angle = acos (cosine);
189 if (angle > M_PI/2) angle = M_PI - angle;
192 double dist = sqrt (sqr_dists[i-begin]);
198 int bin_d =
static_cast<int> (floor (nr_subdiv * dist / max_dist));
199 if (compute_histogram)
201 int bin_a = std::min (nr_subdiv-1, static_cast<int> (floor (nr_subdiv * angle / (M_PI/2))));
202 histogram(bin_a, bin_d)++;
206 if (min_max_angle_by_dist[bin_d][0] > angle) min_max_angle_by_dist[bin_d][0] = angle;
207 if (min_max_angle_by_dist[bin_d][1] < angle) min_max_angle_by_dist[bin_d][1] = angle;
211 double Amint_Amin = 0, Amint_d = 0;
212 double Amaxt_Amax = 0, Amaxt_d = 0;
213 for (
int di=0; di<nr_subdiv; di++)
216 if (min_max_angle_by_dist[di][1] >= 0)
218 double p_min = min_max_angle_by_dist[di][0];
219 double p_max = min_max_angle_by_dist[di][1];
220 double f = (di+0.5)*max_dist/nr_subdiv;
221 Amint_Amin += p_min * p_min;
222 Amint_d += p_min * f;
223 Amaxt_Amax += p_max * p_max;
224 Amaxt_d += p_max * f;
227 float min_radius = Amint_Amin == 0.0f ? float (plane_radius) : float (std::min (Amint_d/Amint_Amin, plane_radius));
228 float max_radius = Amaxt_Amax == 0.0f ? float (plane_radius) : float (std::min (Amaxt_d/Amaxt_Amax, plane_radius));
233 if (min_radius < max_radius)
235 radii.r_min = min_radius;
236 radii.r_max = max_radius;
240 radii.r_max = min_radius;
241 radii.r_min = max_radius;
248 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
252 if (search_radius_ < 0)
254 PCL_ERROR (
"[pcl::%s::computeFeature] A search radius needs to be set!\n", getClassName ().c_str ());
262 std::vector<int> nn_indices;
263 std::vector<float> nn_sqr_dists;
266 if (save_histograms_)
269 histograms_.reset (
new std::vector<Eigen::MatrixXf, Eigen::aligned_allocator<Eigen::MatrixXf> >);
270 histograms_->reserve (output.
points.size ());
273 for (
size_t idx = 0; idx < indices_->size (); ++idx)
276 this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_sqr_dists);
278 histograms_->push_back (
computeRSD (normals_, nn_indices, nn_sqr_dists, search_radius_, nr_subdiv_, plane_radius_, output.
points[idx],
true));
284 for (
size_t idx = 0; idx < indices_->size (); ++idx)
287 this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_sqr_dists);
289 computeRSD (normals_, nn_indices, nn_sqr_dists, search_radius_, nr_subdiv_, plane_radius_, output.
points[idx],
false);
294 #define PCL_INSTANTIATE_RSDEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::RSDEstimation<T,NT,OutT>;
296 #endif // PCL_FEATURES_IMPL_VFH_H_
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Eigen::MatrixXf computeRSD(boost::shared_ptr< const pcl::PointCloud< PointInT > > &surface, boost::shared_ptr< const pcl::PointCloud< PointNT > > &normals, const std::vector< int > &indices, double max_dist, int nr_subdiv, double plane_radius, PointOutT &radii, bool compute_histogram=false)
Estimate the Radius-based Surface Descriptor (RSD) for a given point based on its spatial neighborhoo...
void computeFeature(PointCloudOut &output)
Estimate the estimates the Radius-based Surface Descriptor (RSD) at a set of points given by
uint32_t height
The point cloud height (if organized as an image-structure).
uint32_t width
The point cloud width (if organized as an image-structure).