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; } }