41 #ifndef PCL_FEATURES_IMPL_SPIN_IMAGE_H_ 42 #define PCL_FEATURES_IMPL_SPIN_IMAGE_H_ 45 #include <pcl/point_cloud.h> 47 #include <pcl/exceptions.h> 48 #include <pcl/kdtree/kdtree_flann.h> 49 #include <pcl/features/spin_image.h> 53 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
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)
61 assert (support_angle_cos_ <= 1.0 && support_angle_cos_ >= 0.0);
68 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT> Eigen::ArrayXXd
71 assert (image_width_ > 0);
72 assert (support_angle_cos_ <= 1.0 && support_angle_cos_ >= 0.0);
74 const Eigen::Vector3f origin_point (input_->points[index].getVector3fMap ());
76 Eigen::Vector3f origin_normal;
79 input_normals_->points[index].getNormalVector3fMap () :
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 () :
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));
96 double bin_size = 0.0;
98 bin_size = search_radius_ / image_width_;
100 bin_size = search_radius_ / image_width_ / sqrt(2.0);
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_))
108 "Too few points for spin image, use setMinPointCountInNeighbourhood() to decrease the threshold or use larger feature radius",
109 "spin_image.hpp",
"computeSiForPoint");
113 for (
int i_neigh = 0; i_neigh < neighb_cnt ; i_neigh++)
116 double cos_between_normals = -2.0;
117 if (support_angle_cos_ > 0.0 || is_angular_)
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 ()))
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);
125 "spin_image.hpp",
"computeSiForPoint");
127 cos_between_normals = std::max (-1.0, std::min (1.0, cos_between_normals));
129 if (fabs (cos_between_normals) < support_angle_cos_ )
134 if (cos_between_normals < 0.0)
136 cos_between_normals = -cos_between_normals;
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 ())
146 assert (direction_norm > 0.0);
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()))
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");
157 cos_dir_axis = std::max (-1.0, std::min (1.0, cos_dir_axis));
160 double beta = std::numeric_limits<double>::signaling_NaN ();
161 double alpha = std::numeric_limits<double>::signaling_NaN ();
164 beta = asin (cos_dir_axis);
165 alpha = direction_norm;
169 beta = direction_norm * cos_dir_axis;
170 alpha = direction_norm * sqrt (1.0 - cos_dir_axis*cos_dir_axis);
172 if (fabs (beta) >= bin_size * image_width_ || alpha >= bin_size * image_width_)
178 assert (alpha >= 0.0);
179 assert (alpha <= bin_size * image_width_ + 20 * std::numeric_limits<float>::epsilon () );
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 ());
189 if (alpha_bin == static_cast<int> (image_width_))
193 alpha = bin_size * (alpha_bin + 1) - std::numeric_limits<double>::epsilon ();
195 if (beta_bin ==
int(2*image_width_) )
199 beta = beta_bin_size * (beta_bin - int(image_width_) + 1) - std::numeric_limits<double>::epsilon ();
202 double a = alpha/bin_size - double(alpha_bin);
203 double b = beta/beta_bin_size - double(beta_bin-
int(image_width_));
205 assert (0 <= a && a <= 1);
206 assert (0 <= b && b <= 1);
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;
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);
225 m_matrix = m_averAngles / (m_matrix + std::numeric_limits<double>::epsilon ());
227 else if (neighb_cnt > 1)
230 m_matrix /= m_matrix.sum();
238 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
bool 243 PCL_ERROR (
"[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
250 PCL_ERROR (
"[pcl::%s::initCompute] No input dataset containing normals was given!\n", getClassName ().c_str ());
256 if (input_normals_->points.size () != input_->points.size ())
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");
266 if (search_radius_ == 0)
268 PCL_ERROR (
"[pcl::%s::initCompute] Need a search radius different than 0!\n", getClassName ().c_str ());
274 PCL_ERROR (
"[pcl::%s::initCompute] K-nearest neighbor search for spin images not implemented. Used a search radius instead!\n", getClassName ().c_str ());
283 fake_surface_ =
true;
289 assert(!(use_custom_axis_ && use_custom_axes_cloud_));
291 if (!use_custom_axis_ && !use_custom_axes_cloud_
294 PCL_ERROR (
"[pcl::%s::initCompute] No normals for input cloud were given!\n", getClassName ().c_str ());
300 if ((is_angular_ || support_angle_cos_ > 0.0)
303 PCL_ERROR (
"[pcl::%s::initCompute] No normals for input cloud were given!\n", getClassName ().c_str ());
309 if (use_custom_axes_cloud_
310 && rotation_axes_cloud_->size () == input_->size ())
312 PCL_ERROR (
"[pcl::%s::initCompute] Rotation axis cloud have different size from input!\n", getClassName ().c_str ());
323 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void 326 for (
int i_input = 0; i_input < static_cast<int> (indices_->size ()); ++i_input)
328 Eigen::ArrayXXd res = computeSiForPoint (indices_->at (i_input));
331 for (
int iRow = 0; iRow < res.rows () ; iRow++)
333 for (
int iCol = 0; iCol < res.cols () ; iCol++)
335 output.
points[i_input].histogram[ iRow*res.cols () + iCol ] = static_cast<float> (res (iRow, iCol));
341 #define PCL_INSTANTIATE_SpinImageEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::SpinImageEstimation<T,NT,OutT>; 343 #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.
virtual bool initCompute()
initializes computations specific to spin-image.
A base class for all pcl exceptions which inherits from std::runtime_error.
std::string feature_name_
The feature name.
Eigen::ArrayXXd computeSiForPoint(int index) const
Computes a spin-image for the point of the scan.
virtual bool deinitCompute()
This method should get called after ending the actual computation.
virtual void computeFeature(PointCloudOut &output)
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.
PointCloud represents the base class in PCL for storing collections of 3D points.
Feature represents the base feature class.