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