supervoxel_clustering.cpp

#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>
#include <pcl/point_cloud.h>
#include <pcl/octree/octree_search.h>
#include <pcl/point_cloud.h>
#include <pcl/octree/octree_search.h>
#include <iostream>
#include <vector>
#include <ctime>
#include <iostream>
#include <experimental/filesystem>
namespace fs = std::experimental::filesystem;


//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,
                                       pcl::visualization::PCLVisualizer::Ptr & viewer);


pcl::visualization::PCLVisualizer::Ptr simpleVis (pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr cloud)
{
  // --------------------------------------------
  // -----Open 3D viewer and add point cloud-----
  // --------------------------------------------
  pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
  viewer->setBackgroundColor (0, 0, 0);
  viewer->addPointCloud<pcl::PointXYZRGBA> (cloud, "sample cloud");
  viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
  viewer->addCoordinateSystem (1.0);
  viewer->initCameraParameters ();
  return (viewer);
}


# include <iostream>
# include <dirent.h>
using namespace std;
void directory_contents ( char * directory_path )
{  
  DIR *dh;
  struct dirent * contents; 
  

  
  if ( !dh )
  {
    cout << "The given directory is not found";
    return;
  }
  
  int count = 3;
  while ( ( contents = readdir ( dh ) ) != NULL && count>2)
  {
    string name = contents->d_name;
    cout << name << endl;
  }
  closedir ( dh );
}


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);
  }
  
  std::string toStore;
  std::string toStorePCD;
  std::string name;
  DIR *dh;

  struct dirent * contents; 

  char* directory_path=getcwd(NULL, 0);



  dh = opendir ( directory_path );
  cout << directory_path;
  
  if ( !dh )
  {
    cout << "The given directory is not found";

  }
   
  std::string str(directory_path);
  std::string name3 = "/labels.instances.colored.normals.pcd";
  string allFile;
  

  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);
  while ( ( contents = readdir ( dh ) ) != NULL)
  {
    string name = contents->d_name;

    allFile = directory_path + '/' + name + name3;

        
    toStore = directory_path + '/'+name + "/supervoxel_adj.txt";
    toStorePCD = directory_path +'/'+ name + "/supervoxel.pcd";
     
    ofstream outfile(toStore);

    PointCloudT::Ptr cloud (new PointCloudT);
    pcl::console::print_highlight ("Loading point cloud...\n");
    ifstream ifile;
    ifile.open(allFile);

    if (ifile)
    {
        
        pcl::io::loadPCDFile<PointT> (allFile, *cloud);
 
      //////////////////////////////  //////////////////////////////
      ////// This is how to use supervoxels
      //////////////////////////////  //////////////////////////////

        pcl::SupervoxelClustering<PointT> super (voxel_resolution, seed_resolution);
        if (disable_transform)
            super.setUseSingleCameraTransform (false);
        super.setInputCloud (cloud);
        //cout << " TEST WIDTH OF IMPORT " << cloud->width;
        super.setColorImportance (color_importance);
        super.setSpatialImportance (spatial_importance);
        super.setNormalImportance (normal_importance);
 
        std::map <std::uint32_t, pcl::Supervoxel<PointT>::Ptr > supervoxel_clusters;

        pcl::console::print_highlight ("Extracting supervoxels!\n");
        super.extract (supervoxel_clusters);

        cout << toStorePCD << endl;
      
        PointLCloudT::Ptr labeled_cloud = super.getLabeledCloud(); 
        pcl::io::savePCDFileASCII(toStorePCD, *labeled_cloud);
        pcl::console::print_info ("Found %d supervoxels\n", supervoxel_clusters.size ());

        PointLCloudT::Ptr voxel_labeled_cloud = super.getLabeledVoxelCloud(); 
        //pcl::visualization::PCLVisualizer::Ptr viewer (new pcl::visualization::PCLVisualizer ("3D Viewer"));
        //viewer->setBackgroundColor (0, 0, 0);
        //viewer->addPointCloud (labeled_cloud);

        PointCloudT::Ptr voxel_centroid_cloud = super.getVoxelCentroidCloud ();
        PointLCloudT::Ptr labeled_voxel_cloud = super.getLabeledVoxelCloud ();
        PointNCloudT::Ptr sv_normal_cloud = super.makeSupervoxelNormalCloud (supervoxel_clusters);


        pcl::console::print_highlight ("Getting supervoxel adjacency\n");
        std::multimap<std::uint32_t, std::uint32_t> supervoxel_adjacency;
        super.getSupervoxelAdjacency (supervoxel_adjacency);
        int number = 0;
        int m ;
        //To make a graph of the supervoxel adjacency, we need to iterate through the supervoxel adjacency multimap
        for (auto label_itr = supervoxel_adjacency.cbegin (); label_itr != supervoxel_adjacency.cend (); )
        {

            std::uint32_t supervoxel_label = label_itr->first;

            pcl::Supervoxel<PointT>::Ptr supervoxel = supervoxel_clusters.at (supervoxel_label);

        
            pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGBA>);

            cloud = supervoxel->voxels_;

            number = number + cloud->width;

            std::ostringstream name;
            name << label_itr->first << ".pcd";

            PointCloudT adjacent_supervoxel_centers;
            for (auto adjacent_itr = supervoxel_adjacency.equal_range (supervoxel_label).first; 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_);
                outfile << adjacent_itr->first << "\t" << adjacent_itr->second << "\n";

            }

        //Now we make a name for this polygon
            std::stringstream ss;
            ss << "supervoxel_" << supervoxel_label;
            label_itr = supervoxel_adjacency.upper_bound (supervoxel_label);
            m = supervoxel_label;
        }
      


        outfile.close(); 

      
        pcl::PointCloud<pcl::PointXYZ>::Ptr Changedcloud(new   pcl::PointCloud<pcl::PointXYZ>);
        pcl::io::loadPCDFile<pcl::PointXYZ>(allFile, *Changedcloud);
        //pcl::octree::OctreePointCloudAdjacency<pcl::PointXYZ > deneme(voxel_resolution);
        //deneme.setInputCloud(Changedcloud);
        //deneme.addPointsFromInputCloud();
      
        //pcl::octree::OctreePointCloudAdjacencyContainer<pcl::PointXYZ> *leaf_container;
        //leaf_container = deneme.getLeafContainerAtPoint(Changedcloud->points[1000]);


        //pcl::IndicesPtr indexVectorXX(new std::vector <int>);
        //leaf_container->getPointIndices(*indexVectorXX);

    }
  }
  return (0);
}

void
addSupervoxelConnectionsToViewer (PointT &supervoxel_center,
                                  PointCloudT &adjacent_supervoxel_centers,
                                  std::string supervoxel_name,
                                  pcl::visualization::PCLVisualizer::Ptr & 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
  int i = 0;
  for (auto adjacent_itr = adjacent_supervoxel_centers.begin (); 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 ());
  //cout << "number of points " <<  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);
}