40 #ifndef PCL_FEATURES_IMPL_NORMAL_BASED_SIGNATURE_H_
41 #define PCL_FEATURES_IMPL_NORMAL_BASED_SIGNATURE_H_
43 #include <pcl/features/normal_based_signature.h>
45 template <
typename Po
intT,
typename Po
intNT,
typename Po
intFeature>
void
50 PointFeature test_feature;
52 if (N_prime_ * M_prime_ !=
sizeof (test_feature.values) /
sizeof (float))
54 PCL_ERROR (
"NormalBasedSignatureEstimation: not using the proper signature size: %u vs %u\n", N_prime_ * M_prime_,
sizeof (test_feature.values) / sizeof (
float));
58 std::vector<int> k_indices;
59 std::vector<float> k_sqr_distances;
61 tree_->setInputCloud (input_);
62 output.
points.resize (indices_->size ());
64 for (
size_t index_i = 0; index_i < indices_->size (); ++index_i)
66 size_t point_i = (*indices_)[index_i];
67 Eigen::MatrixXf s_matrix (N_, M_);
69 Eigen::Vector4f center_point = input_->points[point_i].getVector4fMap ();
71 for (
size_t k = 0; k < N_; ++k)
73 Eigen::VectorXf s_row (M_);
75 for (
size_t l = 0; l < M_; ++l)
77 Eigen::Vector4f normal = normals_->points[point_i].getNormalVector4fMap ();
78 Eigen::Vector4f normal_u = Eigen::Vector4f::Zero ();
79 Eigen::Vector4f normal_v = Eigen::Vector4f::Zero ();
81 if (fabs (normal.x ()) > 0.0001f)
83 normal_u.x () = - normal.y () / normal.x ();
86 normal_u.normalize ();
89 else if (fabs (normal.y ()) > 0.0001f)
92 normal_u.y () = - normal.x () / normal.y ();
94 normal_u.normalize ();
100 normal_u.z () = - normal.y () / normal.z ();
102 normal_v = normal.cross3 (normal_u);
104 Eigen::Vector4f zeta_point = 2.0f *
static_cast<float> (l + 1) * scale_h_ /
static_cast<float> (M_) *
105 (cosf (2.0f *
static_cast<float> (M_PI) *
static_cast<float> ((k + 1) / N_)) * normal_u +
106 sinf (2.0f *
static_cast<float> (M_PI) *
static_cast<float> ((k + 1) / N_)) * normal_v);
109 Eigen::Vector4f zeta_point_plus_center = zeta_point + center_point;
111 zeta_point_pcl.x = zeta_point_plus_center.x (); zeta_point_pcl.y = zeta_point_plus_center.y (); zeta_point_pcl.z = zeta_point_plus_center.z ();
113 tree_->radiusSearch (zeta_point_pcl, search_radius_, k_indices, k_sqr_distances);
116 if (k_indices.size () == 0)
118 k_indices.resize (5);
119 k_sqr_distances.resize (5);
120 tree_->nearestKSearch (zeta_point_pcl, 5, k_indices, k_sqr_distances);
123 Eigen::Vector4f average_normal = Eigen::Vector4f::Zero ();
125 float average_normalization_factor = 0.0f;
128 for (
size_t nn_i = 0; nn_i < k_indices.size (); ++nn_i)
130 if (k_sqr_distances[nn_i] < 1e-7f)
132 average_normal = normals_->points[k_indices[nn_i]].getNormalVector4fMap ();
133 average_normalization_factor = 1.0f;
136 average_normal += normals_->points[k_indices[nn_i]].getNormalVector4fMap () / k_sqr_distances[nn_i];
137 average_normalization_factor += 1.0f / k_sqr_distances[nn_i];
139 average_normal /= average_normalization_factor;
140 float s = zeta_point.dot (average_normal) / zeta_point.norm ();
145 Eigen::VectorXf dct_row (M_);
146 for (
int m = 0; m < s_row.size (); ++m)
149 for (
int n = 0; n < s_row.size (); ++n)
150 Xk +=
static_cast<float> (s_row[n] * cos (M_PI / (
static_cast<double> (M_ * n) + 0.5) *
static_cast<double> (k)));
154 s_matrix.row (k).matrix () = dct_row;
158 Eigen::MatrixXf dft_matrix (N_, M_);
159 for (
size_t column_i = 0; column_i < M_; ++column_i)
161 Eigen::VectorXf dft_col (N_);
162 for (
size_t k = 0; k < N_; ++k)
164 float Xk_real = 0.0f, Xk_imag = 0.0f;
165 for (
size_t n = 0; n < N_; ++n)
167 Xk_real +=
static_cast<float> (s_matrix (n, column_i) * cos (2.0f * M_PI /
static_cast<double> (N_ * k * n)));
168 Xk_imag +=
static_cast<float> (s_matrix (n, column_i) * sin (2.0f * M_PI /
static_cast<double> (N_ * k * n)));
170 dft_col[k] = std::sqrt (Xk_real*Xk_real + Xk_imag*Xk_imag);
172 dft_matrix.col (column_i).matrix () = dft_col;
175 Eigen::MatrixXf final_matrix = dft_matrix.block (0, 0, N_prime_, M_prime_);
177 PointFeature feature_point;
178 for (
size_t i = 0; i < N_prime_; ++i)
179 for (
size_t j = 0; j < M_prime_; ++j)
180 feature_point.values[i*M_prime_ + j] = final_matrix (i, j);
182 output.
points[index_i] = feature_point;
188 #define PCL_INSTANTIATE_NormalBasedSignatureEstimation(T,NT,OutT) template class PCL_EXPORTS pcl::NormalBasedSignatureEstimation<T,NT,OutT>;