41 #ifndef PCL_FEATURES_IMPL_SPIN_IMAGE_H_
42 #define PCL_FEATURES_IMPL_SPIN_IMAGE_H_
45 #include <pcl/point_cloud.h>
46 #include <pcl/point_types.h>
47 #include <pcl/exceptions.h>
48 #include <pcl/features/spin_image.h>
52 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
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)
60 assert (support_angle_cos_ <= 1.0 && support_angle_cos_ >= 0.0);
67 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT> Eigen::ArrayXXd
70 assert (image_width_ > 0);
71 assert (support_angle_cos_ <= 1.0 && support_angle_cos_ >= 0.0);
73 const Eigen::Vector3f origin_point (input_->points[index].getVector3fMap ());
75 Eigen::Vector3f origin_normal;
78 input_normals_->points[index].getNormalVector3fMap () :
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 () :
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));
95 double bin_size = 0.0;
97 bin_size = search_radius_ / image_width_;
99 bin_size = search_radius_ / image_width_ / sqrt(2.0);
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_))
107 "Too few points for spin image, use setMinPointCountInNeighbourhood() to decrease the threshold or use larger feature radius",
108 "spin_image.hpp",
"computeSiForPoint");
112 for (
int i_neigh = 0; i_neigh < neighb_cnt ; i_neigh++)
115 double cos_between_normals = -2.0;
116 if (support_angle_cos_ > 0.0 || is_angular_)
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 ()))
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);
124 "spin_image.hpp",
"computeSiForPoint");
126 cos_between_normals = std::max (-1.0, std::min (1.0, cos_between_normals));
128 if (std::abs (cos_between_normals) < support_angle_cos_ )
133 if (cos_between_normals < 0.0)
135 cos_between_normals = -cos_between_normals;
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 ())
145 assert (direction_norm > 0.0);
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()))
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");
156 cos_dir_axis = std::max (-1.0, std::min (1.0, cos_dir_axis));
159 double beta = std::numeric_limits<double>::signaling_NaN ();
160 double alpha = std::numeric_limits<double>::signaling_NaN ();
163 beta = asin (cos_dir_axis);
164 alpha = direction_norm;
168 beta = direction_norm * cos_dir_axis;
169 alpha = direction_norm * sqrt (1.0 - cos_dir_axis*cos_dir_axis);
171 if (std::abs (beta) >= bin_size * image_width_ || alpha >= bin_size * image_width_)
177 assert (alpha >= 0.0);
178 assert (alpha <= bin_size * image_width_ + 20 * std::numeric_limits<float>::epsilon () );
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 ());
188 if (alpha_bin == static_cast<int> (image_width_))
192 alpha = bin_size * (alpha_bin + 1) - std::numeric_limits<double>::epsilon ();
194 if (beta_bin ==
int(2*image_width_) )
198 beta = beta_bin_size * (beta_bin - int(image_width_) + 1) - std::numeric_limits<double>::epsilon ();
201 double a = alpha/bin_size - double(alpha_bin);
202 double b = beta/beta_bin_size - double(beta_bin-
int(image_width_));
204 assert (0 <= a && a <= 1);
205 assert (0 <= b && b <= 1);
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;
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);
224 m_matrix = m_averAngles / (m_matrix + std::numeric_limits<double>::epsilon ());
226 else if (neighb_cnt > 1)
229 m_matrix /= m_matrix.sum();
237 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
bool
242 PCL_ERROR (
"[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
249 PCL_ERROR (
"[pcl::%s::initCompute] No input dataset containing normals was given!\n", getClassName ().c_str ());
255 if (input_normals_->points.size () != input_->points.size ())
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");
265 if (search_radius_ == 0)
267 PCL_ERROR (
"[pcl::%s::initCompute] Need a search radius different than 0!\n", getClassName ().c_str ());
273 PCL_ERROR (
"[pcl::%s::initCompute] K-nearest neighbor search for spin images not implemented. Used a search radius instead!\n", getClassName ().c_str ());
282 fake_surface_ =
true;
288 assert(!(use_custom_axis_ && use_custom_axes_cloud_));
290 if (!use_custom_axis_ && !use_custom_axes_cloud_
293 PCL_ERROR (
"[pcl::%s::initCompute] No normals for input cloud were given!\n", getClassName ().c_str ());
299 if ((is_angular_ || support_angle_cos_ > 0.0)
302 PCL_ERROR (
"[pcl::%s::initCompute] No normals for input cloud were given!\n", getClassName ().c_str ());
308 if (use_custom_axes_cloud_
309 && rotation_axes_cloud_->size () == input_->size ())
311 PCL_ERROR (
"[pcl::%s::initCompute] Rotation axis cloud have different size from input!\n", getClassName ().c_str ());
322 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
325 for (std::size_t i_input = 0; i_input < indices_->size (); ++i_input)
327 Eigen::ArrayXXd res = computeSiForPoint (indices_->at (i_input));
330 for (Eigen::Index iRow = 0; iRow < res.rows () ; iRow++)
332 for (Eigen::Index iCol = 0; iCol < res.cols () ; iCol++)
334 output.
points[i_input].histogram[ iRow*res.cols () + iCol ] =
static_cast<float> (res (iRow, iCol));
340 #define PCL_INSTANTIATE_SpinImageEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::SpinImageEstimation<T,NT,OutT>;
342 #endif // PCL_FEATURES_IMPL_SPIN_IMAGE_H_
SpinImageEstimation(unsigned int image_width=8, double support_angle_cos=0.0, unsigned int min_pts_neighb=0)
Constructs empty spin image estimator.
std::vector< PointT, Eigen::aligned_allocator< PointT > > points
The point data.
A base class for all pcl exceptions which inherits from std::runtime_error.
std::string feature_name_
The feature name.
bool initCompute() override
initializes computations specific to spin-image.
virtual bool deinitCompute()
This method should get called after ending the actual computation.
void computeFeature(PointCloudOut &output) override
Estimate the Spin Image descriptors at a set of points given by setInputWithNormals() using the surfa...
Feature represents the base feature class.
Eigen::ArrayXXd computeSiForPoint(int index) const
Computes a spin-image for the point of the scan.