PCL_supervoxelclustering.zip_PCL 分割_supervoxel_点云_点云分割_点云超体素

#include <pcl/console/parse.h> #include <pcl/point_cloud.h> #include <pcl/point_types.h> #include <pcl/io/pcd_io.h> #include <pcl/visualization/pcl_visualizer.h> #include <pcl/segmentation/supervoxel_clustering.h> //VTK include needed for drawing graph lines #include <vtkPolyLine.h> // Types typedef pcl::PointXYZRGBA PointT; typedef pcl::PointCloud<PointT> PointCloudT; typedef pcl::PointNormal PointNT; typedef pcl::PointCloud<PointNT> PointNCloudT; typedef pcl::PointXYZL PointLT; typedef pcl::PointCloud<PointLT> PointLCloudT; void addSupervoxelConnectionsToViewer(PointT &supervoxel_center, PointCloudT &adjacent_supervoxel_centers, std::string supervoxel_name, boost::shared_ptr<pcl::visualization::PCLVisualizer> & viewer); int main(int argc, char ** argv) { if (argc < 2) { pcl::console::print_error("Syntax is: %s <pcd-file> \n " "--NT Dsables the single cloud transform \n" "-v <voxel resolution>\n-s <seed resolution>\n" "-c <color weight> \n-z <spatial weight> \n" "-n <normal_weight>\n", argv[0]); return (1); } PointCloudT::Ptr cloud = boost::shared_ptr <PointCloudT>(new PointCloudT()); pcl::console::print_highlight("Loading point cloud...\n"); if (pcl::io::loadPCDFile<PointT>(argv[1], *cloud)) { pcl::console::print_error("Error loading cloud file!\n"); return (1); } bool disable_transform = pcl::console::find_switch(argc, argv, "--NT"); float voxel_resolution = 0.008f; bool voxel_res_specified = pcl::console::find_switch(argc, argv, "-v"); if (voxel_res_specified) pcl::console::parse(argc, argv, "-v", voxel_resolution); float seed_resolution = 0.1f; bool seed_res_specified = pcl::console::find_switch(argc, argv, "-s"); if (seed_res_specified) pcl::console::parse(argc, argv, "-s", seed_resolution); float color_importance = 0.2f; if (pcl::console::find_switch(argc, argv, "-c")) pcl::console::parse(argc, argv, "-c", color_importance); float spatial_importance = 0.4f; if (pcl::console::find_switch(argc, argv, "-z")) pcl::console::parse(argc, argv, "-z", spatial_importance); float normal_importance = 1.0f; if (pcl::console::find_switch(argc, argv, "-n")) pcl::console::parse(argc, argv, "-n", normal_importance); ////////////////////////////// ////////////////////////////// ////// This is how to use supervoxels ////////////////////////////// ////////////////////////////// pcl::SupervoxelClustering<PointT> super(voxel_resolution, seed_resolution); if (disable_transform) super.setUseSingleCameraTransform(false); super.setInputCloud(cloud); super.setColorImportance(color_importance); super.setSpatialImportance(spatial_importance); super.setNormalImportance(normal_importance); std::map <uint32_t, pcl::Supervoxel<PointT>::Ptr > supervoxel_clusters; pcl::console::print_highlight("Extracting supervoxels!\n"); super.extract(supervoxel_clusters); pcl::console::print_info("Found %d supervoxels\n", supervoxel_clusters.size()); boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer")); viewer->setBackgroundColor(0, 0, 0); PointCloudT::Ptr voxel_centroid_cloud = super.getVoxelCentroidCloud(); viewer->addPointCloud(voxel_centroid_cloud, "voxel centroids"); viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2.0, "voxel centroids"); viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.95, "voxel centroids"); PointLCloudT::Ptr labeled_voxel_cloud = super.getLabeledVoxelCloud(); viewer->addPointCloud(labeled_voxel_cloud, "labeled voxels"); viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.8, "labeled voxels"); PointNCloudT::Ptr sv_normal_cloud = super.makeSupervoxelNormalCloud(supervoxel_clusters); //We have this disabled so graph is easy to see, uncomment to see supervoxel normals //viewer->addPointCloudNormals<PointNormal> (sv_normal_cloud,1,0.05f, "supervoxel_normals"); pcl::console::print_highlight("Getting supervoxel adjacency\n"); std::multimap<uint32_t, uint32_t> supervoxel_adjacency; super.getSupervoxelAdjacency(supervoxel_adjacency); //To make a graph of the supervoxel adjacency, we need to iterate through the supervoxel adjacency multimap std::multimap<uint32_t, uint32_t>::iterator label_itr = supervoxel_adjacency.begin(); for (; label_itr != supervoxel_adjacency.end(); ) { //First get the label uint32_t supervoxel_label = label_itr->first; //Now get the supervoxel corresponding to the label pcl::Supervoxel<PointT>::Ptr supervoxel = supervoxel_clusters.at(supervoxel_label); //Now we need to iterate through the adjacent supervoxels and make a point cloud of them PointCloudT adjacent_supervoxel_centers; std::multimap<uint32_t, uint32_t>::iterator adjacent_itr = supervoxel_adjacency.equal_range(supervoxel_label).first; for (; adjacent_itr != supervoxel_adjacency.equal_range(supervoxel_label).second; ++adjacent_itr) { pcl::Supervoxel<PointT>::Ptr neighbor_supervoxel = supervoxel_clusters.at(adjacent_itr->second); adjacent_supervoxel_centers.push_back(neighbor_supervoxel->centroid_); } //Now we make a name for this polygon std::stringstream ss; ss << "supervoxel_" << supervoxel_label; //This function is shown below, but is beyond the scope of this tutorial - basically it just generates a "star" polygon mesh from the points given addSupervoxelConnectionsToViewer(supervoxel->centroid_, adjacent_supervoxel_centers, ss.str(), viewer); //Move iterator forward to next label label_itr = supervoxel_adjacency.upper_bound(supervoxel_label); } while (!viewer->wasStopped()) { viewer->spinOnce(100); } return (0); } void addSupervoxelConnectionsToViewer(PointT &supervoxel_center, PointCloudT &adjacent_supervoxel_centers, std::string supervoxel_name, boost::shared_ptr<pcl::visualization::PCLVisualizer> & viewer) { vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New(); vtkSmartPointer<vtkCellArray> cells = vtkSmartPointer<vtkCellArray>::New(); vtkSmartPointer<vtkPolyLine> polyLine = vtkSmartPointer<vtkPolyLine>::New(); //Iterate through all adjacent points, and add a center point to adjacent point pair PointCloudT::iterator adjacent_itr = adjacent_supervoxel_centers.begin(); for (; adjacent_itr != adjacent_supervoxel_centers.end(); ++adjacent_itr) { points->InsertNextPoint(supervoxel_center.data); points->InsertNextPoint(adjacent_itr->data); } // Create a polydata to store everything in vtkSmartPointer<vtkPolyData> polyData = vtkSmartPointer<vtkPolyData>::New(); // Add the points to the dataset polyData->SetPoints(points); polyLine->GetPointIds()->SetNumberOfIds(points->GetNumberOfPoints()); for (unsigned int i = 0; i < points->GetNumberOfPoints(); i++) polyLine->GetPointIds()->SetId(i, i); cells->InsertNextCell(polyLine); // Add the lines to the dataset polyData->SetLines(cells); viewer->addModelFromPolyData(polyData, supervoxel_name); }#include <pcl/io/pcd_io.h> #include <pcl/point_cloud.h> #include <pcl/point_types.h> #include <pcl/visualization/pcl_visualizer.h> typedef pcl::PointXYZRGBA PointT; typedef pcl::PointCloud<PointT> PointCloudT; // Mutex: // boost::mutex cloud_mutex; struct callback_args { // structure used to pass arguments to the callback function PointCloudT::Ptr clicked_points_3d; pcl::visualization::PCLVisualizer::Ptr viewerPtr; }; void pp_callback(const pcl::visualization::PointPickingEvent& event, void* args) { struct callback_args* data = (struct callback_args *)args; if (event.getPointIndex() == -1) return; PointT current_point; event.getPoint(current_point.x, current_point.y, current_point.z); data->clicked_points_3d->points.push_back(current_point); // Draw clicked points in red: pcl::visu