本文整理汇总了C++中pcl::search::KdTree::radiusSearch方法的典型用法代码示例。如果您正苦于以下问题：C++ KdTree::radiusSearch方法的具体用法？C++ KdTree::radiusSearch怎么用？C++ KdTree::radiusSearch使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类pcl::search::KdTree的用法示例。
在下文中一共展示了KdTree::radiusSearch方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: extractEuclideanClusters
void extractEuclideanClusters (
PointCloud<PointXYZRGB >::Ptr cloud, pcl::PointCloud<pcl::Normal >::Ptr normals,
pcl::search::KdTree<PointXYZRGB >::Ptr tree,
float tolerance, std::vector<pcl::PointIndices > &clusters, double eps_angle,
unsigned int min_pts_per_cluster = 1,
unsigned int max_pts_per_cluster = (std::numeric_limits<int >::max) ())
{
// \note If the tree was created over <cloud, indices>, we guarantee a 1-1 mapping between what the tree returns
//and indices[i]
float adjTolerance = 0;
// Create a bool vector of processed point indices, and initialize it to false
std::vector<bool > processed(cloud->points.size(), false);
std::vector<int> nn_indices;
std::vector<float> nn_distances;
// Process all points in the indices vector
std::cout << "Point size is " << cloud->points.size () << std::endl;
for (size_t i = 0; i < cloud->points.size (); ++i)
{
if(processed[i])
continue;
std::vector<int > seed_queue;
int sq_idx = 0;
seed_queue.push_back(i);
processed[i] = true;
int cnt = 0;
while (sq_idx < (int)seed_queue.size())
{
cnt++;
// Search for sq_idx
// adjTolerance = cloud->points[seed_queue[sq_idx]].distance * tolerance;
adjTolerance = tolerance;
if (!tree->radiusSearch(seed_queue[sq_idx], adjTolerance, nn_indices, nn_distances))
{
sq_idx++;
continue;
}
for(size_t j = 1; j < nn_indices.size (); ++j) // nn_indices[0] should be sq_idx
{
if (processed[nn_indices[j]]) // Has this point been processed before ?
continue;
processed[nn_indices[j]] = true;
// [-1;1]
double dot_p =
normals->points[i].normal[0] * normals->points[nn_indices[j]].normal[0] +
normals->points[i].normal[1] * normals->points[nn_indices[j]].normal[1] +
normals->points[i].normal[2] * normals->points[nn_indices[j]].normal[2];
if ( fabs (acos (dot_p)) < eps_angle )
{
processed[nn_indices[j]] = true;
seed_queue.push_back (nn_indices[j]);
}
}
sq_idx++;
}
// If this queue is satisfactory, add to the clusters
if (seed_queue.size () >= min_pts_per_cluster && seed_queue.size () <= max_pts_per_cluster)
{
pcl::PointIndices r;
r.indices.resize (seed_queue.size ());
for (size_t j = 0; j < seed_queue.size (); ++j)
r.indices[j] = seed_queue[j];
sort (r.indices.begin (), r.indices.end ());
r.indices.erase (unique (r.indices.begin (), r.indices.end ()), r.indices.end ());
r.header = cloud->header;
//ROS_INFO ("cluster of size %d data point\n ",r.indices.size());
clusters.push_back(r);
}
}
}