template <typename PointInT, typename PointOutT> void pcl::SIFTKeypoint<PointInT, PointOutT>::computeScaleSpace ( const PointCloudIn &input, KdTree &tree, const std::vector<float> &scales, Eigen::MatrixXf &diff_of_gauss) { std::vector<int> nn_indices; std::vector<float> nn_dist; diff_of_gauss.resize (input.size (), scales.size () - 1); // For efficiency, we will only filter over points within 3 standard deviations const float max_radius = 3.0 * scales.back (); for (size_t i_point = 0; i_point < input.size (); ++i_point) { tree.radiusSearch (i_point, max_radius, nn_indices, nn_dist); // * // * note: at this stage of the algorithm, we must find all points within a radius defined by the maximum scale, // regardless of the configurable search method specified by the user, so we directly employ tree.radiusSearch // here instead of using searchForNeighbors. // For each scale, compute the Gaussian "filter response" at the current point float filter_response = 0; float previous_filter_response; for (size_t i_scale = 0; i_scale < scales.size (); ++i_scale) { float sigma_sqr = pow (scales[i_scale], 2); float numerator = 0.0; float denominator = 0.0; for (size_t i_neighbor = 0; i_neighbor < nn_indices.size (); ++i_neighbor) { const float &intensity = input.points[nn_indices[i_neighbor]].intensity; const float &dist_sqr = nn_dist[i_neighbor]; if (dist_sqr <= 9*sigma_sqr) { float w = exp (-0.5 * dist_sqr / sigma_sqr); numerator += intensity * w; denominator += w; } else break; // i.e. if dist > 3 standard deviations, then terminate early } previous_filter_response = filter_response; filter_response = numerator / denominator; // Compute the difference between adjacent scales if (i_scale > 0) { diff_of_gauss (i_point, i_scale - 1) = filter_response - previous_filter_response; } } } }
template <typename PointInT, typename PointOutT> void pcl::SIFTKeypoint<PointInT, PointOutT>::findScaleSpaceExtrema ( const PointCloudIn &input, KdTree &tree, const Eigen::MatrixXf &diff_of_gauss, std::vector<int> &extrema_indices, std::vector<int> &extrema_scales) { const int k = 25; std::vector<int> nn_indices (k); std::vector<float> nn_dist (k); const int nr_scales = static_cast<int> (diff_of_gauss.cols ()); std::vector<float> min_val (nr_scales), max_val (nr_scales); for (int i_point = 0; i_point < static_cast<int> (input.size ()); ++i_point) { // Define the local neighborhood around the current point const size_t nr_nn = tree.nearestKSearch (i_point, k, nn_indices, nn_dist); //* // * note: the neighborhood for finding local extrema is best defined as a small fixed-k neighborhood, regardless of // the configurable search method specified by the user, so we directly employ tree.nearestKSearch here instead // of using searchForNeighbors // At each scale, find the extreme values of the DoG within the current neighborhood for (int i_scale = 0; i_scale < nr_scales; ++i_scale) { min_val[i_scale] = std::numeric_limits<float>::max (); max_val[i_scale] = -std::numeric_limits<float>::max (); for (size_t i_neighbor = 0; i_neighbor < nr_nn; ++i_neighbor) { const float &d = diff_of_gauss (nn_indices[i_neighbor], i_scale); min_val[i_scale] = (std::min) (min_val[i_scale], d); max_val[i_scale] = (std::max) (max_val[i_scale], d); } } // If the current point is an extreme value with high enough contrast, add it as a keypoint for (int i_scale = 1; i_scale < nr_scales - 1; ++i_scale) { const float &val = diff_of_gauss (i_point, i_scale); // Does the point have sufficient contrast? if (fabs (val) >= min_contrast_) { // Is it a local minimum? if ((val == min_val[i_scale]) && (val < min_val[i_scale - 1]) && (val < min_val[i_scale + 1])) { extrema_indices.push_back (i_point); extrema_scales.push_back (i_scale); } // Is it a local maximum? else if ((val == max_val[i_scale]) && (val > max_val[i_scale - 1]) && (val > max_val[i_scale + 1])) { extrema_indices.push_back (i_point); extrema_scales.push_back (i_scale); } } } } }
void pcl::recognition::ORROctree::build (const PointCloudIn& points, float voxel_size, const PointCloudN* normals, float enlarge_bounds) { if ( voxel_size <= 0.0f ) return; // Get the bounds of the input point set PointXYZ min, max; getMinMax3D(points, min, max); // Enlarge the bounds a bit to avoid points lying exact on the octree boundaries float eps = enlarge_bounds*std::max (std::max (max.x-min.x, max.y-min.y), max.z-min.z); float b[6] = {min.x-eps, max.x+eps, min.y-eps, max.y+eps, min.z-eps, max.z+eps}; // Build an empty octree with the right boundaries and the right number of levels this->build (b, voxel_size); #ifdef PCL_REC_ORR_OCTREE_VERBOSE printf("ORROctree::%s(): start\n", __func__); printf("point set bounds =\n" "[%f, %f]\n" "[%f, %f]\n" "[%f, %f]\n", min.x, max.x, min.y, max.y, min.z, max.z); #endif size_t num_points = points.size (); // Fill the leaves with the points for (size_t i = 0 ; i < num_points ; ++i ) { // Create a leaf which contains the i-th point. ORROctree::Node* node = this->createLeaf (points[i].x, points[i].y, points[i].z); // Make sure that the point is within some leaf if ( !node ) { fprintf (stderr, "WARNING in 'ORROctree::%s()': the point (%f, %f, %f) should be within the octree bounds!\n", __func__, points[i].x, points[i].y, points[i].z); continue; } // Now, that we have the right leaf -> fill it node->getData ()->addToPoint (points[i].x, points[i].y, points[i].z); if ( normals ) node->getData ()->addToNormal (normals->at(i).normal_x, normals->at(i).normal_y, normals->at(i).normal_z); } // Compute the normals and average points for each full octree node if ( normals ) { for ( auto it = full_leaves_.begin() ; it != full_leaves_.end() ; ) { // Compute the average point in the current octree leaf (*it)->getData ()->computeAveragePoint (); // Compute the length of the average normal float normal_length = aux::length3 ((*it)->getData ()->getNormal ()); // We are suppose to use normals. However, it could be that all normals in this leaf are "illegal", because, // e.g., they were not available in the data set. In this case, remove the leaf from the octree. if ( normal_length <= numeric_limits<float>::epsilon () ) { this->deleteBranch (*it); it = full_leaves_.erase (it); } else { aux::mult3 ((*it)->getData ()->getNormal (), 1.0f/normal_length); ++it; } } } else { // Iterate over all full leaves and average points for (const auto &full_leaf : full_leaves_) full_leaf->getData ()->computeAveragePoint (); } #ifdef PCL_REC_ORR_OCTREE_VERBOSE printf("ORROctree::%s(): end\n", __func__); #endif }