void callback(const sensor_msgs::PointCloud2::ConstPtr& pc)
{

  PointCloudT::Ptr cloud = boost::make_shared<PointCloudT>();
  pcl::fromROSMsg(*pc, *cloud);

  size_t height = 8;   // 8 layers
  size_t width = round((start_angle_ - end_angle_) / angular_resolution_) + 1;

  PointT invalid;
  invalid.x = invalid.y = invalid.z = std::numeric_limits<float>::quiet_NaN();
  PointCloudT::Ptr cloud_out = boost::make_shared<PointCloudT>(width, height, invalid);
  cloud_out->is_dense = false;

  for (size_t i = 0; i < cloud->size(); i++)
  {
    const PointT &p = cloud->points[i];
    int col = round((atan2(p.y, p.x) - end_angle_) / angular_resolution_);
    int row = p.layer;

    if (col < 0 || col >= width || row < 0 || row >= height)
    {
      ROS_ERROR("Invalid coordinates: (%d, %d) is outside (0, 0)..(%zu, %zu)!", col, row, width, height);
      continue;
    }
    (*cloud_out)[row * width + col] = p;
  }

  sensor_msgs::PointCloud2::Ptr msg = boost::make_shared<sensor_msgs::PointCloud2>();
  pcl::toROSMsg(*cloud_out, *msg);
  msg->header = pc->header;
  pub.publish(msg);
}

void move_to_frame(const PointCloudT::Ptr &input, const string &target_frame, PointCloudT::Ptr &output) {
    ROS_INFO("Transforming Input Point Cloud to %s frame...", target_frame.c_str());
    ROS_INFO("    Input Cloud Size: %zu", input->size());
    if (input->header.frame_id == target_frame) {
        output = input;
        return;
    }
    while (ros::ok()) {
        tf::StampedTransform stamped_transform;
        try {
            // Look up transform
            tf_listener->lookupTransform(target_frame, input->header.frame_id, ros::Time(0), stamped_transform);

            // Apply transform
            pcl_ros::transformPointCloud(*input, *output, stamped_transform);

            // Store Header Details
            output->header.frame_id = target_frame;
            pcl_conversions::toPCL(ros::Time::now(), output->header.stamp);

            break;
        }
            //keep trying until we get the transform
        catch (tf::TransformException &ex) {
            ROS_ERROR_THROTTLE(1, "%s", ex.what());
            ROS_WARN_THROTTLE(1, "    Waiting for transform from cloud frame (%s) to %s frame. Trying again",
                              input->header.frame_id.c_str(), target_frame.c_str());
            continue;
        }
    }
}

void trk3dFeatures::trkSeq(const PointCloudT::Ptr &cloud2,
                           const PointCloudT::Ptr &keyPts2,
                           const float params[],
                           PointCloudT::Ptr &keyPts,
                           pcl::PointCloud<SHOT1344>::Ptr &Shot1344Cloud_1,
                           std::vector<u16> &matchIdx1, std::vector<u16> &matchIdx2,
                           std::vector<f32> &matchDist)
{
    // construct descriptors
    pcl::PointCloud<SHOT1344>::Ptr Shot1344Cloud_2(new pcl::PointCloud<SHOT1344>);

    extractFeatures featureDetector;
    Shot1344Cloud_2 = featureDetector.Shot1344Descriptor(cloud2, keyPts2, params);

//    // match keypoints
//    featureDetector.crossMatching(Shot1344Cloud_1, Shot1344Cloud_2,
//                                  &matchIdx1, &matchIdx2, &matchDist);


    // find the matching from desc_1 -> desc_2
    std::cout<<"size of Shot1344Cloud_1 in trkSeq: "<<Shot1344Cloud_1->points.size()<<std::endl;
    std::cout<<"size of Shot1344Cloud_2 in trkSeq: "<<Shot1344Cloud_2->points.size()<<std::endl;
    featureDetector.matchKeyPts(Shot1344Cloud_1, Shot1344Cloud_2, &matchIdx2, &matchDist);
    std::cout<<"size of matches in trkSeq: "<<matchIdx2.size()<<std::endl;
    std::cout<<"size of matchDist in trkSeq: "<<matchDist.size()<<std::endl;
    Shot1344Cloud_1.reset(new pcl::PointCloud<SHOT1344>);
    keyPts->points.clear();
    for(size_t i=0; i<matchIdx2.size();i++)
    {
        keyPts->push_back(keyPts2->points.at(matchIdx2[i]));
        Shot1344Cloud_1->push_back(Shot1344Cloud_2->points.at(matchIdx2[i]));
    }
}

double calculate_density(const PointCloudT::Ptr &cloud, const bwi_perception::BoundingBox &box) {
    // TODO: Calculate true volume
    // If the cloud is one point thick in some dimension, we'll assign that dimension a magnitude of 1cm
    double x_dim = max(abs(box.max[0] - box.min[0]), 0.01f);
    double y_dim = max(abs(box.max[1] - box.min[1]), 0.01f);
    double z_dim = max(abs(box.max[2] - box.min[2]), 0.01f);
    double volume = x_dim * y_dim * z_dim;
    return (double) cloud->size() / volume;
}

void removeLowConfidencePoints(cv::Mat& confidence_image, int threshold, PointCloudT::Ptr& cloud)
{
  for (int i=0;i<cloud->height;i++)
  {
    for (int j=0;j<cloud->width;j++)
    {
      if (confidence_image.at<unsigned char>(i,j) < threshold)
      {
        cloud->at(j,i).x = std::numeric_limits<float>::quiet_NaN();
        cloud->at(j,i).y = std::numeric_limits<float>::quiet_NaN();
        cloud->at(j,i).z = std::numeric_limits<float>::quiet_NaN();
       
        confidence_image.at<unsigned char>(i,j) = 0;    // just for visualization
      }
      else
        confidence_image.at<unsigned char>(i,j) = 255;  // just for visualization
    }
  }
  cloud->is_dense = false;
}

void computeNeonVoxels(PointCloudT::Ptr in, PointCloudT::Ptr green, PointCloudT::Ptr orange) {

	green->clear();
	orange->clear();
	//Point Cloud to store out neon cap
	//PointCloudT::Ptr temp_neon_cloud (new PointCloudT);

	for (int i = 0; i < in->points.size(); i++) {
		unsigned int r, g, b;
		r = in->points[i].r;
		g = in->points[i].g;
		b = in->points[i].b;
		// Look for mostly neon value points
		if (g > 175 && (r + b) < 150) {
			green->push_back(in->points[i]);
		}
		else if(r > 200 && (g + b) < 150){
			orange->push_back(in->points[i]);
		}
	}
}

void PlayWindow::ShowPC(PointCloudT::Ptr PC)
{
    updateModelMutex.lock();


    if (ui->checkBox_ShowPC->isChecked())
    {

        if (!viewer->updatePointCloud(PC))
            viewer->addPointCloud(PC);

        Eigen::Matrix4f currentPose = Eigen::Matrix4f::Identity();
        currentPose.block(0,0,3,3) = PC->sensor_orientation_.matrix();
        currentPose.block(0,3,3,1) = PC->sensor_origin_.head(3);

        viewer->updatePointCloudPose("cloud",Eigen::Affine3f(currentPose) );

    }



    ui->qvtkWidget->update ();


    updateModelMutex.unlock();



    // Timing
    times.push_back(QDateTime::currentDateTime().toMSecsSinceEpoch() - PC->header.stamp);

    double mean;
    if (times.size() > 60)
    {
        mean = std::accumulate(times.begin(), times.end(), 0.0) / times.size();
        times.erase(times.begin());
    }

    if (ui->checkBox_SavePC->isChecked())
        pcl::io::savePCDFileASCII(PC->header.frame_id, *PC);


    std::vector<int> ind;
    pcl::removeNaNFromPointCloud(*PC, *PC, ind);
    ui->label_nPoint->setText(QString("Nb Point : %1").arg(PC->size()));
    ui->label_time->setText(QString("Time (ms) : %1").arg(mean));
    ui->label_fps->setText(QString("FPS (Hz) : %1").arg(1000/mean));



    return;
}

int main(int argc, char** argv)
{
  if (argc != 3)
  {
    printUsage(argv);
    return -1;
  }
  
  std::string filename_in  = argv[1];
  std::string filename_out = argv[2];
  
  // read in
  printf("Reading cloud\n");
  PointCloudT::Ptr cloud;
  cloud.reset(new rgbdtools::PointCloudT());
  pcl::PCDReader reader;
  reader.read(filename_in, *cloud);
  
  alignGlobalCloud(cloud);
  
  return 0;
}

void PC_to_Mat(PointCloudT::Ptr &cloud, cv::Mat &result){

  if (cloud->isOrganized()) {
    std::cout << "PointCloud is organized..." << std::endl;

    result = cv::Mat(cloud->height, cloud->width, CV_8UC3);

    if (!cloud->empty()) {

      for (int h=0; h<result.rows; h++) {
        for (int w=0; w<result.cols; w++) {
            PointT point = cloud->at(w, h);

            Eigen::Vector3i rgb = point.getRGBVector3i();

            result.at<cv::Vec3b>(h,w)[0] = rgb[2];
            result.at<cv::Vec3b>(h,w)[1] = rgb[1];
            result.at<cv::Vec3b>(h,w)[2] = rgb[0];
        }
      }
    }
  }
}

void filter_PC_from_BB(PointCloudT::Ptr &cloud, cv::Mat &result, int x, int y, int width, int height){

  const float bad_point = std::numeric_limits<float>::quiet_NaN();

  if (cloud->isOrganized()) {
    std::cout << "PointCloud is organized..." << std::endl;
    result = cv::Mat(cloud->height, cloud->width, CV_8UC3);

    if (!cloud->empty()) {

      for (int h=0; h<result.rows; h++) {
        for (int w=0; w<result.cols; w++) {
            
            // Check if in bounding window
            if ( (h>y && h<(y+height)) && ((w > x) && w < (x+width)) ){
            
              // do nothing

            } else {

              // remove point
              //PointT point = cloud->at(w, h);
              //cloud->at(w, h);
              cloud->at(w, h).x = bad_point;
              cloud->at(w, h).y = bad_point;
              cloud->at(w, h).z = bad_point;
              
              cloud->at(w, h).r = bad_point;
              cloud->at(w, h).g = bad_point;
              cloud->at(w, h).b = bad_point;
            }
        }
      }
    }
  }
}

// Align a rigid object to a scene with clutter and occlusions
int
main (int argc, char **argv)
{
  // Point clouds
  PointCloudT::Ptr object (new PointCloudT);
  PointCloudT::Ptr object_aligned (new PointCloudT);
  PointCloudT::Ptr scene (new PointCloudT);
  FeatureCloudT::Ptr object_features (new FeatureCloudT);
  FeatureCloudT::Ptr scene_features (new FeatureCloudT);
  
  // Get input object and scene
  if (argc != 3)
  {
    pcl::console::print_error ("Syntax is: %s object.pcd scene.pcd\n", argv[0]);
    return (1);
  }
  
  // Load object and scene
  pcl::console::print_highlight ("Loading point clouds...\n");
  if (pcl::io::loadPCDFile<PointNT> (argv[1], *object) < 0 ||
      pcl::io::loadPCDFile<PointNT> (argv[2], *scene) < 0)
  {
    pcl::console::print_error ("Error loading object/scene file!\n");
    return (1);
  }
  
  // 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 (100000); // Number of RANSAC iterations
  align.setNumberOfSamples (3); // Number of points to sample for generating/prerejecting a pose
  align.setCorrespondenceRandomness (5); // Number of nearest features to use
  align.setSimilarityThreshold (0.9f); // Polygonal edge length similarity threshold
  align.setMaxCorrespondenceDistance (2.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");
    return (1);
  }
  
  return (0);
}

int main (int argc, char** argv)
{	
	PointCloudT::Ptr cloud (new PointCloudT);
	PointCloudT::Ptr new_cloud (new PointCloudT);
	bool new_cloud_available_flag = false;
	//pcl::Grabber* grab = new pcl::OpenNIGrabber ();

	PointCloudT::Ptr ddd;

	boost::function<void (const PointCloudT::ConstPtr&)> f =
		boost::bind(&grabberCallback, _1, cloud, &new_cloud_available_flag);
	grab->registerCallback (f);
	viewer->registerKeyboardCallback(keyboardEventOccurred);
	grab->start ();
	
	bool first_time = true;

	FILE* objects;
	objects = fopen ("objects.txt","a");

	while(!new_cloud_available_flag)
		boost::this_thread::sleep(boost::posix_time::milliseconds(1));

	new_cloud_available_flag=false;


	////////////////////
	// invert correction
	////////////////////
				
	Eigen::Matrix4f transMat = Eigen::Matrix4f::Identity(); 
	transMat (1,1) = -1;

    ////////////////////
	// pass filter
	////////////////////

	PointCloudT::Ptr passed_cloud;
	pcl::PassThrough<PointT> pass;
	passed_cloud = boost::shared_ptr<PointCloudT>(new PointCloudT);

	
	////////////////////
	// voxel grid
	////////////////////
	PointCloudT::Ptr voxelized_cloud;
	voxelized_cloud = boost::shared_ptr<PointCloudT>(new PointCloudT);
	pcl::VoxelGrid<PointT> vg;
	vg.setLeafSize (0.001, 0.001, 0.001);
	

	////////////////////
	// sac segmentation
	////////////////////
	
	PointCloudT::Ptr cloud_f;
	PointCloudT::Ptr cloud_plane;
	PointCloudT::Ptr cloud_filtered;
	cloud_f = boost::shared_ptr<PointCloudT>(new PointCloudT);	
	cloud_plane = boost::shared_ptr<PointCloudT> (new PointCloudT);	
	cloud_filtered = boost::shared_ptr<PointCloudT> (new PointCloudT);

	pcl::SACSegmentation<PointT> seg;
	pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
	pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
	seg.setOptimizeCoefficients (true);
	seg.setModelType (pcl::SACMODEL_PLANE);
	seg.setMethodType (pcl::SAC_RANSAC);
	seg.setMaxIterations (100);
	seg.setDistanceThreshold (0.02);

	////////////////////
	// euclidean clustering
	////////////////////
	std::vector<pcl::PointIndices> cluster_indices;
	std::vector<PointCloudT::Ptr> extracted_clusters;
	pcl::search::KdTree<PointT>::Ptr eucl_tree (new pcl::search::KdTree<PointT>);
	pcl::EuclideanClusterExtraction<PointT> ec;
	ec.setClusterTolerance (0.04);
	ec.setMinClusterSize (100);
	ec.setMaxClusterSize (25000);
	ec.setSearchMethod (eucl_tree);

	PointCloudT::Ptr cloud_cluster;

	////////////////////
	// vfh estimate
	////////////////////
	pcl::NormalEstimation<PointT, pcl::Normal> ne;
	pcl::search::KdTree<PointT>::Ptr vfh_tree1 (new pcl::search::KdTree<PointT> ());
	pcl::VFHEstimation<PointT, pcl::Normal, pcl::VFHSignature308> vfh;
	pcl::search::KdTree<PointT>::Ptr vfh_tree2 (new pcl::search::KdTree<PointT> ());
	std::vector<pcl::PointCloud<pcl::VFHSignature308>::Ptr> computed_vfhs;	
	
	ne.setSearchMethod (vfh_tree1);
	ne.setRadiusSearch (0.05);
	vfh.setSearchMethod (vfh_tree2);
	vfh.setRadiusSearch (0.05);
	pcl::PointCloud<pcl::Normal>::Ptr normals;
	pcl::PointCloud<pcl::VFHSignature308>::Ptr vfhs;
//.........这里部分代码省略.........

int
main (int argc, char ** argv)
{
  if (argc < 2)
  {
    pcl::console::print_info ("Syntax is: %s {-p <pcd-file> OR -r <rgb-file> -d <depth-file>} \n --NT  (disables use of single camera transform) \n -o <output-file> \n -O <refined-output-file> \n-l <output-label-file> \n -L <refined-output-label-file> \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);
  }
  
  ///////////////////////////////  //////////////////////////////
  //////////////////////////////  //////////////////////////////
  ////// THIS IS ALL JUST INPUT HANDLING - Scroll down until 
  ////// pcl::SupervoxelClustering<pcl::PointXYZRGB> super
  //////////////////////////////  //////////////////////////////
  std::string rgb_path;
  bool rgb_file_specified = pcl::console::find_switch (argc, argv, "-r");
  if (rgb_file_specified)
    pcl::console::parse (argc, argv, "-r", rgb_path);
  
  std::string depth_path;
  bool depth_file_specified = pcl::console::find_switch (argc, argv, "-d");
  if (depth_file_specified)
    pcl::console::parse (argc, argv, "-d", depth_path);
  
  PointCloudT::Ptr cloud = boost::make_shared < PointCloudT >();
  NormalCloudT::Ptr input_normals = boost::make_shared < NormalCloudT > ();
  
  bool pcd_file_specified = pcl::console::find_switch (argc, argv, "-p");
  std::string pcd_path;
  if (!depth_file_specified || !rgb_file_specified)
  {
    std::cout << "Using point cloud\n";
    if (!pcd_file_specified)
    {
      std::cout << "No cloud specified!\n";
      return (1);
    }else
    {
      pcl::console::parse (argc,argv,"-p",pcd_path);
    }
  }
  
  bool disable_transform = pcl::console::find_switch (argc, argv, "--NT");
  bool ignore_provided_normals = pcl::console::find_switch (argc, argv, "--nonormals");
  bool has_normals = false;
  
  std::string out_path = "test_output.png";;
  pcl::console::parse (argc, argv, "-o", out_path);
  
  std::string out_label_path = "test_output_labels.png";
  pcl::console::parse (argc, argv, "-l", out_label_path);
  
  std::string refined_out_path = "refined_test_output.png";
  pcl::console::parse (argc, argv, "-O", refined_out_path);
  
  std::string refined_out_label_path = "refined_test_output_labels.png";;
  pcl::console::parse (argc, argv, "-L", refined_out_label_path);

  float voxel_resolution = 0.008f;
  pcl::console::parse (argc, argv, "-v", voxel_resolution);
    
  float seed_resolution = 0.08f;
  pcl::console::parse (argc, argv, "-s", seed_resolution);
  
  float color_importance = 0.2f;
  pcl::console::parse (argc, argv, "-c", color_importance);
  
  float spatial_importance = 0.4f;
  pcl::console::parse (argc, argv, "-z", spatial_importance);
  
  float normal_importance = 1.0f;
  pcl::console::parse (argc, argv, "-n", normal_importance);
  
  if (!pcd_file_specified)
  {
    //Read the images
    vtkSmartPointer<vtkImageReader2Factory> reader_factory = vtkSmartPointer<vtkImageReader2Factory>::New ();
    vtkImageReader2* rgb_reader = reader_factory->CreateImageReader2 (rgb_path.c_str ());
    //qDebug () << "RGB File="<< QString::fromStdString(rgb_path);
    if ( ! rgb_reader->CanReadFile (rgb_path.c_str ()))
    {
      std::cout << "Cannot read rgb image file!";
      return (1);
    }
    rgb_reader->SetFileName (rgb_path.c_str ());
    rgb_reader->Update ();
    //qDebug () << "Depth File="<<QString::fromStdString(depth_path);
    vtkImageReader2* depth_reader = reader_factory->CreateImageReader2 (depth_path.c_str ());
    if ( ! depth_reader->CanReadFile (depth_path.c_str ()))
    {
      std::cout << "Cannot read depth image file!";
      return (1);
    }
    depth_reader->SetFileName (depth_path.c_str ());
    depth_reader->Update ();
    
    vtkSmartPointer<vtkImageFlip> flipXFilter = vtkSmartPointer<vtkImageFlip>::New();
    flipXFilter->SetFilteredAxis(0); // flip x axis
    flipXFilter->SetInputConnection(rgb_reader->GetOutputPort());
    flipXFilter->Update();
//.........这里部分代码省略.........

int main (int argc, char** argv)
{
  ros::init(argc, argv, "cv_proj");
  //std::string input_file = "/home/igor/pcds/assembly_objs/ardrone_02_indoor.pcd";
  //std::string input_file = "/home/igor/pcds/assembly_objs/ardrone_03_outdoor.pcd";
  /*
  std::string input_file = "/home/igor/pcds/cluttered/3_objs_ardrone_indoor.pcd";
  std::string output_file = "/home/igor/pcds/cv_proj_out/out_cluttered_indoor_01.pcd";
  std::string template_file = "/home/igor/pcds/templates/indoor_template.pcd";
  std::string out_rgb = "/home/igor/pcds/cv_proj_out/out_result_05.jpg";
  */
  std::string input_file, out_pcd, template_file, out_rgb, out_transf_pcd;

  ros::param::param<std::string>("/cv_proj/input_file", input_file, "/home/igor/pcds/cluttered/3_objs_ardrone_indoor.pcd");
  //ros::param::param<std::string>("/cv_proj/out_pcd", out_pcd, "/home/igor/pcds/cv_proj_out/out_cluttered_indoor_01.pcd");
  ros::param::param<std::string>("/cv_proj/template_file", template_file, "/home/igor/pcds/templates/indoor_template.pcd");
  //ros::param::param<std::string>("/cv_proj/out_rgb", out_rgb, "/home/igor/pcds/cv_proj_out/out_result_05.jpg");
  


  boost::filesystem::path filepath(input_file);
  boost::filesystem::path filename = filepath.filename();
  filename = filename.stem(); // Get rid of the extension
  boost::filesystem::path dir = filepath.parent_path();

  std::string opencv_out_ext = "_filtered.png";
  std::string pcl_out_ext = "_filtered.pcd";
  std::string output_folder = "/output_cv_proj/";
  std::string output_stem;
  output_stem = dir.string() + output_folder + filename.string();

  out_rgb = output_stem + opencv_out_ext;
  out_pcd = output_stem + pcl_out_ext;
  out_transf_pcd = output_stem + "_templ" + pcl_out_ext;

  std::cout << out_rgb << std::endl;
  std::cout << out_pcd << std::endl;

  // Read in the cloud data
  pcl::PCDReader reader;
  PointCloudT::Ptr cloud (new PointCloudT), cloud_f (new PointCloudT);
  reader.read (input_file, *cloud);
  std::cout << "PointCloud before filtering has: " << cloud->points.size () << " data points." << std::endl; //*

  if (cloud->isOrganized()) {
    std::cout << "-- PointCloud cloud is organized" << std::endl;
    std::cout << "-- PointCloud cloud width: " << cloud->width << std::endl;
    std::cout << "-- PointCloud cloud height: " << cloud->height << std::endl;
  }

  /*
  // Create the filtering object: downsample the dataset using a leaf size of 1cm
  pcl::VoxelGrid<PointT> vg;
  PointCloudT::Ptr cloud_filtered (new PointCloudT);
  vg.setInputCloud (cloud);
  //vg.setLeafSize (0.01f, 0.01f, 0.01f);
  vg.setLeafSize (0.001f, 0.001f, 0.001f);
  //
  vg.filter (*cloud_filtered);
  std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size ()  << " data points." << std::endl; //*
  */
  /**/
  PointCloudT::Ptr cloud_filtered (new PointCloudT);
  *cloud_filtered = *cloud;



  // Create the segmentation object for the planar model and set all the parameters
  pcl::SACSegmentation<PointT> seg;
  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
  PointCloudT::Ptr cloud_plane (new PointCloudT ());
  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<PointT> extract;

    extract.setInputCloud (cloud_filtered);
    extract.setIndices (inliers);
    extract.setNegative (false);

    //extract.setUserFilterValue(999);
    extract.setKeepOrganized(true); 
//.........这里部分代码省略.........

// Align a rigid object to a scene with clutter and occlusions
int main (int argc, char **argv)
{

  // Point clouds
  PointCloudT::Ptr object (new PointCloudT);
  PointCloudT::Ptr scene (new PointCloudT);
  PointCloudT::Ptr object_aligned (new PointCloudT);

  // Get input object and scene
  if (argc < 3)
  {
    pcl::console::print_error ("Syntax is: %s object.pcd scene.pcd\n", argv[0]);
    return (1);
  }
  pcl::console::parse_argument (argc, argv, "--max_iterations", in_max_iterations);
  pcl::console::parse_argument (argc, argv, "--num_samples", in_num_samples);
  pcl::console::parse_argument (argc, argv, "--s_thresh", in_similarity_thresh);
  pcl::console::parse_argument (argc, argv, "--max_cdist", in_max_corresp_dist);
  pcl::console::parse_argument (argc, argv, "--inlier_frac", in_inlier_frac);
  pcl::console::parse_argument (argc, argv, "--leaf", in_leaf);
  pcl::console::parse_argument (argc, argv, "--normal_radius", normal_radius);
  pcl::console::parse_argument (argc, argv, "--feature_radius", feature_radius);

  pcl::console::parse_argument (argc, argv, "--icp", in_icp);
  pcl::console::parse_argument (argc, argv, "--max_corr_icp", max_corr_icp);
  pcl::console::parse_argument (argc, argv, "--icp_eps", max_eps_icp);

  // Load object and scene
  pcl::console::print_highlight ("Loading point clouds...\n");

  pcl_tools::cloud_from_stl(argv[2], *object);

  if (pcl::io::loadPCDFile<PointNT> (argv[1], *scene) < 0)
  {
    pcl::console::print_error ("Error loading object/scene file!\n");
    return (1);
  }

  std::vector<int> indices;
  pcl::removeNaNFromPointCloud(*scene, *scene, indices);
  pcl::removeNaNFromPointCloud(*object, *object, indices);

  // /*pcl_tools::icp_result align = */alp_align(object, scene, object_aligned, 50000, 3, 0.9f, 5.5f * leaf, 0.7f);
  // /*pcl_tools::icp_result align = */alp_align(object_aligned, scene, object_aligned, 50000, 3, 0.9f, 7.5f * leaf, 0.4f);

  std::cout << "Inlier frac " << in_inlier_frac << std::endl;
  pcl_tools::icp_result align = alp_align(object, scene, object_aligned, in_max_iterations, in_num_samples, in_similarity_thresh, in_max_corresp_dist, in_inlier_frac, in_leaf);

  pcl::visualization::PCLVisualizer visu("Alignment");
  if (align.converged)
  {
    pcl::console::print_info ("Inliers: %i/%i, scene: %i\n", align.inliers, object->size (), scene->size ());
    
    // Show alignment
    visu.addPointCloud (object, ColorHandlerT (object, 255.0, 0.0, 0.0), "object");
    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.addPointCloudNormals<PointNT>(object);

    visu.spin ();
  }
  else
  {
    pcl::console::print_error ("Alignment failed!\n");
    return (1);
  }

  if (in_icp) {
    pcl::console::print_highlight ("Applying ICP now\n");
    pcl::IterativeClosestPointNonLinear<PointNT, PointNT> icp;
    // pcl::IterativeClosestPoint<PointNT, PointNT> icp;
    pcl_tools::affine_cloud(Eigen::Vector3f::UnitZ(), 0.0, Eigen::Vector3f(0.0, 0.0, 0.02), *object_aligned, *object_aligned);

    icp.setMaximumIterations (100);
    icp.setMaxCorrespondenceDistance(max_corr_icp);
    icp.setTransformationEpsilon (max_eps_icp);
    icp.setInputSource (object_aligned);
    icp.setInputTarget (scene);
    icp.align (*object_aligned);

    if (icp.hasConverged()) {
      pcl::console::print_highlight ("ICP: Converged with fitness %f\n", icp.getFitnessScore());
    }
    // pcl::visualization::PCLVisualizer visu("Alignment");
    // visu.addPointCloud (object, ColorHandlerT (object, 255.0, 0.0, 0.0), "object");
    // visu.addPointCloud (scene, ColorHandlerT (scene, 0.0, 255.0, 0.0), "scene");
    visu.updatePointCloud (object_aligned, ColorHandlerT (object_aligned, 100.0, 50.0, 200.0), "object_aligned");

    // visu.addPointCloudNormals<PointNT>(object);

    visu.spin ();

  }
  return (0);
}

/* collects a cloud by aggregating k successive frames */
void waitForCloudK(int k){
	ros::Rate r(30);
	
	cloud_aggregated->clear();
	
	int counter = 0;
	
	while (ros::ok()){
		ros::spinOnce();
		
		r.sleep();
		
		if (new_cloud_available_flag){
			
			*cloud_aggregated+=*cloud;
			
			new_cloud_available_flag = false;
			
			counter ++;
			
			if (counter >= k){
				cloud_aggregated->header = cloud->header;
				break;
			}
		}
	}
	
}