Point Cloud Library (PCL) 1.14.0
Loading...
Searching...
No Matches
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_types.h>
46#include <pcl/exceptions.h>
47#include <pcl/features/spin_image.h>
48#include <cmath>
49
50//////////////////////////////////////////////////////////////////////////////////////////////
51template <typename PointInT, typename PointNT, typename PointOutT>
53 unsigned int image_width, double support_angle_cos, unsigned int min_pts_neighb) :
54 input_normals_ (), rotation_axes_cloud_ (),
55 rotation_axis_ (), support_angle_cos_ (support_angle_cos),
56 min_pts_neighb_ (min_pts_neighb)
57{
58 if (0.0 > support_angle_cos || support_angle_cos > 1.0) { // may be permit negative cosine?
59 throw PCLException ("Cosine of support angle should be between 0 and 1", "spin_image.hpp", "SpinImageEstimation");
60 }
61 setImageWidth(image_width);
62
63 feature_name_ = "SpinImageEstimation";
64}
65
66
67//////////////////////////////////////////////////////////////////////////////////////////////
68template <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_)[index].getVector3fMap ());
75
76 Eigen::Vector3f origin_normal;
77 origin_normal =
78 input_normals_ ?
79 (*input_normals_)[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_)[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 pcl::Indices 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_)[nn_indices[i_neigh]].getNormalVector3fMap ());
120 if (std::abs (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 (std::abs (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_)[nn_indices[i_neigh]].getVector3fMap () - origin_point);
143 const double direction_norm = direction.norm ();
144 if (std::abs(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 (std::abs(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 (std::abs (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 = static_cast<int>(std::floor (beta / beta_bin_size)) + static_cast<int>(image_width_);
185 assert (0 <= beta_bin && beta_bin < m_matrix.cols ());
186 int alpha_bin = static_cast<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 == static_cast<int>(2*image_width_) ) // border points
196 {
197 beta_bin--;
198 // HACK: to prevent b > 1
199 beta = beta_bin_size * (beta_bin - static_cast<int>(image_width_) + 1) - std::numeric_limits<double>::epsilon ();
200 }
201
202 double a = alpha/bin_size - static_cast<double>(alpha_bin);
203 double b = beta/beta_bin_size - static_cast<double>(beta_bin-static_cast<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) * std::acos (cos_between_normals);
216 m_averAngles (alpha_bin+1, beta_bin) += a * (1-b) * std::acos (cos_between_normals);
217 m_averAngles (alpha_bin, beta_bin+1) += (1-a) * b * std::acos (cos_between_normals);
218 m_averAngles (alpha_bin+1, beta_bin+1) += a * b * std::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//////////////////////////////////////////////////////////////////////////////////////////////
238template <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_->size () != input_->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//////////////////////////////////////////////////////////////////////////////////////////////
323template <typename PointInT, typename PointNT, typename PointOutT> void
325{
326 for (std::size_t i_input = 0; i_input < indices_->size (); ++i_input)
327 {
328 Eigen::ArrayXXd res = computeSiForPoint (indices_->at (i_input));
329
330 // Copy into the resultant cloud
331 for (Eigen::Index iRow = 0; iRow < res.rows () ; iRow++)
332 {
333 for (Eigen::Index iCol = 0; iCol < res.cols () ; iCol++)
334 {
335 output[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
Feature represents the base feature class.
Definition feature.h:107
std::string feature_name_
The feature name.
Definition feature.h:220
virtual bool deinitCompute()
This method should get called after ending the actual computation.
Definition feature.hpp:181
A base class for all pcl exceptions which inherits from std::runtime_error.
Definition exceptions.h:64
typename Feature< PointInT, PointOutT >::PointCloudOut PointCloudOut
Definition spin_image.h:103
Eigen::ArrayXXd computeSiForPoint(int index) const
Computes a spin-image for the point of the scan.
void setImageWidth(unsigned int bin_count)
Sets spin-image resolution.
Definition spin_image.h:134
SpinImageEstimation(unsigned int image_width=8, double support_angle_cos=0.0, unsigned int min_pts_neighb=0)
Constructs empty spin image estimator.
bool initCompute() override
initializes computations specific to spin-image.
void computeFeature(PointCloudOut &output) override
Estimate the Spin Image descriptors at a set of points given by setInputWithNormals() using the surfa...
Defines all the PCL implemented PointT point type structures.
IndicesAllocator<> Indices
Type used for indices in PCL.
Definition types.h:133
#define M_PI
Definition pcl_macros.h:201