void dct_2(float **x, int n1, int n2, float **c1, float **c2, int type)
/*****************************************************************************
in place 2D-DCT
******************************************************************************
x array[][] of the signal before and after transform
n1 length of the signal along the 1st dimension
n2 length of the signal along the 2nd dimension
c1 table for the 1st dimension
c2 table for the 2nd dimension
type 0 for forward and 1 for inverse transform
*****************************************************************************/
{
int i;
/* first along the faster dimension */
for(i=0; i<n2; i++)
dct(x[i], n1, c1, type);
/* then along the slower dimension */
dct_row(x, n1, n2, c2, type);
}
template <typename PointT, typename PointNT, typename PointFeature> void
pcl::NormalBasedSignatureEstimation<PointT, PointNT, PointFeature>::computeFeature (FeatureCloud &output)
{
// do a few checks before starting the computations
PointFeature test_feature;
(void)test_feature;
if (N_prime_ * M_prime_ != sizeof (test_feature.values) / sizeof (float))
{
PCL_ERROR ("NormalBasedSignatureEstimation: not using the proper signature size: %u vs %u\n", N_prime_ * M_prime_, sizeof (test_feature.values) / sizeof (float));
return;
}
std::vector<int> k_indices;
std::vector<float> k_sqr_distances;
tree_->setInputCloud (input_);
output.points.resize (indices_->size ());
for (size_t index_i = 0; index_i < indices_->size (); ++index_i)
{
size_t point_i = (*indices_)[index_i];
Eigen::MatrixXf s_matrix (N_, M_);
Eigen::Vector4f center_point = input_->points[point_i].getVector4fMap ();
for (size_t k = 0; k < N_; ++k)
{
Eigen::VectorXf s_row (M_);
for (size_t l = 0; l < M_; ++l)
{
Eigen::Vector4f normal = normals_->points[point_i].getNormalVector4fMap ();
Eigen::Vector4f normal_u = Eigen::Vector4f::Zero ();
Eigen::Vector4f normal_v = Eigen::Vector4f::Zero ();
if (fabs (normal.x ()) > 0.0001f)
{
normal_u.x () = - normal.y () / normal.x ();
normal_u.y () = 1.0f;
normal_u.z () = 0.0f;
normal_u.normalize ();
}
else if (fabs (normal.y ()) > 0.0001f)
{
normal_u.x () = 1.0f;
normal_u.y () = - normal.x () / normal.y ();
normal_u.z () = 0.0f;
normal_u.normalize ();
}
else
{
normal_u.x () = 0.0f;
normal_u.y () = 1.0f;
normal_u.z () = - normal.y () / normal.z ();
}
normal_v = normal.cross3 (normal_u);
Eigen::Vector4f zeta_point = 2.0f * static_cast<float> (l + 1) * scale_h_ / static_cast<float> (M_) *
(cosf (2.0f * static_cast<float> (M_PI) * static_cast<float> ((k + 1) / N_)) * normal_u +
sinf (2.0f * static_cast<float> (M_PI) * static_cast<float> ((k + 1) / N_)) * normal_v);
// Compute normal by using the neighbors
Eigen::Vector4f zeta_point_plus_center = zeta_point + center_point;
PointT zeta_point_pcl;
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 ();
tree_->radiusSearch (zeta_point_pcl, search_radius_, k_indices, k_sqr_distances);
// Do k nearest search if there are no neighbors nearby
if (k_indices.size () == 0)
{
k_indices.resize (5);
k_sqr_distances.resize (5);
tree_->nearestKSearch (zeta_point_pcl, 5, k_indices, k_sqr_distances);
}
Eigen::Vector4f average_normal = Eigen::Vector4f::Zero ();
float average_normalization_factor = 0.0f;
// Normals weighted by 1/squared_distances
for (size_t nn_i = 0; nn_i < k_indices.size (); ++nn_i)
{
if (k_sqr_distances[nn_i] < 1e-7f)
{
average_normal = normals_->points[k_indices[nn_i]].getNormalVector4fMap ();
average_normalization_factor = 1.0f;
break;
}
average_normal += normals_->points[k_indices[nn_i]].getNormalVector4fMap () / k_sqr_distances[nn_i];
average_normalization_factor += 1.0f / k_sqr_distances[nn_i];
}
average_normal /= average_normalization_factor;
float s = zeta_point.dot (average_normal) / zeta_point.norm ();
s_row[l] = s;
}
// do DCT on the s_matrix row-wise
Eigen::VectorXf dct_row (M_);
for (int m = 0; m < s_row.size (); ++m)
{
float Xk = 0.0f;
for (int n = 0; n < s_row.size (); ++n)
Xk += static_cast<float> (s_row[n] * cos (M_PI / (static_cast<double> (M_ * n) + 0.5) * static_cast<double> (k)));
dct_row[m] = Xk;
}
s_row = dct_row;
s_matrix.row (k).matrix () = dct_row;
}
// do DFT on the s_matrix column-wise
Eigen::MatrixXf dft_matrix (N_, M_);
for (size_t column_i = 0; column_i < M_; ++column_i)
{
Eigen::VectorXf dft_col (N_);
for (size_t k = 0; k < N_; ++k)
{
float Xk_real = 0.0f, Xk_imag = 0.0f;
for (size_t n = 0; n < N_; ++n)
{
Xk_real += static_cast<float> (s_matrix (n, column_i) * cos (2.0f * M_PI / static_cast<double> (N_ * k * n)));
Xk_imag += static_cast<float> (s_matrix (n, column_i) * sin (2.0f * M_PI / static_cast<double> (N_ * k * n)));
}
dft_col[k] = sqrtf (Xk_real*Xk_real + Xk_imag*Xk_imag);
}
dft_matrix.col (column_i).matrix () = dft_col;
}
Eigen::MatrixXf final_matrix = dft_matrix.block (0, 0, N_prime_, M_prime_);
PointFeature feature_point;
for (size_t i = 0; i < N_prime_; ++i)
for (size_t j = 0; j < M_prime_; ++j)
feature_point.values[i*M_prime_ + j] = final_matrix (i, j);
output.points[index_i] = feature_point;
}
}
