00001 #include <pcl/console/parse.h>
00002 #include <pcl/point_cloud.h>
00003 #include <pcl/point_types.h>
00004 #include <pcl/io/pcd_io.h>
00005 #include <pcl/visualization/pcl_visualizer.h>
00006 #include <pcl/segmentation/supervoxel_clustering.h>
00007
00008
00009 typedef pcl::PointXYZRGBA PointT;
00010 typedef pcl::PointCloud<PointT> PointCloudT;
00011 typedef pcl::PointNormal PointNT;
00012 typedef pcl::PointCloud<PointNT> PointNCloudT;
00013 typedef pcl::PointXYZL PointLT;
00014 typedef pcl::PointCloud<PointLT> PointLCloudT;
00015
00016 void addSupervoxelConnectionsToViewer (PointT &supervoxel_center,
00017 PointCloudT &adjacent_supervoxel_centers,
00018 std::string supervoxel_name,
00019 boost::shared_ptr<pcl::visualization::PCLVisualizer> & viewer);
00020
00021
00022 int
00023 main (int argc, char ** argv)
00024 {
00025 if (argc < 2)
00026 {
00027 pcl::console::print_error ("Syntax is: %s <pcd-file> \n "
00028 "--NT Dsables the single cloud transform \n"
00029 "-v <voxel resolution>\n-s <seed resolution>\n"
00030 "-c <color weight> \n-z <spatial weight> \n"
00031 "-n <normal_weight>\n", argv[0]);
00032 return (1);
00033 }
00034
00035
00036 PointCloudT::Ptr cloud = boost::make_shared <PointCloudT> ();
00037 pcl::console::print_highlight ("Loading point cloud...\n");
00038 if (pcl::io::loadPCDFile<PointT> (argv[1], *cloud))
00039 {
00040 pcl::console::print_error ("Error loading cloud file!\n");
00041 return (1);
00042 }
00043
00044
00045 bool use_transform = ! pcl::console::find_switch (argc, argv, "--NT");
00046
00047 float voxel_resolution = 0.008f;
00048 bool voxel_res_specified = pcl::console::find_switch (argc, argv, "-v");
00049 if (voxel_res_specified)
00050 pcl::console::parse (argc, argv, "-v", voxel_resolution);
00051
00052 float seed_resolution = 0.1f;
00053 bool seed_res_specified = pcl::console::find_switch (argc, argv, "-s");
00054 if (seed_res_specified)
00055 pcl::console::parse (argc, argv, "-s", seed_resolution);
00056
00057 float color_importance = 0.2f;
00058 if (pcl::console::find_switch (argc, argv, "-c"))
00059 pcl::console::parse (argc, argv, "-c", color_importance);
00060
00061 float spatial_importance = 0.4f;
00062 if (pcl::console::find_switch (argc, argv, "-z"))
00063 pcl::console::parse (argc, argv, "-z", spatial_importance);
00064
00065 float normal_importance = 1.0f;
00066 if (pcl::console::find_switch (argc, argv, "-n"))
00067 pcl::console::parse (argc, argv, "-n", normal_importance);
00068
00072
00073 pcl::SupervoxelClustering<PointT> super (voxel_resolution, seed_resolution, use_transform);
00074 super.setInputCloud (cloud);
00075 super.setColorImportance (color_importance);
00076 super.setSpatialImportance (spatial_importance);
00077 super.setNormalImportance (normal_importance);
00078
00079 std::map <uint32_t, pcl::Supervoxel<PointT>::Ptr > supervoxel_clusters;
00080
00081 pcl::console::print_highlight ("Extracting supervoxels!\n");
00082 super.extract (supervoxel_clusters);
00083 pcl::console::print_info ("Found %d supervoxels\n", supervoxel_clusters.size ());
00084
00085 boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
00086 viewer->setBackgroundColor (0, 0, 0);
00087
00088 PointCloudT::Ptr voxel_centroid_cloud = super.getVoxelCentroidCloud ();
00089 viewer->addPointCloud (voxel_centroid_cloud, "voxel centroids");
00090 viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,2.0, "voxel centroids");
00091 viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY,0.95, "voxel centroids");
00092
00093 PointCloudT::Ptr colored_voxel_cloud = super.getColoredVoxelCloud ();
00094 viewer->addPointCloud (colored_voxel_cloud, "colored voxels");
00095 viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY,0.8, "colored voxels");
00096
00097 PointNCloudT::Ptr sv_normal_cloud = super.makeSupervoxelNormalCloud (supervoxel_clusters);
00098
00099
00100
00101 pcl::console::print_highlight ("Getting supervoxel adjacency\n");
00102 std::multimap<uint32_t, uint32_t> supervoxel_adjacency;
00103 super.getSupervoxelAdjacency (supervoxel_adjacency);
00104
00105 std::multimap<uint32_t,uint32_t>::iterator label_itr = supervoxel_adjacency.begin ();
00106 for ( ; label_itr != supervoxel_adjacency.end (); )
00107 {
00108
00109 uint32_t supervoxel_label = label_itr->first;
00110
00111 pcl::Supervoxel<PointT>::Ptr supervoxel = supervoxel_clusters.at (supervoxel_label);
00112
00113
00114 PointCloudT adjacent_supervoxel_centers;
00115 std::multimap<uint32_t,uint32_t>::iterator adjacent_itr = supervoxel_adjacency.equal_range (supervoxel_label).first;
00116 for ( ; adjacent_itr!=supervoxel_adjacency.equal_range (supervoxel_label).second; ++adjacent_itr)
00117 {
00118 pcl::Supervoxel<PointT>::Ptr neighbor_supervoxel = supervoxel_clusters.at (adjacent_itr->second);
00119 adjacent_supervoxel_centers.push_back (neighbor_supervoxel->centroid_);
00120 }
00121
00122 std::stringstream ss;
00123 ss << "supervoxel_" << supervoxel_label;
00124
00125 addSupervoxelConnectionsToViewer (supervoxel->centroid_, adjacent_supervoxel_centers, ss.str (), viewer);
00126
00127 label_itr = supervoxel_adjacency.upper_bound (supervoxel_label);
00128 }
00129
00130 while (!viewer->wasStopped ())
00131 {
00132 viewer->spinOnce (100);
00133 }
00134 return (0);
00135 }
00136
00137 void
00138 addSupervoxelConnectionsToViewer (PointT &supervoxel_center,
00139 PointCloudT &adjacent_supervoxel_centers,
00140 std::string supervoxel_name,
00141 boost::shared_ptr<pcl::visualization::PCLVisualizer> & viewer)
00142 {
00143 vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New ();
00144 vtkSmartPointer<vtkCellArray> cells = vtkSmartPointer<vtkCellArray>::New ();
00145 vtkSmartPointer<vtkPolyLine> polyLine = vtkSmartPointer<vtkPolyLine>::New ();
00146
00147
00148 PointCloudT::iterator adjacent_itr = adjacent_supervoxel_centers.begin ();
00149 for ( ; adjacent_itr != adjacent_supervoxel_centers.end (); ++adjacent_itr)
00150 {
00151 points->InsertNextPoint (supervoxel_center.data);
00152 points->InsertNextPoint (adjacent_itr->data);
00153 }
00154
00155 vtkSmartPointer<vtkPolyData> polyData = vtkSmartPointer<vtkPolyData>::New ();
00156
00157 polyData->SetPoints (points);
00158 polyLine->GetPointIds ()->SetNumberOfIds(points->GetNumberOfPoints ());
00159 for(unsigned int i = 0; i < points->GetNumberOfPoints (); i++)
00160 polyLine->GetPointIds ()->SetId (i,i);
00161 cells->InsertNextCell (polyLine);
00162
00163 polyData->SetLines (cells);
00164 viewer->addModelFromPolyData (polyData,supervoxel_name);
00165 }
00166
00167