Point Cloud Library (PCL)  1.8.1-dev
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
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
49 pcl::computeRSD (boost::shared_ptr<const pcl::PointCloud<PointInT> > &surface, boost::shared_ptr<const pcl::PointCloud<PointNT> > &normals,
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  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++)
71  {
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;
75  }
76 
77  // Compute distance by normal angle distribution for points
78  std::vector<int>::const_iterator i, begin (indices.begin()), end (indices.end());
79  for(i = begin+1; i != end; ++i)
80  {
81  // compute angle between the two lines going through normals (disregard orientation!)
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; /// \note: orientation is neglected!
89 
90  // Compute point to point distance
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));
94 
95  if (dist > max_dist)
96  continue; /// \note: we neglect points that are outside the specified interval!
97 
98  // compute bins and increase
99  int bin_d = static_cast<int> (floor (nr_subdiv * dist / max_dist));
100  if (compute_histogram)
101  {
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)++;
104  }
105 
106  // update min-max values for distance bins
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;
109  }
110 
111  // Estimate radius from min and max lines
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++)
115  {
116  // combute the members of A'*A*r = A'*D
117  if (min_max_angle_by_dist[di][1] >= 0)
118  {
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;
126  }
127  }
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));
130 
131  // Small correction of the systematic error of the estimation (based on analysis with nr_subdiv_ = 5)
132  min_radius *= 1.1f;
133  max_radius *= 0.9f;
134  if (min_radius < max_radius)
135  {
136  radii.r_min = min_radius;
137  radii.r_max = max_radius;
138  }
139  else
140  {
141  radii.r_max = min_radius;
142  radii.r_min = max_radius;
143  }
144 
145  return histogram;
146 }
147 
148 //////////////////////////////////////////////////////////////////////////////////////////////
149 template <typename PointNT, typename PointOutT> Eigen::MatrixXf
150 pcl::computeRSD (boost::shared_ptr<const pcl::PointCloud<PointNT> > &normals,
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)
153 {
154  // Check if the full histogram has to be saved or not
155  Eigen::MatrixXf histogram;
156  if (compute_histogram)
157  histogram = Eigen::MatrixXf::Zero (nr_subdiv, nr_subdiv);
158 
159  // Check if enough points are provided or not
160  if (indices.size () < 2)
161  {
162  radii.r_max = 0;
163  radii.r_min = 0;
164  return histogram;
165  }
166 
167  // Initialize minimum and maximum angle values in each distance bin
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++)
172  {
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;
176  }
177 
178  // Compute distance by normal angle distribution for points
179  std::vector<int>::const_iterator i, begin (indices.begin()), end (indices.end());
180  for(i = begin+1; i != end; ++i)
181  {
182  // compute angle between the two lines going through normals (disregard orientation!)
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; /// \note: orientation is neglected!
190 
191  // Compute point to point distance
192  double dist = sqrt (sqr_dists[i-begin]);
193 
194  if (dist > max_dist)
195  continue; /// \note: we neglect points that are outside the specified interval!
196 
197  // compute bins and increase
198  int bin_d = static_cast<int> (floor (nr_subdiv * dist / max_dist));
199  if (compute_histogram)
200  {
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)++;
203  }
204 
205  // update min-max values for distance bins
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;
208  }
209 
210  // Estimate radius from min and max lines
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++)
214  {
215  // combute the members of A'*A*r = A'*D
216  if (min_max_angle_by_dist[di][1] >= 0)
217  {
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;
225  }
226  }
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));
229 
230  // Small correction of the systematic error of the estimation (based on analysis with nr_subdiv_ = 5)
231  min_radius *= 1.1f;
232  max_radius *= 0.9f;
233  if (min_radius < max_radius)
234  {
235  radii.r_min = min_radius;
236  radii.r_max = max_radius;
237  }
238  else
239  {
240  radii.r_max = min_radius;
241  radii.r_min = max_radius;
242  }
243 
244  return histogram;
245 }
246 
247 //////////////////////////////////////////////////////////////////////////////////////////////
248 template <typename PointInT, typename PointNT, typename PointOutT> void
250 {
251  // Check if search_radius_ was set
252  if (search_radius_ < 0)
253  {
254  PCL_ERROR ("[pcl::%s::computeFeature] A search radius needs to be set!\n", getClassName ().c_str ());
255  output.width = output.height = 0;
256  output.points.clear ();
257  return;
258  }
259 
260  // List of indices and corresponding squared distances for a neighborhood
261  // \note resize is irrelevant for a radiusSearch ().
262  std::vector<int> nn_indices;
263  std::vector<float> nn_sqr_dists;
264 
265  // Check if the full histogram has to be saved or not
266  if (save_histograms_)
267  {
268  // Reserve space for the output histogram dataset
269  histograms_.reset (new std::vector<Eigen::MatrixXf, Eigen::aligned_allocator<Eigen::MatrixXf> >);
270  histograms_->reserve (output.points.size ());
271 
272  // Iterating over the entire index vector
273  for (size_t idx = 0; idx < indices_->size (); ++idx)
274  {
275  // Compute and store r_min and r_max in the output cloud
276  this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_sqr_dists);
277  //histograms_->push_back (computeRSD (surface_, normals_, nn_indices, search_radius_, nr_subdiv_, plane_radius_, output.points[idx], true));
278  histograms_->push_back (computeRSD (normals_, nn_indices, nn_sqr_dists, search_radius_, nr_subdiv_, plane_radius_, output.points[idx], true));
279  }
280  }
281  else
282  {
283  // Iterating over the entire index vector
284  for (size_t idx = 0; idx < indices_->size (); ++idx)
285  {
286  // Compute and store r_min and r_max in the output cloud
287  this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_sqr_dists);
288  //computeRSD (surface_, normals_, nn_indices, search_radius_, nr_subdiv_, plane_radius_, output.points[idx], false);
289  computeRSD (normals_, nn_indices, nn_sqr_dists, search_radius_, nr_subdiv_, plane_radius_, output.points[idx], false);
290  }
291  }
292 }
293 
294 #define PCL_INSTANTIATE_RSDEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::RSDEstimation<T,NT,OutT>;
295 
296 #endif // PCL_FEATURES_IMPL_RSD_H_
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...
Definition: rsd.hpp:49
void computeFeature(PointCloudOut &output)
Estimate the estimates the Radius-based Surface Descriptor (RSD) at a set of points given by <setInpu...
Definition: rsd.hpp:249
uint32_t width
The point cloud width (if organized as an image-structure).
Definition: point_cloud.h:413
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
uint32_t height
The point cloud height (if organized as an image-structure).
Definition: point_cloud.h:415