text: // Align a rigid object to a scene with clutter and occlusions
  
  #include <iostream>
  #include <ros/ros.h>
  #include <Eigen/Core>
  #include <vector>
  //#include <Matrix.h>
  #include <sensor_msgs/PointCloud2.h>
  #include <pcl_conversions/pcl_conversions.h>
  #include <pcl/ModelCoefficients.h>
  #include <pcl/point_types.h>
  #include <pcl/io/pcd_io.h>
  #include <pcl/filters/extract_indices.h>
  #include <pcl/filters/voxel_grid.h>
  #include <pcl/features/normal_3d.h>
  #include <pcl/kdtree/kdtree.h>
  #include <pcl/sample_consensus/method_types.h>
  #include <pcl/sample_consensus/model_types.h>
  #include <pcl/segmentation/sac_segmentation.h>
  #include <pcl/segmentation/extract_clusters.h>
  #include <pcl/visualization/cloud_viewer.h>
  //#include <pcl/common/centroid.h>
  #include <pcl/registration/icp.h>
  #include <pcl/point_cloud.h>
  #include "pcl/common/transformation_from_correspondences.h"
  //#include <boost/thread/thread.hpp>
  #include <pcl/filters/project_inliers.h>
  #include <pcl/common/time.h>
  #include <pcl/console/print.h>
  #include <pcl/features/normal_3d_omp.h>
  #include <pcl/features/fpfh_omp.h>
  #include <pcl/filters/filter.h>
  #include <pcl/registration/icp.h>
  #include <pcl/registration/sample_consensus_prerejective.h>
  #include <pcl/visualization/pcl_visualizer.h>
  #include <vtkCellArray.h>
  #include <iostream>
  #include <ros/ros.h>
  #include <Eigen/Core>
  #include <vector>
  //#include <Matrix.h>
  #include <sensor_msgs/PointCloud2.h>
  #include <pcl_conversions/pcl_conversions.h>
  #include <pcl/ModelCoefficients.h>
  #include <pcl/point_types.h>
  #include <pcl/io/pcd_io.h>
  #include <pcl/filters/extract_indices.h>
  #include <pcl/filters/voxel_grid.h>
  #include <pcl/features/normal_3d.h>
  #include <pcl/kdtree/kdtree.h>
  #include <pcl/sample_consensus/method_types.h>
  #include <pcl/sample_consensus/model_types.h>
  #include <pcl/segmentation/sac_segmentation.h>
  #include <pcl/segmentation/extract_clusters.h>
  #include <pcl/visualization/cloud_viewer.h>
  //#include <pcl/common/centroid.h>
  
  #include <boost/thread/thread.hpp>
  
  
  typedef pcl::PointNormal PointNT;
  typedef pcl::PointCloud<PointNT> PointCloudT;
  typedef pcl::FPFHSignature33 FeatureT;
  typedef pcl::FPFHEstimationOMP<PointNT,PointNT,FeatureT> FeatureEstimationT;
  typedef pcl::PointCloud<FeatureT> FeatureCloudT;
  typedef pcl::visualization::PointCloudColorHandlerCustom<PointNT> ColorHandlerT;
  
  PointCloudT::Ptr object (new PointCloudT);
  
  pcl::PointCloud<pcl::PointXYZ>::Ptr cluster (new pcl::PointCloud<pcl::PointXYZ>);
  
  void
  cloud_cb (const pcl::PCLPointCloud2ConstPtr& cloud_blob)
  {
    // Read in the cloud data
  
    PointCloudT::Ptr object_aligned (new PointCloudT);
    PointCloudT::Ptr scene (new PointCloudT);
    FeatureCloudT::Ptr object_features (new FeatureCloudT);
    FeatureCloudT::Ptr scene_features (new FeatureCloudT);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cluster (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
    //pcl::PCLPointCloud2::Ptr cloud_filtered_blob (new pcl::PCLPointCloud2);
    pcl::fromPCLPointCloud2 (*cloud_blob, *cloud);
    std::cout << "PointCloud before filtering has: " << cloud->points.size () << " data points." << std::endl; //*
  
    // Create the filtering object: downsample the dataset using a leaf size of 1cm
    pcl::VoxelGrid<pcl::PointXYZ> vg;
    
    vg.setInputCloud (cloud);
    vg.setLeafSize (0.01f, 0.01f, 0.01f);
    vg.filter (*cloud_filtered);
    std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size ()  << " data points." << std::endl; //*
  
    // Create the segmentation object for the planar model and set all the parameters
    pcl::SACSegmentation<pcl::PointXYZ> seg;
    pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
    pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ());
    pcl::PCDWriter writer;
    seg.setOptimizeCoefficients (true);
    seg.setModelType (pcl::SACMODEL_PLANE);
    seg.setMethodType (pcl::SAC_RANSAC);
    seg.setMaxIterations (100);
    seg.setDistanceThreshold (0.02);
  
    int i=0, nr_points = (int) cloud_filtered->points.size ();
    while (cloud_filtered->points.size () > 0.3 * nr_points)
    {
      // Segment the largest planar component from the remaining cloud
      seg.setInputCloud (cloud_filtered);
      seg.segment (*inliers, *coefficients);
      if (inliers->indices.size () == 0)
      {
        std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
        break;
      }
  
      // Extract the planar inliers from the input cloud
      pcl::ExtractIndices<pcl::PointXYZ> extract;
      extract.setInputCloud (cloud_filtered);
      extract.setIndices (inliers);
      extract.setNegative (false);
  
      // Get the points associated with the planar surface
      extract.filter (*cloud_plane);
      std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;
  
      // Remove the planar inliers, extract the rest
      extract.setNegative (true);
      extract.filter (*cloud_f);
      *cloud_filtered = *cloud_f;
    }
  
    // Creating the KdTree object for the search method of the extraction
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
    tree->setInputCloud (cloud_filtered);
  
    std::vector<pcl::PointIndices> cluster_indices;
    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
    ec.setClusterTolerance (0.02); // 2cm
    ec.setMinClusterSize (100);
    ec.setMaxClusterSize (25000);
    ec.setSearchMethod (tree);
    ec.setInputCloud (cloud_filtered);
    ec.extract (cluster_indices);
  
    int j = 0;
    //cloud_filtered_blob = cloud_plane;
    for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
    {
      pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
      for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
        cloud_cluster->points.push_back (cloud_filtered->points[*pit]); //*
      cloud_cluster->width = cloud_cluster->points.size ();
      cloud_cluster->height = 1;
      cloud_cluster->is_dense = true;
        //uint8_t r = 0, g = 255, b = 0;
        //uint32_t rgb = ((uint32_t)r << 16 | (uint32_t)g << 8 | (uint32_t)b);
        //cloud_filtered->at(*it).rgb = *reinterpret_cast<float*>(&rgb);    
      
      std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;
      std::stringstream ss;
      ss << "cloud_cluster_" << j << ".pcd";
      writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false); //* ---you should be able to write a  normal cloud here  pcl::PCDWriter writer;   writer.write<pcl::PointNormal> (ss.str (), *cloud_cluster_don, false);
      //pcl_conversions::toPCL(*cloud_msg, *cloud);
      //pcl::toPCLPointCloud2 (*cloud_cluster, *cloud_filtered_blob);
      //pcl::concatenatePointCloud (cloud_cluster,cloud_filtered_blob, cloud_filtered_blob);
      pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer");
      viewer.showCloud (cloud_cluster);
      while (!viewer.wasStopped ())
      {
      }
      //pcl::compute3DCentroid(*cloud_cluster, const pcl::PointIndices& *indices, Eigen::Matrix<float,4,1>& centroid); 	
      
  
      j++;
    }
    //////////////---conversion of the cloud to cloud with normals
    // Normal estimation*
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
    pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_cloud (new pcl::search::KdTree<pcl::PointXYZ>);
    tree_cloud->setInputCloud (cloud);
    n.setInputCloud (cloud);
    n.setSearchMethod (tree_cloud);
    n.setKSearch (20);
    n.compute (*normals);
    //* normals should not contain the point normals + surface curvatures
  
    // Concatenate the XYZ and normal fields*
    pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals (new pcl::PointCloud<pcl::PointNormal>);
    pcl::concatenateFields (*cloud, *normals, *cloud_with_normals);
    //* cloud_with_normals = cloud + normals
    
    ////////-----pose estimation
    scene = cloud_with_normals;
  
    //pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
    pcl::PointCloud<pcl::Normal>::Ptr normals_cluster (new pcl::PointCloud<pcl::Normal>);
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree_cluster (new pcl::search::KdTree<pcl::PointXYZ>);
    tree_cluster->setInputCloud (cluster);
    n.setInputCloud (cluster);
    n.setSearchMethod (tree_cluster);
    n.setKSearch (20);
    n.compute (*normals_cluster);
    //* normals should not contain the point normals + surface curvatures
  
    // Concatenate the XYZ and normal fields*
    pcl::PointCloud<pcl::PointNormal>::Ptr cluster_with_normals (new pcl::PointCloud<pcl::PointNormal>);
    pcl::concatenateFields (*cluster, *normals_cluster, *cluster_with_normals);
    
  
    object = cluster_with_normals;
    // Downsample
    pcl::console::print_highlight ("Downsampling...\n");
    pcl::VoxelGrid<PointNT> grid;
    const float leaf = 0.005f;
    grid.setLeafSize (leaf, leaf, leaf);
    grid.setInputCloud (object);
    grid.filter (*object);
    grid.setInputCloud (scene);
    grid.filter (*scene);
    
    // Estimate normals for scene
    pcl::console::print_highlight ("Estimating scene normals...\n");
    pcl::NormalEstimationOMP<PointNT,PointNT> nest;
    nest.setRadiusSearch (0.01);
    nest.setInputCloud (scene);
    nest.compute (*scene);
    
    // Estimate features
    pcl::console::print_highlight ("Estimating features...\n");
    FeatureEstimationT fest;
    fest.setRadiusSearch (0.025);
    fest.setInputCloud (object);
    fest.setInputNormals (object);
    fest.compute (*object_features);
    fest.setInputCloud (scene);
    fest.setInputNormals (scene);
    fest.compute (*scene_features);
    
    // Perform alignment
    pcl::console::print_highlight ("Starting alignment...\n");
    pcl::SampleConsensusPrerejective<PointNT,PointNT,FeatureT> align;
    align.setInputSource (object);
    align.setSourceFeatures (object_features);
    align.setInputTarget (scene);
    align.setTargetFeatures (scene_features);
    align.setMaximumIterations (10000); // Number of RANSAC iterations
    align.setNumberOfSamples (3); // Number of points to sample for generating/prerejecting a pose
    align.setCorrespondenceRandomness (2); // Number of nearest features to use
    align.setSimilarityThreshold (0.9f); // Polygonal edge length similarity threshold
    align.setMaxCorrespondenceDistance (1.5f * leaf); // Inlier threshold
    align.setInlierFraction (0.25f); // Required inlier fraction for accepting a pose hypothesis
    {
      pcl::ScopeTime t("Alignment");
      align.align (*object_aligned);
    }
    
    if (align.hasConverged ())
    {
      // Print results
      printf ("\n");
      Eigen::Matrix4f transformation = align.getFinalTransformation ();
      pcl::console::print_info ("    | %6.3f %6.3f %6.3f | \n", transformation (0,0), transformation (0,1), transformation (0,2));
      pcl::console::print_info ("R = | %6.3f %6.3f %6.3f | \n", transformation (1,0), transformation (1,1), transformation (1,2));
      pcl::console::print_info ("    | %6.3f %6.3f %6.3f | \n", transformation (2,0), transformation (2,1), transformation (2,2));
      pcl::console::print_info ("\n");
      pcl::console::print_info ("t = < %0.3f, %0.3f, %0.3f >\n", transformation (0,3), transformation (1,3), transformation (2,3));
      pcl::console::print_info ("\n");
      pcl::console::print_info ("Inliers: %i/%i\n", align.getInliers ().size (), object->size ());
      
      // Show alignment
      pcl::visualization::PCLVisualizer visu("Alignment");
      visu.addPointCloud (scene, ColorHandlerT (scene, 0.0, 255.0, 0.0), "scene");
      visu.addPointCloud (object_aligned, ColorHandlerT (object_aligned, 0.0, 0.0, 255.0), "object_aligned");
      visu.spin ();
    }
    else
    {
      pcl::console::print_error ("Alignment failed!\n");
  
    }
    
  
  
  } 
  
  int
  main (int argc, char** argv)
  {
    // Initialize ROS
    ros::init (argc, argv, "obstacle_projection");
    ros::NodeHandle nh;
  
    // Create a ROS subscriber for the input point cloud
    ros::Subscriber sub = nh.subscribe ("input", 1, cloud_cb);
  
      // Load object and scene
    pcl::console::print_highlight ("Loading point clouds...\n");
    if (pcl::io::loadPCDFile<PointXYZ> (argv[2], *cluster) < 0)     
    {
      pcl::console::print_error ("Error loading object/scene file!\n");
      return (1);
    }
    ros::spin ();
  }
  