72 assert (image_width_ > 0);
73 assert (support_angle_cos_ <= 1.0 && support_angle_cos_ >= 0.0);
75 const Eigen::Vector3f origin_point ((*input_)[index].getVector3fMap ());
77 Eigen::Vector3f origin_normal;
80 (*input_normals_)[index].getNormalVector3fMap () :
83 const Eigen::Vector3f rotation_axis = use_custom_axis_ ?
84 rotation_axis_.getNormalVector3fMap () :
85 use_custom_axes_cloud_ ?
86 (*rotation_axes_cloud_)[index].getNormalVector3fMap () :
89 Eigen::ArrayXXd m_matrix (Eigen::ArrayXXd::Zero (image_width_+1, 2*image_width_+1));
90 Eigen::ArrayXXd m_averAngles (Eigen::ArrayXXd::Zero (image_width_+1, 2*image_width_+1));
97 double bin_size = 0.0;
99 bin_size = search_radius_ / image_width_;
101 bin_size = search_radius_ / image_width_ / sqrt(2.0);
104 std::vector<float> nn_sqr_dists;
105 const int neighb_cnt = this->searchForNeighbors (index, search_radius_, nn_indices, nn_sqr_dists);
106 if (neighb_cnt <
static_cast<int> (min_pts_neighb_))
109 "Too few points for spin image, use setMinPointCountInNeighbourhood() to decrease the threshold or use larger feature radius",
110 "spin_image.hpp",
"computeSiForPoint");
114 for (
int i_neigh = 0; i_neigh < neighb_cnt ; i_neigh++)
117 double cos_between_normals = -2.0;
118 if (support_angle_cos_ > 0.0 || is_angular_)
120 cos_between_normals = origin_normal.dot ((*input_normals_)[nn_indices[i_neigh]].getNormalVector3fMap ());
121 if (std::abs (cos_between_normals) > (1.0 + 10*std::numeric_limits<float>::epsilon ()))
123 PCL_ERROR (
"[pcl::%s::computeSiForPoint] Normal for the point %d and/or the point %d are not normalized, dot ptoduct is %f.\n",
124 getClassName ().c_str (), nn_indices[i_neigh], index, cos_between_normals);
126 "spin_image.hpp",
"computeSiForPoint");
128 cos_between_normals = std::max (-1.0, std::min (1.0, cos_between_normals));
130 if (std::abs (cos_between_normals) < support_angle_cos_ )
135 if (cos_between_normals < 0.0)
137 cos_between_normals = -cos_between_normals;
142 const Eigen::Vector3f direction (
143 (*surface_)[nn_indices[i_neigh]].getVector3fMap () - origin_point);
144 const double direction_norm = direction.norm ();
145 if (std::abs(direction_norm) < 10*std::numeric_limits<double>::epsilon ())
147 assert (direction_norm > 0.0);
150 double cos_dir_axis = direction.dot(rotation_axis) / direction_norm;
151 if (std::abs(cos_dir_axis) > (1.0 + 10*std::numeric_limits<float>::epsilon()))
153 PCL_ERROR (
"[pcl::%s::computeSiForPoint] Rotation axis for the point %d are not normalized, dot ptoduct is %f.\n",
154 getClassName ().c_str (), index, cos_dir_axis);
155 throw PCLException (
"Some rotation axis is not normalized",
156 "spin_image.hpp",
"computeSiForPoint");
158 cos_dir_axis = std::max (-1.0, std::min (1.0, cos_dir_axis));
161 double beta = std::numeric_limits<double>::signaling_NaN ();
162 double alpha = std::numeric_limits<double>::signaling_NaN ();
165 beta = asin (cos_dir_axis);
166 alpha = direction_norm;
170 beta = direction_norm * cos_dir_axis;
171 alpha = direction_norm * sqrt (1.0 - cos_dir_axis*cos_dir_axis);
173 if (std::abs (beta) >= bin_size * image_width_ || alpha >= bin_size * image_width_)
179 assert (alpha >= 0.0);
180 assert (alpha <= bin_size * image_width_ + 20 * std::numeric_limits<float>::epsilon () );
184 double beta_bin_size = is_radial_ ? (
M_PI / 2 / image_width_) : bin_size;
185 int beta_bin =
static_cast<int>(std::floor (beta / beta_bin_size)) +
static_cast<int>(image_width_);
186 assert (0 <= beta_bin && beta_bin < m_matrix.cols ());
187 int alpha_bin =
static_cast<int>(std::floor (alpha / bin_size));
188 assert (0 <= alpha_bin && alpha_bin < m_matrix.rows ());
190 if (alpha_bin ==
static_cast<int> (image_width_))
194 alpha = bin_size * (alpha_bin + 1) - std::numeric_limits<double>::epsilon ();
196 if (beta_bin ==
static_cast<int>(2*image_width_) )
200 beta = beta_bin_size * (beta_bin -
static_cast<int>(image_width_) + 1) - std::numeric_limits<double>::epsilon ();
203 double a = alpha/bin_size -
static_cast<double>(alpha_bin);
204 double b = beta/beta_bin_size -
static_cast<double>(beta_bin-
static_cast<int>(image_width_));
206 assert (0 <= a && a <= 1);
207 assert (0 <= b && b <= 1);
209 m_matrix (alpha_bin, beta_bin) += (1-a) * (1-b);
210 m_matrix (alpha_bin+1, beta_bin) += a * (1-b);
211 m_matrix (alpha_bin, beta_bin+1) += (1-a) * b;
212 m_matrix (alpha_bin+1, beta_bin+1) += a * b;
216 m_averAngles (alpha_bin, beta_bin) += (1-a) * (1-b) * std::acos (cos_between_normals);
217 m_averAngles (alpha_bin+1, beta_bin) += a * (1-b) * std::acos (cos_between_normals);
218 m_averAngles (alpha_bin, beta_bin+1) += (1-a) * b * std::acos (cos_between_normals);
219 m_averAngles (alpha_bin+1, beta_bin+1) += a * b * std::acos (cos_between_normals);
226 m_matrix = m_averAngles / (m_matrix + std::numeric_limits<double>::epsilon ());
228 else if (neighb_cnt > 1)
231 m_matrix /= m_matrix.sum();
244 PCL_ERROR (
"[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
251 PCL_ERROR (
"[pcl::%s::initCompute] No input dataset containing normals was given!\n", getClassName ().c_str ());
257 if (input_normals_->size () != input_->size ())
259 PCL_ERROR (
"[pcl::%s::initCompute] ", getClassName ().c_str ());
260 PCL_ERROR (
"The number of points in the input dataset differs from ");
261 PCL_ERROR (
"the number of points in the dataset containing the normals!\n");
267 if (search_radius_ == 0)
269 PCL_ERROR (
"[pcl::%s::initCompute] Need a search radius different than 0!\n", getClassName ().c_str ());
275 PCL_ERROR (
"[pcl::%s::initCompute] K-nearest neighbor search for spin images not implemented. Used a search radius instead!\n", getClassName ().c_str ());
284 fake_surface_ =
true;
290 assert(!(use_custom_axis_ && use_custom_axes_cloud_));
292 if (!use_custom_axis_ && !use_custom_axes_cloud_
295 PCL_ERROR (
"[pcl::%s::initCompute] No normals for input cloud were given!\n", getClassName ().c_str ());
301 if ((is_angular_ || support_angle_cos_ > 0.0)
304 PCL_ERROR (
"[pcl::%s::initCompute] No normals for input cloud were given!\n", getClassName ().c_str ());
310 if (use_custom_axes_cloud_
311 && rotation_axes_cloud_->size () == input_->size ())
313 PCL_ERROR (
"[pcl::%s::initCompute] Rotation axis cloud have different size from input!\n", getClassName ().c_str ());