Point Cloud Library (PCL)  1.11.0-dev
rsd.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2009, Willow Garage, Inc.
6  * Copyright (c) 2012-, Open Perception, Inc.
7  *
8  * All rights reserved.
9  *
10  * Redistribution and use in source and binary forms, with or without
11  * modification, are permitted provided that the following conditions
12  * are met:
13  *
14  * * Redistributions of source code must retain the above copyright
15  * notice, this list of conditions and the following disclaimer.
16  * * Redistributions in binary form must reproduce the above
17  * copyright notice, this list of conditions and the following
18  * disclaimer in the documentation and/or other materials provided
19  * with the distribution.
20  * * Neither the name of the copyright holder(s) nor the names of its
21  * contributors may be used to endorse or promote products derived
22  * from this software without specific prior written permission.
23  *
24  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35  * POSSIBILITY OF SUCH DAMAGE.
36  *
37  * $Id$
38  *
39  */
40 
41 #ifndef PCL_FEATURES_IMPL_RSD_H_
42 #define PCL_FEATURES_IMPL_RSD_H_
43 
44 #include <cfloat>
45 #include <pcl/features/rsd.h>
46 
47 //////////////////////////////////////////////////////////////////////////////////////////////
48 template <typename PointInT, typename PointNT, typename PointOutT> Eigen::MatrixXf
50  const std::vector<int> &indices, double max_dist,
51  int nr_subdiv, double plane_radius, PointOutT &radii, bool compute_histogram)
52 {
53  // Check if the full histogram has to be saved or not
54  Eigen::MatrixXf histogram;
55  if (compute_histogram)
56  histogram = Eigen::MatrixXf::Zero (nr_subdiv, nr_subdiv);
57 
58  // Check if enough points are provided or not
59  if (indices.size () < 2)
60  {
61  radii.r_max = 0;
62  radii.r_min = 0;
63  return histogram;
64  }
65 
66  // Initialize minimum and maximum angle values in each distance bin
67  std::vector<std::vector<double> > min_max_angle_by_dist (nr_subdiv);
68  for (auto& minmax: min_max_angle_by_dist)
69  {
70  minmax.resize (2);
71  minmax[0] = std::numeric_limits<double>::max();
72  minmax[1] = -std::numeric_limits<double>::max();
73  }
74  min_max_angle_by_dist[0][0] = min_max_angle_by_dist[0][1] = 0.0;
75 
76  // Compute distance by normal angle distribution for points
77  std::vector<int>::const_iterator i, begin (indices.begin()), end (indices.end());
78  for (i = begin+1; i != end; ++i)
79  {
80  // compute angle between the two lines going through normals (disregard orientation!)
81  double cosine = normals.points[*i].normal[0] * normals.points[*begin].normal[0] +
82  normals.points[*i].normal[1] * normals.points[*begin].normal[1] +
83  normals.points[*i].normal[2] * normals.points[*begin].normal[2];
84  if (cosine > 1) cosine = 1;
85  if (cosine < -1) cosine = -1;
86  double angle = std::acos (cosine);
87  if (angle > M_PI/2) angle = M_PI - angle; /// \note: orientation is neglected!
88 
89  // Compute point to point distance
90  double dist = sqrt ((surface.points[*i].x - surface.points[*begin].x) * (surface.points[*i].x - surface.points[*begin].x) +
91  (surface.points[*i].y - surface.points[*begin].y) * (surface.points[*i].y - surface.points[*begin].y) +
92  (surface.points[*i].z - surface.points[*begin].z) * (surface.points[*i].z - surface.points[*begin].z));
93 
94  if (dist > max_dist)
95  continue; /// \note: we neglect points that are outside the specified interval!
96 
97  // compute bins and increase
98  int bin_d = static_cast<int> (std::floor (nr_subdiv * dist / max_dist));
99  if (compute_histogram)
100  {
101  int bin_a = std::min (nr_subdiv-1, static_cast<int> (std::floor (nr_subdiv * angle / (M_PI/2))));
102  histogram(bin_a, bin_d)++;
103  }
104 
105  // update min-max values for distance bins
106  min_max_angle_by_dist[bin_d][0] = std::min(angle, min_max_angle_by_dist[bin_d][0]);
107  min_max_angle_by_dist[bin_d][1] = std::max(angle, min_max_angle_by_dist[bin_d][1]);
108  }
109 
110  // Estimate radius from min and max lines
111  double Amint_Amin = 0, Amint_d = 0;
112  double Amaxt_Amax = 0, Amaxt_d = 0;
113  for (int di=0; di<nr_subdiv; di++)
114  {
115  // combute the members of A'*A*r = A'*D
116  if (min_max_angle_by_dist[di][1] >= 0)
117  {
118  double p_min = min_max_angle_by_dist[di][0];
119  double p_max = min_max_angle_by_dist[di][1];
120  double f = (di+0.5)*max_dist/nr_subdiv;
121  Amint_Amin += p_min * p_min;
122  Amint_d += p_min * f;
123  Amaxt_Amax += p_max * p_max;
124  Amaxt_d += p_max * f;
125  }
126  }
127  float min_radius = Amint_Amin == 0.0f ? float (plane_radius) : float (std::min (Amint_d/Amint_Amin, plane_radius));
128  float max_radius = Amaxt_Amax == 0.0f ? float (plane_radius) : float (std::min (Amaxt_d/Amaxt_Amax, plane_radius));
129 
130  // Small correction of the systematic error of the estimation (based on analysis with nr_subdiv_ = 5)
131  min_radius *= 1.1f;
132  max_radius *= 0.9f;
133  if (min_radius < max_radius)
134  {
135  radii.r_min = min_radius;
136  radii.r_max = max_radius;
137  }
138  else
139  {
140  radii.r_max = min_radius;
141  radii.r_min = max_radius;
142  }
143 
144  return histogram;
145 }
146 
147 //////////////////////////////////////////////////////////////////////////////////////////////
148 template <typename PointNT, typename PointOutT> Eigen::MatrixXf
150  const std::vector<int> &indices, const std::vector<float> &sqr_dists, double max_dist,
151  int nr_subdiv, double plane_radius, PointOutT &radii, bool compute_histogram)
152 {
153  // Check if the full histogram has to be saved or not
154  Eigen::MatrixXf histogram;
155  if (compute_histogram)
156  histogram = Eigen::MatrixXf::Zero (nr_subdiv, nr_subdiv);
157 
158  // Check if enough points are provided or not
159  if (indices.size () < 2)
160  {
161  radii.r_max = 0;
162  radii.r_min = 0;
163  return histogram;
164  }
165 
166  // Initialize minimum and maximum angle values in each distance bin
167  std::vector<std::vector<double> > min_max_angle_by_dist (nr_subdiv);
168  min_max_angle_by_dist[0].resize (2);
169  min_max_angle_by_dist[0][0] = min_max_angle_by_dist[0][1] = 0.0;
170  for (int di=1; di<nr_subdiv; di++)
171  {
172  min_max_angle_by_dist[di].resize (2);
173  min_max_angle_by_dist[di][0] = +DBL_MAX;
174  min_max_angle_by_dist[di][1] = -DBL_MAX;
175  }
176 
177  // Compute distance by normal angle distribution for points
178  std::vector<int>::const_iterator i, begin (indices.begin()), end (indices.end());
179  for (i = begin+1; i != end; ++i)
180  {
181  // compute angle between the two lines going through normals (disregard orientation!)
182  double cosine = normals.points[*i].normal[0] * normals.points[*begin].normal[0] +
183  normals.points[*i].normal[1] * normals.points[*begin].normal[1] +
184  normals.points[*i].normal[2] * normals.points[*begin].normal[2];
185  if (cosine > 1) cosine = 1;
186  if (cosine < -1) cosine = -1;
187  double angle = std::acos (cosine);
188  if (angle > M_PI/2) angle = M_PI - angle; /// \note: orientation is neglected!
189 
190  // Compute point to point distance
191  double dist = sqrt (sqr_dists[i-begin]);
192 
193  if (dist > max_dist)
194  continue; /// \note: we neglect points that are outside the specified interval!
195 
196  // compute bins and increase
197  int bin_d = static_cast<int> (std::floor (nr_subdiv * dist / max_dist));
198  if (compute_histogram)
199  {
200  int bin_a = std::min (nr_subdiv-1, static_cast<int> (std::floor (nr_subdiv * angle / (M_PI/2))));
201  histogram(bin_a, bin_d)++;
202  }
203 
204  // update min-max values for distance bins
205  if (min_max_angle_by_dist[bin_d][0] > angle) min_max_angle_by_dist[bin_d][0] = angle;
206  if (min_max_angle_by_dist[bin_d][1] < angle) min_max_angle_by_dist[bin_d][1] = angle;
207  }
208 
209  // Estimate radius from min and max lines
210  double Amint_Amin = 0, Amint_d = 0;
211  double Amaxt_Amax = 0, Amaxt_d = 0;
212  for (int di=0; di<nr_subdiv; di++)
213  {
214  // combute the members of A'*A*r = A'*D
215  if (min_max_angle_by_dist[di][1] >= 0)
216  {
217  double p_min = min_max_angle_by_dist[di][0];
218  double p_max = min_max_angle_by_dist[di][1];
219  double f = (di+0.5)*max_dist/nr_subdiv;
220  Amint_Amin += p_min * p_min;
221  Amint_d += p_min * f;
222  Amaxt_Amax += p_max * p_max;
223  Amaxt_d += p_max * f;
224  }
225  }
226  float min_radius = Amint_Amin == 0.0f ? float (plane_radius) : float (std::min (Amint_d/Amint_Amin, plane_radius));
227  float max_radius = Amaxt_Amax == 0.0f ? float (plane_radius) : float (std::min (Amaxt_d/Amaxt_Amax, plane_radius));
228 
229  // Small correction of the systematic error of the estimation (based on analysis with nr_subdiv_ = 5)
230  min_radius *= 1.1f;
231  max_radius *= 0.9f;
232  if (min_radius < max_radius)
233  {
234  radii.r_min = min_radius;
235  radii.r_max = max_radius;
236  }
237  else
238  {
239  radii.r_max = min_radius;
240  radii.r_min = max_radius;
241  }
242 
243  return histogram;
244 }
245 
246 //////////////////////////////////////////////////////////////////////////////////////////////
247 template <typename PointInT, typename PointNT, typename PointOutT> void
249 {
250  // Check if search_radius_ was set
251  if (search_radius_ < 0)
252  {
253  PCL_ERROR ("[pcl::%s::computeFeature] A search radius needs to be set!\n", getClassName ().c_str ());
254  output.width = output.height = 0;
255  output.points.clear ();
256  return;
257  }
258 
259  // List of indices and corresponding squared distances for a neighborhood
260  // \note resize is irrelevant for a radiusSearch ().
261  std::vector<int> nn_indices;
262  std::vector<float> nn_sqr_dists;
263 
264  // Check if the full histogram has to be saved or not
265  if (save_histograms_)
266  {
267  // Reserve space for the output histogram dataset
268  histograms_.reset (new std::vector<Eigen::MatrixXf, Eigen::aligned_allocator<Eigen::MatrixXf> >);
269  histograms_->reserve (output.points.size ());
270 
271  // Iterating over the entire index vector
272  for (std::size_t idx = 0; idx < indices_->size (); ++idx)
273  {
274  // Compute and store r_min and r_max in the output cloud
275  this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_sqr_dists);
276  //histograms_->push_back (computeRSD (*surface_, *normals_, nn_indices, search_radius_, nr_subdiv_, plane_radius_, output.points[idx], true));
277  histograms_->push_back (computeRSD (*normals_, nn_indices, nn_sqr_dists, search_radius_, nr_subdiv_, plane_radius_, output.points[idx], true));
278  }
279  }
280  else
281  {
282  // Iterating over the entire index vector
283  for (std::size_t idx = 0; idx < indices_->size (); ++idx)
284  {
285  // Compute and store r_min and r_max in the output cloud
286  this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_sqr_dists);
287  //computeRSD (*surface_, *normals_, nn_indices, search_radius_, nr_subdiv_, plane_radius_, output.points[idx], false);
288  computeRSD (*normals_, nn_indices, nn_sqr_dists, search_radius_, nr_subdiv_, plane_radius_, output.points[idx], false);
289  }
290  }
291 }
292 
293 #define PCL_INSTANTIATE_RSDEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::RSDEstimation<T,NT,OutT>;
294 
295 #endif // PCL_FEATURES_IMPL_RSD_H_
pcl::PointCloud::height
std::uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:416
pcl::PointCloud::points
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:411
pcl::RSDEstimation::computeFeature
void computeFeature(PointCloudOut &output) override
Estimate the estimates the Radius-based Surface Descriptor (RSD) at a set of points given by <setInpu...
Definition: rsd.hpp:248
pcl::PointCloud< PointInT >
pcl::PointCloud::width
std::uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:414
M_PI
#define M_PI
Definition: pcl_macros.h:195
pcl::computeRSD
Eigen::MatrixXf computeRSD(const pcl::PointCloud< PointInT > &surface, 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...
Definition: rsd.hpp:49