void WorldDownloadManager::cropCloudWithSphere(const Eigen::Vector3f & center,const float radius, typename pcl::PointCloud<PointT>::ConstPtr cloud,typename pcl::PointCloud<PointT>::Ptr out_cloud) { const uint cloud_size = cloud->size(); std::vector<bool> valid_points(cloud_size,true); // check the points for (uint i = 0; i < cloud_size; i++) { const PointT & pt = (*cloud)[i]; const Eigen::Vector3f ept(pt.x,pt.y,pt.z); if ((ept - center).squaredNorm() > radius * radius) valid_points[i] = false; } // discard invalid points out_cloud->clear(); out_cloud->reserve(cloud_size); uint count = 0; for (uint i = 0; i < cloud_size; i++) if (valid_points[i]) { out_cloud->push_back((*cloud)[i]); count++; } out_cloud->resize(count); }
void WorldDownloadManager::cropCloud(const Eigen::Vector3f & min,const Eigen::Vector3f & max, typename pcl::PointCloud<PointT>::ConstPtr cloud,typename pcl::PointCloud<PointT>::Ptr out_cloud) { const uint cloud_size = cloud->size(); std::vector<bool> valid_points(cloud_size,true); // check the points for (uint i = 0; i < cloud_size; i++) { const PointT & pt = (*cloud)[i]; if (pt.x > max.x() || pt.y > max.y() || pt.z > max.z() || pt.x < min.x() || pt.y < min.y() || pt.z < min.z()) valid_points[i] = false; } // discard invalid points out_cloud->clear(); out_cloud->reserve(cloud_size); uint count = 0; for (uint i = 0; i < cloud_size; i++) if (valid_points[i]) { out_cloud->push_back((*cloud)[i]); count++; } out_cloud->resize(count); }
template <typename PointT> void pcl::SupervoxelClustering<PointT>::SupervoxelHelper::getNormals (typename pcl::PointCloud<Normal>::Ptr &normals) const { normals.reset (new pcl::PointCloud<Normal>); normals->clear (); normals->resize (leaves_.size ()); typename SupervoxelHelper::const_iterator leaf_itr; typename pcl::PointCloud<Normal>::iterator normal_itr = normals->begin (); for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr, ++normal_itr) { const VoxelData& leaf_data = (*leaf_itr)->getData (); leaf_data.getNormal (*normal_itr); } }
template <typename PointT> void pcl::SupervoxelClustering<PointT>::SupervoxelHelper::getNormals (typename pcl::PointCloud<Normal>::Ptr &normals) const { normals = boost::make_shared<pcl::PointCloud<Normal> > (); normals->clear (); normals->resize (leaves_.size ()); typename std::set<LeafContainerT*>::iterator leaf_itr; typename pcl::PointCloud<Normal>::iterator normal_itr = normals->begin (); for (leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr, ++normal_itr) { const VoxelData& leaf_data = (*leaf_itr)->getData (); leaf_data.getNormal (*normal_itr); } }
template <typename PointT> void pcl::SupervoxelClustering<PointT>::SupervoxelHelper::getVoxels (typename pcl::PointCloud<PointT>::Ptr &voxels) const { voxels = boost::make_shared<pcl::PointCloud<PointT> > (); voxels->clear (); voxels->resize (leaves_.size ()); typename pcl::PointCloud<PointT>::iterator voxel_itr = voxels->begin (); //typename std::set<LeafContainerT*>::iterator leaf_itr; for (typename std::set<LeafContainerT*>::const_iterator leaf_itr = leaves_.begin (); leaf_itr != leaves_.end (); ++leaf_itr, ++voxel_itr) { const VoxelData& leaf_data = (*leaf_itr)->getData (); leaf_data.getPoint (*voxel_itr); } }
void WorldDownloadManager::removeDuplicatePoints(typename pcl::PointCloud<PointT>::ConstPtr cloud, TrianglesConstPtr triangles,typename pcl::PointCloud<PointT>::Ptr out_cloud,TrianglesPtr out_triangles) { const uint64 input_size = cloud->size(); std::vector<uint64> ordered(input_size); for (uint64 i = 0; i < input_size; i++) ordered[i] = i; std::sort(ordered.begin(),ordered.end(),WorldDownloadManager_mergeMeshTrianglesComparator<PointT>(*cloud)); typedef WorldDownloadManager_mergeMeshTrianglesAverageClass<PointT> AverageClass; std::vector<uint64> re_association_vector(input_size); std::vector<AverageClass, Eigen::aligned_allocator<AverageClass> > average_vector; uint64 output_i = 0; re_association_vector[ordered[0]] = 0; for (uint64 src_i = 1; src_i < input_size; src_i++) { const PointT & prev = (*cloud)[ordered[src_i - 1]]; const PointT & current = (*cloud)[ordered[src_i]]; const int not_equal = std::memcmp(prev.data,current.data,sizeof(float) * 3); if (not_equal) output_i++; re_association_vector[ordered[src_i]] = output_i; } const uint64 output_size = output_i + 1; out_cloud->resize(output_size); average_vector.resize(output_size); for (uint64 i = 0; i < input_size; i++) average_vector[re_association_vector[i]].insert((*cloud)[i]); for (uint64 i = 0; i < output_size; i++) (*out_cloud)[i] = average_vector[i].get(); if (out_triangles && triangles) { const uint64 triangles_size = triangles->size(); uint64 out_triangles_size = 0; out_triangles->resize(triangles_size); for (uint64 i = 0; i < triangles_size; i++) { kinfu_msgs::KinfuMeshTriangle new_tri; for (uint h = 0; h < 3; h++) new_tri.vertex_id[h] = re_association_vector[(*triangles)[i].vertex_id[h]]; bool degenerate = false; for (uint h = 0; h < 3; h++) if (new_tri.vertex_id[h] == new_tri.vertex_id[(h + 1) % 3]) degenerate = true; if (!degenerate) { (*out_triangles)[out_triangles_size] = new_tri; out_triangles_size++; } } out_triangles->resize(out_triangles_size); } }
void WorldDownloadManager::cropMesh(const kinfu_msgs::KinfuCloudPoint & min, const kinfu_msgs::KinfuCloudPoint & max,typename pcl::PointCloud<PointT>::ConstPtr cloud, TrianglesConstPtr triangles,typename pcl::PointCloud<PointT>::Ptr out_cloud, TrianglesPtr out_triangles) { const uint triangles_size = triangles->size(); const uint cloud_size = cloud->size(); std::vector<bool> valid_points(cloud_size,true); std::vector<uint> valid_points_remap(cloud_size,0); uint offset; // check the points for (uint i = 0; i < cloud_size; i++) { const PointT & pt = (*cloud)[i]; if (pt.x > max.x || pt.y > max.y || pt.z > max.z || pt.x < min.x || pt.y < min.y || pt.z < min.z) valid_points[i] = false; } // discard invalid points out_cloud->clear(); out_cloud->reserve(cloud_size); offset = 0; for (uint i = 0; i < cloud_size; i++) if (valid_points[i]) { out_cloud->push_back((*cloud)[i]); // save new position for triangles remap valid_points_remap[i] = offset; offset++; } out_cloud->resize(offset); // discard invalid triangles out_triangles->clear(); out_triangles->reserve(triangles_size); offset = 0; for (uint i = 0; i < triangles_size; i++) { const kinfu_msgs::KinfuMeshTriangle & tri = (*triangles)[i]; bool is_valid = true; // validate all the vertices for (uint h = 0; h < 3; h++) if (!valid_points[tri.vertex_id[h]]) { is_valid = false; break; } if (is_valid) { kinfu_msgs::KinfuMeshTriangle out_tri; // remap the triangle for (uint h = 0; h < 3; h++) out_tri.vertex_id[h] = valid_points_remap[(*triangles)[i].vertex_id[h]]; out_triangles->push_back(out_tri); offset++; } } out_triangles->resize(offset); }