pcl2::Neighborhood
pcl2::computeFixedRadiusNeighborhood (Cloud & cloud, const MatF & query, float r)
{
// Convert point cloud
MatF xyz = cloud["xyz"];
assert (xyz.rows () >= 1);
assert (xyz.cols () == 3);
pcl::PointCloud<pcl::PointXYZ>::Ptr input (new pcl::PointCloud<pcl::PointXYZ>);
input->width = cloud.size ();
input->height = 1;
input->is_dense = false;
input->points.resize (cloud.size ());
for (size_t i = 0; i < xyz.rows (); ++i)
{
input->points[i].x = xyz (i, 0);
input->points[i].y = xyz (i, 1);
input->points[i].z = xyz (i, 2);
}
// Convert query point
assert (query.rows () == 1);
assert (query.cols () == 3);
pcl::PointXYZ q;
q.x = query (0, 0);
q.y = query (0, 1);
q.z = query (0, 2);
// Perform neighbor search
pcl::KdTreeFLANN<pcl::PointXYZ> tree;
tree.setInputCloud (input);
std::vector<int> idx_vec;
std::vector<float> dist_vec;
size_t k = (size_t) tree.radiusSearch (q, r, idx_vec, dist_vec);
assert (k == idx_vec.size ());
// Convert output
EigenMat<int> neighbor_indices (k, 1);
EigenMat<float> squared_distances (k, 1);
for (size_t i = 0; i < k; ++i)
{
neighbor_indices (i, 0) = idx_vec[i];
squared_distances (i, 0) = dist_vec[i];
}
//Cloud neighborhood = cloud (neighbor_indices);
Neighborhood neighborhood (cloud, neighbor_indices);
neighborhood.insert ("dist", squared_distances);
return (neighborhood);
}
void Application::dfs(SparseGraph &G,VertexId v,Array1D<bool> &mark,
Neighborhood &component)
{
Stack<VertexId> S;
S.push(v);
mark[v]=true;
while(!S.isEmpty()) {
VertexId v=S.pop();
component.insert(v);
bool shouldDelete;
Neighborhood &children=*G.getNeighborsOf(v,shouldDelete);
for(Neighborhood::iterator cur=children.begin(), end=children.end() ;
cur!=end ; ++cur) {
VertexId child=*cur;
if(mark[child]) continue;
S.push(child);
mark[child]=true;
//component.insert(child);
}
}
}
