void WorldDownloadManager::separateMesh(Mesh::ConstPtr mesh,PointCloud::Ptr points,TrianglesPtr triangles) { // convert the point cloud pcl::fromPCLPointCloud2(mesh->cloud,*points); if (triangles) { // convert the triangles const uint mesh_size = mesh->polygons.size(); triangles->reserve(mesh_size); for (uint triangle_i = 0; triangle_i < mesh_size; triangle_i++) { const pcl::Vertices &v = mesh->polygons[triangle_i]; if (v.vertices.size() != 3) { ROS_ERROR("WARNING: polygon %u has %u vertices, only triangles are supported.",triangle_i,uint(v.vertices.size())); continue; } kinfu_msgs::KinfuMeshTriangle tri; for (uint i = 0; i < 3; i++) tri.vertex_id[i] = v.vertices[i]; triangles->push_back(tri); } } }
void WorldDownloadManager::cropMesh(const kinfu_msgs::KinfuCloudPoint & min, const kinfu_msgs::KinfuCloudPoint & max,PointCloud::ConstPtr cloud, TrianglesConstPtr triangles,PointCloud::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); std::cout << "Starting with " << cloud_size << " points and " << triangles_size << " triangles.\n"; uint offset; // check the points for (uint i = 0; i < cloud_size; i++) { const pcl::PointXYZ & 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); std::cout << "Ended with " << out_cloud->size() << " points and " << out_triangles->size() << " triangles.\n"; }
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); } }