pcl::PCLPointCloud2Ptr PlanSegmentor::passthrough_filter(pcl::PCLPointCloud2Ptr p_input,
                                                         double p_min_distance,
                                                         double p_max_distance)
{
    pcl::PassThrough<pcl::PCLPointCloud2> pt_filter;
    pt_filter.setFilterFieldName ("z");
    pt_filter.setFilterLimits (p_min_distance, p_max_distance);
    pt_filter.setKeepOrganized (false);
    pt_filter.setInputCloud (p_input);
    pcl::PCLPointCloud2::Ptr cloud_filtered(new pcl::PCLPointCloud2);
    pt_filter.filter (*cloud_filtered);

    //added by JeanJean
    pt_filter.setInputCloud(cloud_filtered);
    pt_filter.setFilterFieldName("x");
    pt_filter.setFilterLimits(-1.0, 1.0);
    pcl::PCLPointCloud2::Ptr ptr_cloud_filtered_x(new pcl::PCLPointCloud2);
    pt_filter.filter(*ptr_cloud_filtered_x);

    pt_filter.setInputCloud(ptr_cloud_filtered_x);
    pt_filter.setFilterFieldName("y");
    pt_filter.setFilterLimits(-1.0, 1.0);
    pcl::PCLPointCloud2::Ptr ptr_cloud_filtered_y(new pcl::PCLPointCloud2);
    pt_filter.filter(*ptr_cloud_filtered_y);
    /////////////////////////////////////////////////

    return ptr_cloud_filtered_y;
}

int
main (int argc, char** argv)
{
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);

  // Fill in the cloud data
  pcl::PCDReader reader;
  // Replace the path below with the path where you saved your file
  reader.read<pcl::PointXYZ> ("table_scene_lms400.pcd", *cloud);

  std::cerr << "Cloud before filtering: " << std::endl;
  std::cerr << *cloud << std::endl;

  // Create the filtering object
  pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
  sor.setInputCloud (cloud);
  sor.setMeanK (50);
  sor.setStddevMulThresh (1.0);
  sor.filter (*cloud_filtered);

  std::cerr << "Cloud after filtering: " << std::endl;
  std::cerr << *cloud_filtered << std::endl;

  pcl::PCDWriter writer;
  writer.write<pcl::PointXYZ> ("table_scene_lms400_inliers.pcd", *cloud_filtered, false);

  sor.setNegative (true);
  sor.filter (*cloud_filtered);
  writer.write<pcl::PointXYZ> ("table_scene_lms400_outliers.pcd", *cloud_filtered, false);

  return (0);
}

Eigen::Vector3d estimate_plane_normals(PointCloud::Ptr cloud_f)
{
    PointCloud::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>),cloud_plane (new pcl::PointCloud<pcl::PointXYZRGB>),cloud_f_aux (new pcl::PointCloud<pcl::PointXYZRGB>);
    PointCloud::Ptr cloud_filtered_while (new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::PassThrough<pcl::PointXYZRGB> pass;
    pass.setInputCloud (cloud_f);
    pass.setFilterFieldName ("z");
    pass.setFilterLimits (0.0, 1.1);
    //pass.setFilterLimitsNegative (true);
    pass.filter (*cloud_filtered);

    pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor;
    sor.setInputCloud (cloud_filtered);
    sor.setMeanK (50);
    sor.setStddevMulThresh (2);
    sor.filter (*cloud_filtered);

    pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
    // Create the segmentation object
    pcl::SACSegmentation<pcl::PointXYZRGB> seg;
    // Optional
    seg.setOptimizeCoefficients (true);
    // Mandatory
    seg.setModelType (pcl::SACMODEL_PLANE);
    seg.setMethodType (pcl::SAC_RANSAC);
    seg.setDistanceThreshold (0.01);
    cloud_filtered_while =cloud_filtered;
    int i = 0, nr_points = (int) cloud_filtered_while->points.size ();
    // While 30% of the original cloud is still there
    pcl::ExtractIndices<pcl::PointXYZRGB> extract;
    while (cloud_filtered_while->points.size () > 0.3 * nr_points)
    {
        // Segment the largest planar component from the remaining cloud
        seg.setInputCloud (cloud_filtered_while);
        seg.segment (*inliers, *coefficients);
        if (inliers->indices.size () == 0)
        {
            std::cerr << "Could not estimate a planar model for the given dataset." << std::endl;
            break;
        }
        // Extract the planar inliers from the input cloud
        extract.setInputCloud (cloud_filtered_while);
        extract.setIndices (inliers);
        // Remove the planar inliers, extract the rest
        extract.setNegative (false);
        extract.filter (*cloud_plane);
        // Create the filtering object
        extract.setNegative (true);
        extract.filter (*cloud_f_aux);
        cloud_filtered_while.swap (cloud_f_aux);
        i++;
    }

    Eigen::Vector3d plane_normal_vector ;
    for(int i=0;i<3;i++)
        plane_normal_vector(i) = coefficients->values[i];

    return plane_normal_vector;
}

int
main (int argc, char** argv)
{
  if (argc != 3)
  {
    std::cerr << "please provide filename followed by leaf size (e.g. cloud.pcd 0.01)" << std::endl;
    exit(0);
  }
  sensor_msgs::PointCloud2::Ptr cloud (new sensor_msgs::PointCloud2 ());
  sensor_msgs::PointCloud2::Ptr cloud_filtered (new sensor_msgs::PointCloud2 ());
  float leafSize = atof(argv[2]);

  //Fill in the cloud data
  pcl::PCDReader reader;
  // Replace the path below with the path where you saved your file
  reader.read (argv[1], *cloud); // Remember to download the file first!

  std::cerr << "PointCloud before filtering: " << cloud->width * cloud->height 
       << " data points (" << pcl::getFieldsList (*cloud) << ").";

  // Create the filtering object
  pcl::VoxelGrid<sensor_msgs::PointCloud2> sor;
  sor.setInputCloud (cloud);
  sor.setLeafSize (leafSize, leafSize, leafSize);
  sor.filter (*cloud_filtered);

  std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height 
       << " data points (" << pcl::getFieldsList (*cloud_filtered) << ").";

  pcl::PCDWriter writer;
  writer.write ("filter_out.pcd", *cloud_filtered, 
         Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), false);

  return (0);
}

// CALLBACKS
void pointcloudCallback(const pcl::PointCloud<pcl::PointXYZ>::ConstPtr& cloud)
{
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZ>);
   // Create the filtering objects
  pcl::PassThrough<pcl::PointXYZ> pass;
  pass.setInputCloud (cloud);
  pass.setFilterFieldName (fieldName_);
  if(numBands_ <= 1){ 
    // Just center it to be nice
    pass.setFilterLimits ((startPoint_+endPoint_)/2.0, (startPoint_+endPoint_)/2.0+bandWidth_);
    pass.filter(*cloud_filtered);
    *output_cloud = *cloud_filtered;
  } else {
    // Do the first one manually so that certain parameters like frame_id always match
    pass.setFilterLimits (startPoint_, startPoint_+bandWidth_);
    pass.filter(*cloud_filtered);
    *output_cloud = *cloud_filtered;
    float dL = endPoint_-startPoint_;
    
    for(int i = 1; i < numBands_; i++){
      float sLine = startPoint_ + i*dL/(float)(numBands_-1);
      float eLine = sLine + bandWidth_;
      pass.setFilterLimits (sLine, eLine);
      pass.filter(*cloud_filtered);
      *output_cloud += *cloud_filtered;
    }
  }
  cloudout_pub_.publish(*output_cloud);
}

void voxelGridFilter( pcl::PointCloud<PointType>::Ptr cloud )
{
    // フィルター前の点群の数を表示する
    pcl::console::print_info( "before point clouds : %d\n",
        cloud->size() );

    // フィルターする範囲
    // Kinect FusionはKinectのカメラ座標系で記録されるのでメートル単位
    // 0.01の場合は1cm単位でフィルターする
    float leaf = 0.01f;

    pcl::VoxelGrid<PointType> grid;

    // フィルターする範囲を設定
    grid.setLeafSize( leaf, leaf, leaf );

    // フィルターする点群を設定
    grid.setInputCloud( cloud );

    // フィルターする(新しい点群に保存する)
    pcl::PointCloud<PointType>::Ptr
        cloud_filtered( new pcl::PointCloud<PointType> );
    grid.filter( *cloud_filtered );

    // 点群を戻す
    pcl::copyPointCloud( *cloud_filtered, *cloud );

    // フィルター後の点群の数を表示する
    pcl::console::print_info( "after point clouds : %d\n",
        cloud->size() );
}

void PassThrough::filter_xyz() {
    CLOG(LTRACE) <<"filter_xyz()";
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = in_cloud_xyz.read();

    if (!pass_through) {
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
        pcl::PassThrough<pcl::PointXYZ> pass;
        pass.setInputCloud (cloud);
        pass.setFilterFieldName ("x");
        pass.setFilterLimits (xa, xb);
        pass.setFilterLimitsNegative (negative_x);
        pass.filter (*cloud_filtered);
        // Set resulting cloud as input.
        pass.setInputCloud (cloud_filtered);
        pass.setFilterFieldName ("y");
        pass.setFilterLimits (ya, yb);
        pass.setFilterLimitsNegative (negative_y);
        pass.filter (*cloud_filtered);
        pass.setFilterFieldName ("z");
        pass.setFilterLimits (za, zb);
        pass.setFilterLimitsNegative (negative_z);
        pass.filter (*cloud_filtered);
        out_cloud_xyz.write(cloud_filtered);
    } else
        out_cloud_xyz.write(cloud);
}

int PCLWrapper::filter(unsigned short *depth_data, float *point_cloud_data){
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(
			new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(
			new pcl::PointCloud<pcl::PointXYZ>);
	// Fill in the cloud data
	cloud->width = IMAGE_WIDTH;
	cloud->height = IMAGE_HEIGHT;
	cloud->points.resize(cloud->width * cloud->height);

	//copy the data...? slow but for testing now.
	int i=0;
	const float fx_d = 1.0/5.5879981950414015e+02;
	const float fy_d = 1.0/5.5874227168094478e+02;
	const float cx_d = 3.1844162327317980e+02;
	const float cy_d = 2.4574257294583529e+02;
	unsigned short *my_depth_data = depth_data;
	float *my_point_cloud_data = point_cloud_data;
	double sum=0;
	for(int y=0; y<IMAGE_HEIGHT; y++){
		for(int x=0; x<IMAGE_WIDTH; x++){
			//copy the data to the point cloud struc object.
			unsigned short d_val = *my_depth_data;
		    float my_z = d_val*0.001f;
		    sum=sum+my_z;
		    float my_x = my_z * (x-cx_d) * fx_d;
		    float my_y = my_z * (y-cy_d) * fy_d;
		    cloud->points[i].x = my_x;
		    cloud->points[i].y = my_y;
		    cloud->points[i].z = my_z;
		    my_depth_data++;
			i++;
		}
	}
	// Create the filtering object
	pcl::PassThrough < pcl::PointXYZ > pass;
	pass.setInputCloud(cloud);
	pass.setFilterFieldName("z");
	pass.setFilterLimits(0.0, 100.0);
//	//pass.setFilterLimitsNegative (true);
	pass.filter(*cloud_filtered);

	size_t new_size = cloud_filtered->width * cloud_filtered->height;
//	char buf[1024];
//	sprintf(buf, "Original: %d, Filtered: %d, Ratio: %f, Sum %f \n", IMAGE_WIDTH*IMAGE_HEIGHT, new_size, ((float)new_size)/(IMAGE_WIDTH*IMAGE_HEIGHT), sum);
//	__android_log_write(ANDROID_LOG_INFO, "PCL FILTER TESTING:", buf);

	//save the results to the pointer
	for(int i=0; i<cloud_filtered->width*cloud_filtered->height;i++){
		float x = cloud_filtered->points[i].x;
		float y = cloud_filtered->points[i].y;
		float z = cloud_filtered->points[i].z;

		*my_point_cloud_data=x;
		*(my_point_cloud_data+1)=y;
		*(my_point_cloud_data+2)=z;
		my_point_cloud_data+=3;
	}
	return new_size;
}

bool PointCloudFunctions::statisticalOutlierRemovalAndSave(const cv::SparseMat &vmt, string fileName, int meanK, double stdDevMulThreshold)
{
    //    "Our sparse outlier removal is based on the computation of the distribution of point to neighbors distances in the input dataset.
    //    For each point, we compute the mean distance from it to all its neighbors. By assuming that the resulted distribution is
    //    Gaussian with a mean and a standard deviation, all points whose mean distances are outside an interval defined by the global
    //    distances mean and standard deviation can be considered as outliers and trimmed from the dataset."

    PointCloud<PointXYZI>::Ptr cloud = convertToPointCloud(vmt);
    PointCloud<PointXYZI>::Ptr cloud_filtered (new PointCloud<PointXYZI>);

    // Create the filtering object
    StatisticalOutlierRemoval<PointXYZI> sor;
    sor.setInputCloud (cloud);
    sor.setMeanK (meanK);
    sor.setStddevMulThresh (stdDevMulThreshold);
    sor.filter (*cloud_filtered);

    cloud->clear();
    cloud.reset();

    bool resultRet = (pcl::io::savePCDFileASCII (fileName, *cloud_filtered) >= 0); //FIXME: what is the return value? it's not mentioned in http://docs.pointclouds.org/1.6.0/group__io.html#ga5e406a5854fa8ad026cad85158fef266

    cloud_filtered->clear();
    cloud_filtered.reset();

    return resultRet;
}

int
main (int argc, char** argv)
{
  pcl::PCLPointCloud2::Ptr cloud (new pcl::PCLPointCloud2 ());
  pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2 ());

  // Fill in the cloud data
  pcl::PCDReader reader;
  // Replace the path below with the path where you saved your file
  reader.read ("table_scene_lms400.pcd", *cloud); // Remember to download the file first!

  std::cerr << "PointCloud before filtering: " << cloud->width * cloud->height 
       << " data points (" << pcl::getFieldsList (*cloud) << ").";

  // Create the filtering object
  pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
  sor.setInputCloud (cloud);
  sor.setLeafSize (0.01f, 0.01f, 0.01f);
  sor.filter (*cloud_filtered);

  std::cerr << "PointCloud after filtering: " << cloud_filtered->width * cloud_filtered->height 
       << " data points (" << pcl::getFieldsList (*cloud_filtered) << ").";

  pcl::PCDWriter writer;
  writer.write ("table_scene_lms400_downsampled.pcd", *cloud_filtered, 
         Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), false);

  return (0);
}

int main (int argc, char** argv) {
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>);

	if (argc != 2) {
                PCL_ERROR ("Syntax: %s input.pcd\n", argv[0]);
                return -1;
        }

        pcl::io::loadPCDFile (argv[1], *cloud);
	std::string inputfile = argv[1];

        std::cout << "Loaded "
                  << cloud->width * cloud->height
                  << " data points from " << inputfile << "."
                  << std::endl;

	pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor;
	sor.setInputCloud (cloud);
	sor.setMeanK (100);
	sor.setStddevMulThresh(1.0);
	sor.filter (*cloud_filtered);

	std::string delimiter = ".pcd";
	std::string outfile_inliers = "inliersCloud" + inputfile.substr(inputfile.find(delimiter) - 1, inputfile.find('\0'));

	pcl::io::savePCDFileASCII (outfile_inliers, *cloud_filtered);
        std::cerr << "Saved "
                  << cloud_filtered->width * cloud_filtered->height
                  << " data points to " << outfile_inliers << "."
                  << std::endl;

	return (0);
}

int main (int argc, char** argv)
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);

	// Fill in the cloud data
	pcl::PCDReader reader;
	// Replace the path below with the path where you saved your file
	reader.read<pcl::PointXYZ> (argv[1], *cloud);

	std::cerr << "Cloud before filtering: " << std::endl;
	std::cerr << *cloud << std::endl;

	// Create the filtering object
	pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
	sor.setInputCloud (cloud);
	sor.setMeanK (400);
	sor.setStddevMulThresh (3.5);
	sor.filter (*cloud_filtered);

	std::cerr << "Cloud after filtering: " << std::endl;
	std::cerr << *cloud_filtered << std::endl;

	//pcl::PCDWriter writer;
	//writer.write<pcl::PointXYZ> (argv[2], *cloud_filtered, false);

	pcl::io::savePCDFileBinary(argv[2], *cloud_filtered);
	return (0);
}

int  main (int argc, char** argv){

	pcl::PointCloud<pcl::PointXYZ>::Ptr
  	  cloud (new pcl::PointCloud<pcl::PointXYZ> ()),
  	  cloud_filtered (new pcl::PointCloud<pcl::PointXYZ> ()),
  	  cloud_removed (new pcl::PointCloud<pcl::PointXYZ> ());

  // Fill in the cloud data
  cloud->width  = 5;
  cloud->height = 1;
  cloud->points.resize (cloud->width * cloud->height);

  for (size_t i = 0; i < cloud->points.size (); ++i)
  {
    cloud->points[i].x = 1024 * rand () / (RAND_MAX + 1.0);
    cloud->points[i].y = 1024 * rand () / (RAND_MAX + 1.0);
    cloud->points[i].z = 1024 * rand () / (RAND_MAX + 1.0);
  }
  std::cerr << "Cloud before filtering: " << std::endl;
  for (size_t i = 0; i < cloud->points.size (); ++i)
    std::cerr << "    " << cloud->points[i].x << " " << cloud->points[i].y << " " << cloud->points[i].z << std::endl;

  // Create the filtering object
  pcl::PassThrough<pcl::PointXYZ> pass(true);
  pass.setInputCloud (cloud);
  pass.setFilterFieldName ("z");
  pass.setFilterLimits (0.0, 1.0);
  //pass.setFilterLimitsNegative (true);

  pass.filter (*cloud_filtered);
  pcl::IndicesConstPtr indices = pass.getRemovedIndices();
//  std::cerr<<"n_indices: "<<indices->size()<<std::endl;
//  std::cerr<<"indices: ";
//  for(unsigned int i=0;i<indices->size();i++)
//	  std::cerr<<(*indices)[i]<<"\t";
//  std::cerr<<std::endl;

  pcl::PointIndices::Ptr removed_indices (new pcl::PointIndices());
  removed_indices->indices = *indices;

  pcl::ExtractIndices<pcl::PointXYZ> extract;
  extract.setInputCloud (cloud);
  extract.setIndices (removed_indices);
  extract.setNegative (false);
  extract.filter (*cloud_removed);

  std::cerr <<std::endl<< "Cloud after filtering: " << std::endl;
  for (size_t i = 0; i < cloud_filtered->points.size (); ++i)
    std::cerr 	<< cloud_filtered->points[i].x << " "
    			<< cloud_filtered->points[i].y << " "
    			<< cloud_filtered->points[i].z << std::endl;

  std::cerr <<std::endl<< "Cloud being removed: " << std::endl;
  for (size_t i = 0; i < cloud_removed->points.size (); ++i)
    std::cerr 	<< cloud_removed->points[i].x << " "
    			<< cloud_removed->points[i].y << " "
    			<< cloud_removed->points[i].z << std::endl;
  return (0);
}

int
main (int argc, char** argv)
{
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>), 
                                      cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>), 
                                      cloud_projected (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PCDReader reader;

  reader.read ("table_scene_mug_stereo_textured.pcd", *cloud);
  // Build a filter to remove spurious NaNs
  pcl::PassThrough<pcl::PointXYZ> pass;
  pass.setInputCloud (cloud);
  pass.setFilterFieldName ("z");
  pass.setFilterLimits (0, 1.1);
  pass.filter (*cloud_filtered);
  std::cerr << "PointCloud after filtering has: "
            << cloud_filtered->points.size () << " data points." << std::endl;

  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
  // Create the segmentation object
  pcl::SACSegmentation<pcl::PointXYZ> seg;
  // Optional
  seg.setOptimizeCoefficients (true);
  // Mandatory
  seg.setModelType (pcl::SACMODEL_PLANE);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setDistanceThreshold (0.01);

  seg.setInputCloud (cloud_filtered);
  seg.segment (*inliers, *coefficients);
  std::cerr << "PointCloud after segmentation has: "
            << inliers->indices.size () << " inliers." << std::endl;

  // Project the model inliers
  pcl::ProjectInliers<pcl::PointXYZ> proj;
  proj.setModelType (pcl::SACMODEL_PLANE);
  proj.setIndices (inliers);
  proj.setInputCloud (cloud_filtered);
  proj.setModelCoefficients (coefficients);
  proj.filter (*cloud_projected);
  std::cerr << "PointCloud after projection has: "
            << cloud_projected->points.size () << " data points." << std::endl;

  // Create a Concave Hull representation of the projected inliers
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_hull (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::ConcaveHull<pcl::PointXYZ> chull;
  chull.setInputCloud (cloud_projected);
  chull.setAlpha (0.1);
  chull.reconstruct (*cloud_hull);

  std::cerr << "Concave hull has: " << cloud_hull->points.size ()
            << " data points." << std::endl;

  pcl::PCDWriter writer;
  writer.write ("table_scene_mug_stereo_textured_hull.pcd", *cloud_hull, false);

  return (0);
}

QList <pcl::cloud_composer::CloudComposerItem*>
pcl::cloud_composer::VoxelGridDownsampleTool::performAction (ConstItemList input_data, PointTypeFlags::PointType)
{
  QList <CloudComposerItem*> output;
  const CloudComposerItem* input_item;
  // Check input data length
  if ( input_data.size () == 0)
  {
    qCritical () << "Empty input in VoxelGridDownsampleTool!";
    return output;
  }
  else if ( input_data.size () > 1)
  {
    qWarning () << "Input vector has more than one item in VoxelGridDownsampleTool";
  }
  input_item = input_data.value (0);
    
  if (input_item->type () == CloudComposerItem::CLOUD_ITEM)
  {
    double leaf_x = parameter_model_->getProperty("Leaf Size x").toDouble ();
    double leaf_y = parameter_model_->getProperty("Leaf Size y").toDouble ();
    double leaf_z = parameter_model_->getProperty("Leaf Size z").toDouble ();
    
    pcl::PCLPointCloud2::ConstPtr input_cloud = input_item->data (ItemDataRole::CLOUD_BLOB).value <pcl::PCLPointCloud2::ConstPtr> ();
    
    //////////////// THE WORK - FILTERING OUTLIERS ///////////////////
    // Create the filtering object
    pcl::VoxelGrid<pcl::PCLPointCloud2> vox_grid;
    vox_grid.setInputCloud (input_cloud);
    vox_grid.setLeafSize (float (leaf_x), float (leaf_y), float (leaf_z));
    
    
    //Create output cloud
    pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2);
    //Filter!  
    vox_grid.filter (*cloud_filtered);

    //////////////////////////////////////////////////////////////////
    //Get copies of the original origin and orientation
    Eigen::Vector4f source_origin = input_item->data (ItemDataRole::ORIGIN).value<Eigen::Vector4f> ();
    Eigen::Quaternionf source_orientation =  input_item->data (ItemDataRole::ORIENTATION).value<Eigen::Quaternionf> ();
    //Put the modified cloud into an item, stick in output
    CloudItem* cloud_item = new CloudItem (input_item->text () + tr (" vox ds")
                                           , cloud_filtered
                                           , source_origin
                                           , source_orientation);

    
    output.append (cloud_item);
  }
  else
  {
    qDebug () << "Input item in VoxelGridDownsampleTool is not a cloud!!!";
  }
  
  
  return output;
}

     //capture cloud data and process filtering
     void cloud_cb_ (const pcl::PointCloud<pcl::PointXYZ>::ConstPtr &cloud) //this is a subfunction from a sub object under main function
     {
	    pcl::PassThrough<pcl::PointXYZ> pass; //generate the object which contains the function of pass filtering
	    pcl::VoxelGrid<pcl::PointXYZ> sor;    //generate the object which contains the function of voxel filtering
	    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>); //generate the container of filtered clouds
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filteredv (new pcl::PointCloud<pcl::PointXYZ>); //generate the container of filtered voxel clouds
       if (!viewer.wasStopped())
       //modify pointclouds here, it's assumed it's captured and used as parameters in the function
        {
			
		pass.setInputCloud (cloud); //pcl::PassThrough<pcl::PointXYZ> 
		pass.setFilterFieldName ("z");
		pass.setFilterLimits (0.0, 1.0);
  //pass.setFilterLimitsNegative (true);
		pass.filter (*cloud_filtered);
       
       sor.setInputCloud (cloud_filtered);
	   sor.setLeafSize (0.01f, 0.01f, 0.01f);// set the density of the voxel grid
       sor.filter (*cloud_filteredv);
       ///////////for normals
       //~ std::vector<int> indices (floor (cloud_filteredv->points.size () / 10));
	   //~ for (size_t i = 0; indices.size (); ++i) indices[i] = i;
//~ 
       //~ // Create the normal estimation class, and pass the input dataset to it
  //~ pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
  //~ ne.setInputCloud (cloud_filteredv);
//~ 
  //~ // Pass the indices
  //~ boost::shared_ptr<std::vector<int> > indicesptr (new std::vector<int> (indices));
  //~ ne.setIndices (indicesptr);
//~ 
  //~ // Create an empty kdtree representation, and pass it to the normal estimation object.
  //~ // Its content will be filled inside the object, based on the given input dataset (as no other search surface is given).
  //~ pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
  //~ ne.setSearchMethod (tree);
//~ 
  //~ // Output datasets
  //~ pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
//~ 
  //~ // Use all neighbors in a sphere of radius 3cm
  //~ ne.setRadiusSearch (0.03);
//~ 
  //~ // Compute the features
  //~ ne.compute (*cloud_normals);

       
       
       
       
       
       
       
       ///////////for normals
       
         viewer.showCloud (cloud_filteredv);
	 }
         
     }

PointCloudConstPtr ICPWrapper::downsampleCloud (PointCloudConstPtr input)
{
  const double voxel_size = 0.01;
  PointCloudPtr cloud_filtered (new PointCloud);
  pcl17::VoxelGrid<PointType> downsampler;
  downsampler.setInputCloud (input);
  downsampler.setLeafSize (voxel_size, voxel_size, voxel_size);
  downsampler.filter (*cloud_filtered);
  return cloud_filtered;
}

pcl::PointCloud<pcl::PointXYZRGB>::Ptr VoxelGrid_filter(pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr){

pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB> ());
    pcl::VoxelGrid<pcl::PointXYZRGB> sorvg;
     sorvg.setInputCloud (point_cloud_ptr);
     sorvg.setLeafSize (0.01f, 0.01f, 0.01f);
     sorvg.filter(*cloud_filtered);


return cloud_filtered;
}

void cloud_cb (sensor_msgs::PointCloud2ConstPtr input) {

  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZRGB>);
  pcl::fromROSMsg (*input, *cloud_filtered);
  //std::cout << "Input pointCloud 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::PointXYZRGB> seg;
  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZRGB> ());
  seg.setOptimizeCoefficients (true);
  seg.setModelType (ModelType);//(pcl::SACMODEL_PLANE);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setMaxIterations (MaxIterations);//(100);
  seg.setDistanceThreshold (DistanceThreshold);//(0.02);
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZRGB>);

  int i=0, nr_points = (int) cloud_filtered->points.size ();
  while (cloud_filtered->points.size () > RatioLimit * nr_points)//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::PointXYZRGB> extract;
    extract.setInputCloud (cloud_filtered);
    extract.setIndices (inliers);
    // extract.setNegative (false);

    // // Write the planar inliers to disk
    // 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 (ExtractNegative);
    extract.filter (*cloud_f);
    cloud_filtered = cloud_f;
  }


  sensor_msgs::PointCloud2 output;
  pcl::toROSMsg (*cloud_filtered , output);
  output.header.stamp = ros::Time::now();
  output.header.frame_id = "/camera_rgb_optical_frame";
  // Publish the data
  cloud_pub.publish (output);
}

void greedy_proj () {
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PCLPointCloud2::Ptr cloud_blob (new pcl::PCLPointCloud2 ());
  pcl::PCLPointCloud2::Ptr cloud_filtered (new pcl::PCLPointCloud2 ());
  pcl::io::loadPCDFile ("input.pcd", *cloud_blob);
  pcl::fromPCLPointCloud2 (*cloud_blob, *cloud);

  cloud_blob = cloud_filtered;

  // 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 (new pcl::search::KdTree<pcl::PointXYZ>);
  tree->setInputCloud (cloud);
  n.setInputCloud (cloud);
  n.setSearchMethod (tree);
  n.setKSearch (20);
  n.compute (*normals);

  // 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);

  // Create search tree
  pcl::search::KdTree<pcl::PointNormal>::Ptr tree2 (new pcl::search::KdTree<pcl::PointNormal>);
  tree2->setInputCloud (cloud_with_normals);

  // Initialize objects
  pcl::GreedyProjectionTriangulation<pcl::PointNormal> gp3;
  pcl::PolygonMesh triangles;

  // Set the maximum distance between connected points (maximum edge length)
  gp3.setSearchRadius (1);

  // Set typical values for the parameters
  gp3.setMu (2.5);
  gp3.setMaximumNearestNeighbors (10);
  gp3.setMaximumSurfaceAngle(M_PI/4); // 45 degrees
  gp3.setMinimumAngle(M_PI/18); // 10 degrees
  gp3.setMaximumAngle(2*M_PI/3); // 120 degrees
  gp3.setNormalConsistency(false);

  // Get result
  gp3.setInputCloud (cloud_with_normals);
  gp3.setSearchMethod (tree2);
  gp3.reconstruct (triangles);

  // Additional vertex information
  std::vector<int> parts = gp3.getPartIDs();
  std::vector<int> states = gp3.getPointStates();

  pcl::io::saveVTKFile("output.vtk", triangles);
  pcl::io::savePolygonFileSTL("output.stl", triangles);
}