41 #ifndef PCL_FEATURES_IMPL_SPIN_IMAGE_H_ 42 #define PCL_FEATURES_IMPL_SPIN_IMAGE_H_ 46 #include <pcl/exceptions.h> 47 #include <pcl/features/spin_image.h> 51 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
53 unsigned int image_width,
double support_angle_cos,
unsigned int min_pts_neighb) :
54 input_normals_ (), rotation_axes_cloud_ (),
55 is_angular_ (false), rotation_axis_ (), use_custom_axis_(false), use_custom_axes_cloud_ (false),
56 is_radial_ (false), image_width_ (image_width), support_angle_cos_ (support_angle_cos),
57 min_pts_neighb_ (min_pts_neighb)
59 assert (support_angle_cos_ <= 1.0 && support_angle_cos_ >= 0.0);
66 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT> Eigen::ArrayXXd
69 assert (image_width_ > 0);
70 assert (support_angle_cos_ <= 1.0 && support_angle_cos_ >= 0.0);
72 const Eigen::Vector3f origin_point ((*input_)[index].getVector3fMap ());
74 Eigen::Vector3f origin_normal;
77 (*input_normals_)[index].getNormalVector3fMap () :
80 const Eigen::Vector3f rotation_axis = use_custom_axis_ ?
81 rotation_axis_.getNormalVector3fMap () :
82 use_custom_axes_cloud_ ?
83 (*rotation_axes_cloud_)[index].getNormalVector3fMap () :
86 Eigen::ArrayXXd m_matrix (Eigen::ArrayXXd::Zero (image_width_+1, 2*image_width_+1));
87 Eigen::ArrayXXd m_averAngles (Eigen::ArrayXXd::Zero (image_width_+1, 2*image_width_+1));
94 double bin_size = 0.0;
96 bin_size = search_radius_ / image_width_;
98 bin_size = search_radius_ / image_width_ / sqrt(2.0);
100 std::vector<int> nn_indices;
101 std::vector<float> nn_sqr_dists;
102 const int neighb_cnt = this->searchForNeighbors (index, search_radius_, nn_indices, nn_sqr_dists);
103 if (neighb_cnt < static_cast<int> (min_pts_neighb_))
106 "Too few points for spin image, use setMinPointCountInNeighbourhood() to decrease the threshold or use larger feature radius",
107 "spin_image.hpp",
"computeSiForPoint");
111 for (
int i_neigh = 0; i_neigh < neighb_cnt ; i_neigh++)
114 double cos_between_normals = -2.0;
115 if (support_angle_cos_ > 0.0 || is_angular_)
117 cos_between_normals = origin_normal.dot ((*input_normals_)[nn_indices[i_neigh]].getNormalVector3fMap ());
118 if (std::abs (cos_between_normals) > (1.0 + 10*std::numeric_limits<float>::epsilon ()))
120 PCL_ERROR (
"[pcl::%s::computeSiForPoint] Normal for the point %d and/or the point %d are not normalized, dot ptoduct is %f.\n",
121 getClassName ().c_str (), nn_indices[i_neigh], index, cos_between_normals);
123 "spin_image.hpp",
"computeSiForPoint");
125 cos_between_normals = std::max (-1.0, std::min (1.0, cos_between_normals));
127 if (std::abs (cos_between_normals) < support_angle_cos_ )
132 if (cos_between_normals < 0.0)
134 cos_between_normals = -cos_between_normals;
139 const Eigen::Vector3f direction (
140 (*surface_)[nn_indices[i_neigh]].getVector3fMap () - origin_point);
141 const double direction_norm = direction.norm ();
142 if (std::abs(direction_norm) < 10*std::numeric_limits<double>::epsilon ())
144 assert (direction_norm > 0.0);
147 double cos_dir_axis = direction.dot(rotation_axis) / direction_norm;
148 if (std::abs(cos_dir_axis) > (1.0 + 10*std::numeric_limits<float>::epsilon()))
150 PCL_ERROR (
"[pcl::%s::computeSiForPoint] Rotation axis for the point %d are not normalized, dot ptoduct is %f.\n",
151 getClassName ().c_str (), index, cos_dir_axis);
152 throw PCLException (
"Some rotation axis is not normalized",
153 "spin_image.hpp",
"computeSiForPoint");
155 cos_dir_axis = std::max (-1.0, std::min (1.0, cos_dir_axis));
158 double beta = std::numeric_limits<double>::signaling_NaN ();
159 double alpha = std::numeric_limits<double>::signaling_NaN ();
162 beta = asin (cos_dir_axis);
163 alpha = direction_norm;
167 beta = direction_norm * cos_dir_axis;
168 alpha = direction_norm * sqrt (1.0 - cos_dir_axis*cos_dir_axis);
170 if (std::abs (beta) >= bin_size * image_width_ || alpha >= bin_size * image_width_)
176 assert (alpha >= 0.0);
177 assert (alpha <= bin_size * image_width_ + 20 * std::numeric_limits<float>::epsilon () );
181 double beta_bin_size = is_radial_ ? (
M_PI / 2 / image_width_) : bin_size;
182 int beta_bin = int(std::floor (beta / beta_bin_size)) + int(image_width_);
183 assert (0 <= beta_bin && beta_bin < m_matrix.cols ());
184 int alpha_bin = int(std::floor (alpha / bin_size));
185 assert (0 <= alpha_bin && alpha_bin < m_matrix.rows ());
187 if (alpha_bin == static_cast<int> (image_width_))
191 alpha = bin_size * (alpha_bin + 1) - std::numeric_limits<double>::epsilon ();
193 if (beta_bin ==
int(2*image_width_) )
197 beta = beta_bin_size * (beta_bin - int(image_width_) + 1) - std::numeric_limits<double>::epsilon ();
200 double a = alpha/bin_size - double(alpha_bin);
201 double b = beta/beta_bin_size - double(beta_bin-
int(image_width_));
203 assert (0 <= a && a <= 1);
204 assert (0 <= b && b <= 1);
206 m_matrix (alpha_bin, beta_bin) += (1-a) * (1-b);
207 m_matrix (alpha_bin+1, beta_bin) += a * (1-b);
208 m_matrix (alpha_bin, beta_bin+1) += (1-a) * b;
209 m_matrix (alpha_bin+1, beta_bin+1) += a * b;
213 m_averAngles (alpha_bin, beta_bin) += (1-a) * (1-b) * std::acos (cos_between_normals);
214 m_averAngles (alpha_bin+1, beta_bin) += a * (1-b) * std::acos (cos_between_normals);
215 m_averAngles (alpha_bin, beta_bin+1) += (1-a) * b * std::acos (cos_between_normals);
216 m_averAngles (alpha_bin+1, beta_bin+1) += a * b * std::acos (cos_between_normals);
223 m_matrix = m_averAngles / (m_matrix + std::numeric_limits<double>::epsilon ());
225 else if (neighb_cnt > 1)
228 m_matrix /= m_matrix.sum();
236 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
bool 241 PCL_ERROR (
"[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
248 PCL_ERROR (
"[pcl::%s::initCompute] No input dataset containing normals was given!\n", getClassName ().c_str ());
254 if (input_normals_->size () != input_->size ())
256 PCL_ERROR (
"[pcl::%s::initCompute] ", getClassName ().c_str ());
257 PCL_ERROR (
"The number of points in the input dataset differs from ");
258 PCL_ERROR (
"the number of points in the dataset containing the normals!\n");
264 if (search_radius_ == 0)
266 PCL_ERROR (
"[pcl::%s::initCompute] Need a search radius different than 0!\n", getClassName ().c_str ());
272 PCL_ERROR (
"[pcl::%s::initCompute] K-nearest neighbor search for spin images not implemented. Used a search radius instead!\n", getClassName ().c_str ());
281 fake_surface_ =
true;
287 assert(!(use_custom_axis_ && use_custom_axes_cloud_));
289 if (!use_custom_axis_ && !use_custom_axes_cloud_
292 PCL_ERROR (
"[pcl::%s::initCompute] No normals for input cloud were given!\n", getClassName ().c_str ());
298 if ((is_angular_ || support_angle_cos_ > 0.0)
301 PCL_ERROR (
"[pcl::%s::initCompute] No normals for input cloud were given!\n", getClassName ().c_str ());
307 if (use_custom_axes_cloud_
308 && rotation_axes_cloud_->size () == input_->size ())
310 PCL_ERROR (
"[pcl::%s::initCompute] Rotation axis cloud have different size from input!\n", getClassName ().c_str ());
321 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void 324 for (std::size_t i_input = 0; i_input < indices_->size (); ++i_input)
326 Eigen::ArrayXXd res = computeSiForPoint (indices_->at (i_input));
329 for (Eigen::Index iRow = 0; iRow < res.rows () ; iRow++)
331 for (Eigen::Index iCol = 0; iCol < res.cols () ; iCol++)
333 output[i_input].histogram[ iRow*res.cols () + iCol ] =
static_cast<float> (res (iRow, iCol));
339 #define PCL_INSTANTIATE_SpinImageEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::SpinImageEstimation<T,NT,OutT>; 341 #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.
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.
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.
Defines all the PCL implemented PointT point type structures.
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.