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); } } }