Point Cloud Library (PCL)  1.8.1
spin_image.hpp
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2010-2012, 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_SPIN_IMAGE_H_
42 #define PCL_FEATURES_IMPL_SPIN_IMAGE_H_
43 
44 #include <limits>
45 #include <pcl/point_cloud.h>
46 #include <pcl/point_types.h>
47 #include <pcl/exceptions.h>
48 #include <pcl/kdtree/kdtree_flann.h>
49 #include <pcl/features/spin_image.h>
50 #include <cmath>
51 
52 //////////////////////////////////////////////////////////////////////////////////////////////
53 template <typename PointInT, typename PointNT, typename PointOutT>
55  unsigned int image_width, double support_angle_cos, unsigned int min_pts_neighb) :
56  input_normals_ (), rotation_axes_cloud_ (),
57  is_angular_ (false), rotation_axis_ (), use_custom_axis_(false), use_custom_axes_cloud_ (false),
58  is_radial_ (false), image_width_ (image_width), support_angle_cos_ (support_angle_cos),
59  min_pts_neighb_ (min_pts_neighb)
60 {
61  assert (support_angle_cos_ <= 1.0 && support_angle_cos_ >= 0.0); // may be permit negative cosine?
62 
63  feature_name_ = "SpinImageEstimation";
64 }
65 
66 
67 //////////////////////////////////////////////////////////////////////////////////////////////
68 template <typename PointInT, typename PointNT, typename PointOutT> Eigen::ArrayXXd
70 {
71  assert (image_width_ > 0);
72  assert (support_angle_cos_ <= 1.0 && support_angle_cos_ >= 0.0); // may be permit negative cosine?
73 
74  const Eigen::Vector3f origin_point (input_->points[index].getVector3fMap ());
75 
76  Eigen::Vector3f origin_normal;
77  origin_normal =
78  input_normals_ ?
79  input_normals_->points[index].getNormalVector3fMap () :
80  Eigen::Vector3f (); // just a placeholder; should never be used!
81 
82  const Eigen::Vector3f rotation_axis = use_custom_axis_ ?
83  rotation_axis_.getNormalVector3fMap () :
84  use_custom_axes_cloud_ ?
85  rotation_axes_cloud_->points[index].getNormalVector3fMap () :
86  origin_normal;
87 
88  Eigen::ArrayXXd m_matrix (Eigen::ArrayXXd::Zero (image_width_+1, 2*image_width_+1));
89  Eigen::ArrayXXd m_averAngles (Eigen::ArrayXXd::Zero (image_width_+1, 2*image_width_+1));
90 
91  // OK, we are interested in the points of the cylinder of height 2*r and
92  // base radius r, where r = m_dBinSize * in_iImageWidth
93  // it can be embedded to the sphere of radius sqrt(2) * m_dBinSize * in_iImageWidth
94  // suppose that points are uniformly distributed, so we lose ~40%
95  // according to the volumes ratio
96  double bin_size = 0.0;
97  if (is_radial_)
98  bin_size = search_radius_ / image_width_;
99  else
100  bin_size = search_radius_ / image_width_ / sqrt(2.0);
101 
102  std::vector<int> nn_indices;
103  std::vector<float> nn_sqr_dists;
104  const int neighb_cnt = this->searchForNeighbors (index, search_radius_, nn_indices, nn_sqr_dists);
105  if (neighb_cnt < static_cast<int> (min_pts_neighb_))
106  {
107  throw PCLException (
108  "Too few points for spin image, use setMinPointCountInNeighbourhood() to decrease the threshold or use larger feature radius",
109  "spin_image.hpp", "computeSiForPoint");
110  }
111 
112  // for all neighbor points
113  for (int i_neigh = 0; i_neigh < neighb_cnt ; i_neigh++)
114  {
115  // first, skip the points with distant normals
116  double cos_between_normals = -2.0; // should be initialized if used
117  if (support_angle_cos_ > 0.0 || is_angular_) // not bogus
118  {
119  cos_between_normals = origin_normal.dot (input_normals_->points[nn_indices[i_neigh]].getNormalVector3fMap ());
120  if (fabs (cos_between_normals) > (1.0 + 10*std::numeric_limits<float>::epsilon ())) // should be okay for numeric stability
121  {
122  PCL_ERROR ("[pcl::%s::computeSiForPoint] Normal for the point %d and/or the point %d are not normalized, dot ptoduct is %f.\n",
123  getClassName ().c_str (), nn_indices[i_neigh], index, cos_between_normals);
124  throw PCLException ("Some normals are not normalized",
125  "spin_image.hpp", "computeSiForPoint");
126  }
127  cos_between_normals = std::max (-1.0, std::min (1.0, cos_between_normals));
128 
129  if (fabs (cos_between_normals) < support_angle_cos_ ) // allow counter-directed normals
130  {
131  continue;
132  }
133 
134  if (cos_between_normals < 0.0)
135  {
136  cos_between_normals = -cos_between_normals; // the normal is not used explicitly from now
137  }
138  }
139 
140  // now compute the coordinate in cylindric coordinate system associated with the origin point
141  const Eigen::Vector3f direction (
142  surface_->points[nn_indices[i_neigh]].getVector3fMap () - origin_point);
143  const double direction_norm = direction.norm ();
144  if (fabs(direction_norm) < 10*std::numeric_limits<double>::epsilon ())
145  continue; // ignore the point itself; it does not contribute really
146  assert (direction_norm > 0.0);
147 
148  // the angle between the normal vector and the direction to the point
149  double cos_dir_axis = direction.dot(rotation_axis) / direction_norm;
150  if (fabs(cos_dir_axis) > (1.0 + 10*std::numeric_limits<float>::epsilon())) // should be okay for numeric stability
151  {
152  PCL_ERROR ("[pcl::%s::computeSiForPoint] Rotation axis for the point %d are not normalized, dot ptoduct is %f.\n",
153  getClassName ().c_str (), index, cos_dir_axis);
154  throw PCLException ("Some rotation axis is not normalized",
155  "spin_image.hpp", "computeSiForPoint");
156  }
157  cos_dir_axis = std::max (-1.0, std::min (1.0, cos_dir_axis));
158 
159  // compute coordinates w.r.t. the reference frame
160  double beta = std::numeric_limits<double>::signaling_NaN ();
161  double alpha = std::numeric_limits<double>::signaling_NaN ();
162  if (is_radial_) // radial spin image structure
163  {
164  beta = asin (cos_dir_axis); // yes, arc sine! to get the angle against tangent, not normal!
165  alpha = direction_norm;
166  }
167  else // rectangular spin-image structure
168  {
169  beta = direction_norm * cos_dir_axis;
170  alpha = direction_norm * sqrt (1.0 - cos_dir_axis*cos_dir_axis);
171 
172  if (fabs (beta) >= bin_size * image_width_ || alpha >= bin_size * image_width_)
173  {
174  continue; // outside the cylinder
175  }
176  }
177 
178  assert (alpha >= 0.0);
179  assert (alpha <= bin_size * image_width_ + 20 * std::numeric_limits<float>::epsilon () );
180 
181 
182  // bilinear interpolation
183  double beta_bin_size = is_radial_ ? (M_PI / 2 / image_width_) : bin_size;
184  int beta_bin = int(std::floor (beta / beta_bin_size)) + int(image_width_);
185  assert (0 <= beta_bin && beta_bin < m_matrix.cols ());
186  int alpha_bin = int(std::floor (alpha / bin_size));
187  assert (0 <= alpha_bin && alpha_bin < m_matrix.rows ());
188 
189  if (alpha_bin == static_cast<int> (image_width_)) // border points
190  {
191  alpha_bin--;
192  // HACK: to prevent a > 1
193  alpha = bin_size * (alpha_bin + 1) - std::numeric_limits<double>::epsilon ();
194  }
195  if (beta_bin == int(2*image_width_) ) // border points
196  {
197  beta_bin--;
198  // HACK: to prevent b > 1
199  beta = beta_bin_size * (beta_bin - int(image_width_) + 1) - std::numeric_limits<double>::epsilon ();
200  }
201 
202  double a = alpha/bin_size - double(alpha_bin);
203  double b = beta/beta_bin_size - double(beta_bin-int(image_width_));
204 
205  assert (0 <= a && a <= 1);
206  assert (0 <= b && b <= 1);
207 
208  m_matrix (alpha_bin, beta_bin) += (1-a) * (1-b);
209  m_matrix (alpha_bin+1, beta_bin) += a * (1-b);
210  m_matrix (alpha_bin, beta_bin+1) += (1-a) * b;
211  m_matrix (alpha_bin+1, beta_bin+1) += a * b;
212 
213  if (is_angular_)
214  {
215  m_averAngles (alpha_bin, beta_bin) += (1-a) * (1-b) * acos (cos_between_normals);
216  m_averAngles (alpha_bin+1, beta_bin) += a * (1-b) * acos (cos_between_normals);
217  m_averAngles (alpha_bin, beta_bin+1) += (1-a) * b * acos (cos_between_normals);
218  m_averAngles (alpha_bin+1, beta_bin+1) += a * b * acos (cos_between_normals);
219  }
220  }
221 
222  if (is_angular_)
223  {
224  // transform sum to average
225  m_matrix = m_averAngles / (m_matrix + std::numeric_limits<double>::epsilon ()); // +eps to avoid division by zero
226  }
227  else if (neighb_cnt > 1) // to avoid division by zero, also no need to divide by 1
228  {
229  // normalization
230  m_matrix /= m_matrix.sum();
231  }
232 
233  return m_matrix;
234 }
235 
236 
237 //////////////////////////////////////////////////////////////////////////////////////////////
238 template <typename PointInT, typename PointNT, typename PointOutT> bool
240 {
242  {
243  PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
244  return (false);
245  }
246 
247  // Check if input normals are set
248  if (!input_normals_)
249  {
250  PCL_ERROR ("[pcl::%s::initCompute] No input dataset containing normals was given!\n", getClassName ().c_str ());
252  return (false);
253  }
254 
255  // Check if the size of normals is the same as the size of the surface
256  if (input_normals_->points.size () != input_->points.size ())
257  {
258  PCL_ERROR ("[pcl::%s::initCompute] ", getClassName ().c_str ());
259  PCL_ERROR ("The number of points in the input dataset differs from ");
260  PCL_ERROR ("the number of points in the dataset containing the normals!\n");
262  return (false);
263  }
264 
265  // We need a positive definite search radius to continue
266  if (search_radius_ == 0)
267  {
268  PCL_ERROR ("[pcl::%s::initCompute] Need a search radius different than 0!\n", getClassName ().c_str ());
270  return (false);
271  }
272  if (k_ != 0)
273  {
274  PCL_ERROR ("[pcl::%s::initCompute] K-nearest neighbor search for spin images not implemented. Used a search radius instead!\n", getClassName ().c_str ());
276  return (false);
277  }
278  // If the surface won't be set, make fake surface and fake surface normals
279  // if we wouldn't do it here, the following method would alarm that no surface normals is given
280  if (!surface_)
281  {
282  surface_ = input_;
283  fake_surface_ = true;
284  }
285 
286  //if (fake_surface_ && !input_normals_)
287  // input_normals_ = normals_; // normals_ is set, as checked earlier
288 
289  assert(!(use_custom_axis_ && use_custom_axes_cloud_));
290 
291  if (!use_custom_axis_ && !use_custom_axes_cloud_ // use input normals as rotation axes
292  && !input_normals_)
293  {
294  PCL_ERROR ("[pcl::%s::initCompute] No normals for input cloud were given!\n", getClassName ().c_str ());
295  // Cleanup
297  return (false);
298  }
299 
300  if ((is_angular_ || support_angle_cos_ > 0.0) // support angle is not bogus NOTE this is for randomly-flipped normals
301  && !input_normals_)
302  {
303  PCL_ERROR ("[pcl::%s::initCompute] No normals for input cloud were given!\n", getClassName ().c_str ());
304  // Cleanup
306  return (false);
307  }
308 
309  if (use_custom_axes_cloud_
310  && rotation_axes_cloud_->size () == input_->size ())
311  {
312  PCL_ERROR ("[pcl::%s::initCompute] Rotation axis cloud have different size from input!\n", getClassName ().c_str ());
313  // Cleanup
315  return (false);
316  }
317 
318  return (true);
319 }
320 
321 
322 //////////////////////////////////////////////////////////////////////////////////////////////
323 template <typename PointInT, typename PointNT, typename PointOutT> void
325 {
326  for (int i_input = 0; i_input < static_cast<int> (indices_->size ()); ++i_input)
327  {
328  Eigen::ArrayXXd res = computeSiForPoint (indices_->at (i_input));
329 
330  // Copy into the resultant cloud
331  for (int iRow = 0; iRow < res.rows () ; iRow++)
332  {
333  for (int iCol = 0; iCol < res.cols () ; iCol++)
334  {
335  output.points[i_input].histogram[ iRow*res.cols () + iCol ] = static_cast<float> (res (iRow, iCol));
336  }
337  }
338  }
339 }
340 
341 #define PCL_INSTANTIATE_SpinImageEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::SpinImageEstimation<T,NT,OutT>;
342 
343 #endif // PCL_FEATURES_IMPL_SPIN_IMAGE_H_
344 
SpinImageEstimation(unsigned int image_width=8, double support_angle_cos=0.0, unsigned int min_pts_neighb=0)
Constructs empty spin image estimator.
Definition: spin_image.hpp:54
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
Definition: point_cloud.h:410
virtual bool initCompute()
initializes computations specific to spin-image.
Definition: spin_image.hpp:239
A base class for all pcl exceptions which inherits from std::runtime_error.
Definition: exceptions.h:64
std::string feature_name_
The feature name.
Definition: feature.h:222
Eigen::ArrayXXd computeSiForPoint(int index) const
Computes a spin-image for the point of the scan.
Definition: spin_image.hpp:69
virtual bool deinitCompute()
This method should get called after ending the actual computation.
Definition: feature.hpp:176
virtual void computeFeature(PointCloudOut &output)
Estimate the Spin Image descriptors at a set of points given by setInputWithNormals() using the surfa...
Definition: spin_image.hpp:324
Defines all the PCL implemented PointT point type structures.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
Feature represents the base feature class.
Definition: feature.h:105