| 47 | // (PCL_XYZ_POINT_TYPES)(PCL_NORMAL_POINT_TYPES)); #endif // PCL_NO_PRECOMPILE |
| 48 | |
| 49 | void |
| 50 | pcl::PPFHashMapSearch::setInputFeatureCloud( |
| 51 | PointCloud<PPFSignature>::ConstPtr feature_cloud) |
| 52 | { |
| 53 | // Discretize the feature cloud and insert it in the hash map |
| 54 | feature_hash_map_->clear(); |
| 55 | auto n = |
| 56 | static_cast<unsigned int>(std::sqrt(static_cast<float>(feature_cloud->size()))); |
| 57 | int d1, d2, d3, d4; |
| 58 | max_dist_ = -1.0; |
| 59 | alpha_m_.resize(n); |
| 60 | for (std::size_t i = 0; i < n; ++i) { |
| 61 | std::vector<float> alpha_m_row(n); |
| 62 | for (std::size_t j = 0; j < n; ++j) { |
| 63 | d1 = static_cast<int>( |
| 64 | std::floor((*feature_cloud)[i * n + j].f1 / angle_discretization_step_)); |
| 65 | d2 = static_cast<int>( |
| 66 | std::floor((*feature_cloud)[i * n + j].f2 / angle_discretization_step_)); |
| 67 | d3 = static_cast<int>( |
| 68 | std::floor((*feature_cloud)[i * n + j].f3 / angle_discretization_step_)); |
| 69 | d4 = static_cast<int>( |
| 70 | std::floor((*feature_cloud)[i * n + j].f4 / distance_discretization_step_)); |
| 71 | feature_hash_map_->insert( |
| 72 | std::pair<HashKeyStruct, std::pair<std::size_t, std::size_t>>( |
| 73 | HashKeyStruct(d1, d2, d3, d4), |
| 74 | std::pair<std::size_t, std::size_t>(i, j))); |
| 75 | alpha_m_row[j] = (*feature_cloud)[i * n + j].alpha_m; |
| 76 | |
| 77 | if (max_dist_ < (*feature_cloud)[i * n + j].f4) |
| 78 | max_dist_ = (*feature_cloud)[i * n + j].f4; |
| 79 | } |
| 80 | alpha_m_[i] = alpha_m_row; |
| 81 | } |
| 82 | |
| 83 | internals_initialized_ = true; |
| 84 | } |
| 85 | |
| 86 | ////////////////////////////////////////////////////////////////////////////////////////////// |
| 87 | void |