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