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;
}
}
}
}
void
pcl_ros::MovingLeastSquares::input_indices_callback (const PointCloudInConstPtr &cloud,
const PointIndicesConstPtr &indices)
{
// No subscribers, no work
if (pub_output_.getNumSubscribers () <= 0 && pub_normals_.getNumSubscribers () <= 0)
return;
// Output points have the same type as the input, they are only smoothed
PointCloudIn output;
// Normals are also estimated and published on a separate topic
NormalCloudOut::Ptr normals (new NormalCloudOut ());
// If cloud is given, check if it's valid
if (!isValid (cloud))
{
NODELET_ERROR ("[%s::input_indices_callback] Invalid input!", getName ().c_str ());
output.header = cloud->header;
pub_output_.publish (output.makeShared ());
return;
}
// If indices are given, check if they are valid
if (indices && !isValid (indices, "indices"))
{
NODELET_ERROR ("[%s::input_indices_callback] Invalid indices!", getName ().c_str ());
output.header = cloud->header;
pub_output_.publish (output.makeShared ());
return;
}
/// DEBUG
if (indices)
NODELET_DEBUG ("[%s::input_indices_model_callback]\n"
" - PointCloud with %d data points (%s), stamp %f, and frame %s on topic %s received.\n"
" - PointIndices with %zu values, stamp %f, and frame %s on topic %s received.",
getName ().c_str (),
cloud->width * cloud->height, pcl::getFieldsList (*cloud).c_str (), fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str (),
indices->indices.size (), indices->header.stamp.toSec (), indices->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("indices").c_str ());
else
NODELET_DEBUG ("[%s::input_callback] PointCloud with %d data points, stamp %f, and frame %s on topic %s received.", getName ().c_str (), cloud->width * cloud->height, fromPCL(cloud->header).stamp.toSec (), cloud->header.frame_id.c_str (), getMTPrivateNodeHandle ().resolveName ("input").c_str ());
///
// Reset the indices and surface pointers
impl_.setInputCloud (cloud);
IndicesPtr indices_ptr;
if (indices)
indices_ptr.reset (new std::vector<int> (indices->indices));
impl_.setIndices (indices_ptr);
// Initialize the spatial locator
// Do the reconstructon
//impl_.process (output);
// Publish a Boost shared ptr const data
// Enforce that the TF frame and the timestamp are copied
output.header = cloud->header;
pub_output_.publish (output.makeShared ());
normals->header = cloud->header;
pub_normals_.publish (normals);
}
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
}
