C++ PCLVisualizer::spinOnce方法代码示例

int main (int argc, char const* argv[])
{
	if (argc != 2) {
		cout << "Usage : obb_test filename.pcd" << endl;
		return 1;
	}
	
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
 	if (pcl::io::loadPCDFile<pcl::PointXYZ> (argv[1], *cloud) == -1) {
    PCL_ERROR ("Couldn't read file test_pcd.pcd \n");
    return 1;
  }

	cloud_viewer.addPointCloud (cloud, "single_cloud");

	OrientedBoundingBox obb;
	Eigen::Quaternionf q;
	Eigen::Vector3f t, dims;
	obb.compute_obb_pca (cloud, q, t, dims);
  cloud_viewer.addCube(t, q, dims.x(), dims.y(), dims.z());
  cout << dims.x() << " " << dims.y() << " " << dims.z() << endl;
  
  while (!cloud_viewer.wasStopped()) {
    cloud_viewer.spinOnce(1);
	}
  
	return 0;
}

void Inspector::updateDepth(const openni_wrapper::Image& image,
                            const openni_wrapper::DepthImage& depth)
{
  frame_.depth_->setZero();
  ushort data[depth.getHeight() * depth.getWidth()];
  depth.fillDepthImageRaw(depth.getWidth(), depth.getHeight(), data);
  int i = 0;
  for(size_t y = 0; y < depth.getHeight(); ++y) {
    for(size_t x = 0; x < depth.getWidth(); ++x, ++i) {
      if(data[i] == depth.getNoSampleValue() || data[i] == depth.getShadowValue())
        continue;
      frame_.depth_->coeffRef(y, x) = data[i];
    }
  }

  if(dddm_ && use_intrinsics_)
    dddm_->undistort(frame_.depth_.get());

  frame_.img_ = oniToCV(image);
    
  Cloud::Ptr cloud(new Cloud);
  FrameProjector proj;
  proj.width_ = 640;
  proj.height_ = 480;
  proj.cx_ = proj.width_ / 2;
  proj.cy_ = proj.height_ / 2;
  proj.fx_ = 525;
  proj.fy_ = 525;
  proj.frameToCloud(frame_, cloud.get());
  pcd_vis_.updatePointCloud(cloud, "cloud");
  pcd_vis_.spinOnce(5);
}

    void OnNewMapFrame(pcl::PointCloud< pcl::PointXYZ > mapFrame)
    {
        _viewer.removeAllPointClouds(0);
        _viewer.addPointCloud(mapFrame.makeShared(), "map");
        _viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "map");
        _viewer.spinOnce();

    }

 /* \brief Graphic loop for the viewer
  *
  */
 void run()
 {
   while (!viz.wasStopped())
   {
     //main loop of the visualizer
     viz.spinOnce(100);
     boost::this_thread::sleep(boost::posix_time::microseconds(100000));
   }
 }

 void
 run ()
 {
   // initial processing
   process ();
 
   while (!viewer.wasStopped ())
     viewer.spinOnce (100);
 }

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

    if(argc < 3) {
        PCL_ERROR("run as ./project /path/to/cloud1/ /path/to/cloud2/ [pcd format]");
        return -1;
    }

    string fCloud1 = "";
    string fCloud2 = "";
    string models = "";
    string training = "";
    int NN;

    pcl::console::parse_argument(argc, argv, "-cloud1", fCloud1);
    pcl::console::parse_argument(argc, argv, "-cloud2", fCloud2);
    pcl::console::parse_argument(argc, argv, "-models", models);
    pcl::console::parse_argument(argc, argv, "-training", training);
    pcl::console::parse_argument(argc, argv, "-nn", NN);

    moi.setVerbose(true);


    classificator.setModelsDir(models);
    classificator.setTrainingDir(training);
    classificator.setNN(NN);
    classificator.setup();

    viewer.registerKeyboardCallback(&keyboard_cb, NULL);
    viewer.setBackgroundColor(0, 0, 0);

    viewer.initCameraParameters();

    if(fCloud1 != "" && fCloud2 != "") {
        if(pcl::io::loadPCDFile<pcl::PointXYZ>(fCloud1, *cloud1) == -1) {
            PCL_ERROR("Cloud1 reading failed\n");
            return(-1);
        }

        if(pcl::io::loadPCDFile<pcl::PointXYZ>(fCloud2, *cloud2) == -1) {
            PCL_ERROR("Cloud2 reading failed\n");
            return(-1);
        }

        findObjectsAndClassify();
    } else {
        keyboardCbLock = false;
        printInstructions();
    }

    while(!viewer.wasStopped()) {
        viewer.spinOnce(100);
        boost::this_thread::sleep (boost::posix_time::milliseconds(100));
    }

    return 0;
}

//.........这里部分代码省略.........
  new_cloud_available_flag = false;

  cloud_mutex.lock ();    // for not overwriting the point cloud

  // Display pointcloud:
  pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud);
  viewer.addPointCloud<PointT> (cloud, rgb, "input_cloud");
  viewer.setCameraPosition(0,0,-2,0,-1,0,0);

  // Add point picking callback to viewer:
  struct callback_args cb_args;
  PointCloudT::Ptr clicked_points_3d (new PointCloudT);
  cb_args.clicked_points_3d = clicked_points_3d;
  cb_args.viewerPtr = pcl::visualization::PCLVisualizer::Ptr(&viewer);
  viewer.registerPointPickingCallback (pp_callback, (void*)&cb_args);
  std::cout << "Shift+click on three floor points, then press 'Q'..." << std::endl;

  // Spin until 'Q' is pressed:
  viewer.spin();
  std::cout << "done." << std::endl;
  
  cloud_mutex.unlock ();    

  // Ground plane estimation:
  Eigen::VectorXf ground_coeffs;
  ground_coeffs.resize(4);
  std::vector<int> clicked_points_indices;
  for (unsigned int i = 0; i < clicked_points_3d->points.size(); i++)
    clicked_points_indices.push_back(i);
  pcl::SampleConsensusModelPlane<PointT> model_plane(clicked_points_3d);
  model_plane.computeModelCoefficients(clicked_points_indices,ground_coeffs);
  std::cout << "Ground plane: " << ground_coeffs(0) << " " << ground_coeffs(1) << " " << ground_coeffs(2) << " " << ground_coeffs(3) << std::endl;

  // Initialize new viewer:
  pcl::visualization::PCLVisualizer viewer("PCL Viewer");          // viewer initialization
  viewer.setCameraPosition(0,0,-2,0,-1,0,0);

  // Create classifier for people detection:  
  pcl::people::PersonClassifier<pcl::RGB> person_classifier;
  person_classifier.loadSVMFromFile(svm_filename);   // load trained SVM

  // People detection app initialization:
  pcl::people::GroundBasedPeopleDetectionApp<PointT> people_detector;    // people detection object
  people_detector.setVoxelSize(voxel_size);                        // set the voxel size
  people_detector.setIntrinsics(rgb_intrinsics_matrix);            // set RGB camera intrinsic parameters
  people_detector.setClassifier(person_classifier);                // set person classifier
  people_detector.setHeightLimits(min_height, max_height);         // set person classifier
//  people_detector.setSensorPortraitOrientation(true);             // set sensor orientation to vertical

  // For timing:
  static unsigned count = 0;
  static double last = pcl::getTime ();

  // Main loop:
  while (!viewer.wasStopped())
  {
    if (new_cloud_available_flag && cloud_mutex.try_lock ())    // if a new cloud is available
    {
      new_cloud_available_flag = false;

      // Perform people detection on the new cloud:
      std::vector<pcl::people::PersonCluster<PointT> > clusters;   // vector containing persons clusters
      people_detector.setInputCloud(cloud);
      people_detector.setGround(ground_coeffs);                    // set floor coefficients
      people_detector.compute(clusters);                           // perform people detection

      ground_coeffs = people_detector.getGround();                 // get updated floor coefficients

      // Draw cloud and people bounding boxes in the viewer:
      viewer.removeAllPointClouds();
      viewer.removeAllShapes();
      pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud);
      viewer.addPointCloud<PointT> (cloud, rgb, "input_cloud");
      unsigned int k = 0;
      for(std::vector<pcl::people::PersonCluster<PointT> >::iterator it = clusters.begin(); it != clusters.end(); ++it)
      {
        if(it->getPersonConfidence() > min_confidence)             // draw only people with confidence above a threshold
        {
          // draw theoretical person bounding box in the PCL viewer:
          it->drawTBoundingBox(viewer, k);
          k++;
        }
      }
      std::cout << k << " people found" << std::endl;
      viewer.spinOnce();

      // Display average framerate:
      if (++count == 30)
      {
        double now = pcl::getTime ();
        std::cout << "Average framerate: " << double(count)/double(now - last) << " Hz" <<  std::endl;
        count = 0;
        last = now;
      }
      cloud_mutex.unlock ();
    }
  }

  return 0;
}

void
compute (const sensor_msgs::PointCloud2::ConstPtr &input, sensor_msgs::PointCloud2 &output,
         float th_dd, int max_search)
{
  CloudPtr cloud (new Cloud);
  fromROSMsg (*input, *cloud);

  pcl::PointCloud<pcl::Normal>::Ptr normal (new pcl::PointCloud<pcl::Normal>);
  pcl::IntegralImageNormalEstimation<PointXYZRGBA, pcl::Normal> ne;
  ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
  ne.setNormalSmoothingSize (10.0f);
  ne.setBorderPolicy (ne.BORDER_POLICY_MIRROR);
  ne.setInputCloud (cloud);
  ne.compute (*normal);

  TicToc tt;
  tt.tic ();

  //OrganizedEdgeBase<PointXYZRGBA, Label> oed;
  //OrganizedEdgeFromRGB<PointXYZRGBA, Label> oed;
  //OrganizedEdgeFromNormals<PointXYZRGBA, Normal, Label> oed;
  OrganizedEdgeFromRGBNormals<PointXYZRGBA, Normal, Label> oed;
  oed.setInputNormals (normal);
  oed.setInputCloud (cloud);
  oed.setDepthDisconThreshold (th_dd);
  oed.setMaxSearchNeighbors (max_search);
  oed.setEdgeType (oed.EDGELABEL_NAN_BOUNDARY | oed.EDGELABEL_OCCLUDING | oed.EDGELABEL_OCCLUDED | oed.EDGELABEL_HIGH_CURVATURE | oed.EDGELABEL_RGB_CANNY);
  PointCloud<Label> labels;
  std::vector<PointIndices> label_indices;
  oed.compute (labels, label_indices);
  print_info ("Detecting all edges... [done, "); print_value ("%g", tt.toc ()); print_info (" ms]\n");

  // Make gray point clouds
  for (int idx = 0; idx < (int)cloud->points.size (); idx++)
  {
    uint8_t gray = (cloud->points[idx].r + cloud->points[idx].g + cloud->points[idx].b)/3;
    cloud->points[idx].r = cloud->points[idx].g = cloud->points[idx].b = gray;
  }

  // Display edges in PCLVisualizer
  viewer.setSize (640, 480);
  viewer.addCoordinateSystem (0.2f);
  viewer.addPointCloud (cloud, "original point cloud");
  viewer.registerKeyboardCallback(&keyboard_callback);

  pcl::PointCloud<pcl::PointXYZRGBA>::Ptr occluding_edges (new pcl::PointCloud<pcl::PointXYZRGBA>), 
    occluded_edges (new pcl::PointCloud<pcl::PointXYZRGBA>), 
    nan_boundary_edges (new pcl::PointCloud<pcl::PointXYZRGBA>),
    high_curvature_edges (new pcl::PointCloud<pcl::PointXYZRGBA>),
    rgb_edges (new pcl::PointCloud<pcl::PointXYZRGBA>);

  pcl::copyPointCloud (*cloud, label_indices[0].indices, *nan_boundary_edges);
  pcl::copyPointCloud (*cloud, label_indices[1].indices, *occluding_edges);
  pcl::copyPointCloud (*cloud, label_indices[2].indices, *occluded_edges);
  pcl::copyPointCloud (*cloud, label_indices[3].indices, *high_curvature_edges);
  pcl::copyPointCloud (*cloud, label_indices[4].indices, *rgb_edges);

  const int point_size = 2;
  viewer.addPointCloud<pcl::PointXYZRGBA> (nan_boundary_edges, "nan boundary edges");
  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "nan boundary edges");
  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 0.0f, 1.0f, "nan boundary edges");

  viewer.addPointCloud<pcl::PointXYZRGBA> (occluding_edges, "occluding edges");
  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "occluding edges");
  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 1.0f, 0.0f, "occluding edges");

  viewer.addPointCloud<pcl::PointXYZRGBA> (occluded_edges, "occluded edges");
  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "occluded edges");
  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0f, 0.0f, 0.0f, "occluded edges");

  viewer.addPointCloud<pcl::PointXYZRGBA> (high_curvature_edges, "high curvature edges");
  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "high curvature edges");
  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0f, 1.0f, 0.0f, "high curvature edges");

  viewer.addPointCloud<pcl::PointXYZRGBA> (rgb_edges, "rgb edges");
  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "rgb edges");
  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 1.0f, 1.0f, "rgb edges");

  while (!viewer.wasStopped ())
  {
    viewer.spinOnce ();
    pcl_sleep(0.1);
  }

  // Combine point clouds and edge labels
  sensor_msgs::PointCloud2 output_edges;  
  toROSMsg (labels, output_edges);
  concatenateFields (*input, output_edges, output);
}

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>);
 
   pcl::io::loadPCDFile (argv[1], *cloud);
 
    pcl::copyPointCloud( *cloud,*cloud_filtered);
 
    float i ;
    float j;
    float k;
 
    cv::namedWindow( "picture");
 
    cvCreateTrackbar("X_limit", "picture", &a, 30, NULL);
    cvCreateTrackbar("Y_limit", "picture", &b, 30, NULL);
    cvCreateTrackbar("Z_limit", "picture", &c, 30, NULL);
 
    char last_c = 0;

//	pcl::visualization::PCLVisualizer viewer ("picture");
        
        
	viewer.setBackgroundColor (0.0, 0.0, 0.5);//set backgroung according to the color of points

	pcl::PassThrough<pcl::PointXYZ> pass;
        

	while (!viewer.wasStopped ())
             {

               
		pcl::copyPointCloud(*cloud_filtered, *cloud);
 
        
 
        i = 0.1*((float)a);
        j = 0.1*((float)b);
        k = 0.1*((float)c);
 
        
 
//        cout << "i = " << i << " j = " << j << " k = " << k << endl;
 
        
        pass.setInputCloud (cloud);
	pass.setFilterFieldName ("y");
        pass.setFilterLimits (-j, j);
        pass.filter (*cloud);
 
        pass.setInputCloud (cloud);
        pass.setFilterFieldName ("x");
        pass.setFilterLimits (-i, i);
        pass.filter (*cloud);

	pass.setInputCloud (cloud);
        pass.setFilterFieldName ("z");
        pass.setFilterLimits (-k,k);
        pass.filter (*cloud);

	viewer.addPointCloud (cloud, "scene_cloud");
	viewer.spinOnce ();
	viewer.removePointCloud("scene_cloud");
	waitKey(10);
	}

return 0;
}

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

  //ROS Initialization
  ros::init(argc, argv, "detecting_people");
  ros::NodeHandle nh;
  ros::Rate rate(13);

  ros::Subscriber state_sub = nh.subscribe("followme_state", 5, &stateCallback);
  ros::Publisher people_pub = nh.advertise<frmsg::people>("followme_people", 5);
  frmsg::people pub_people_;

  CloudConverter* cc_ = new CloudConverter();

  while (!cc_->ready_xyzrgb_)
  {
    ros::spinOnce();
    rate.sleep();
    if (!ros::ok())
    {
      printf("Terminated by Control-c.\n");
      return -1;
    }
  }

  // Input parameter from the .yaml
  std::string package_path_ = ros::package::getPath("detecting_people") + "/";
  cv::FileStorage* fs_ = new cv::FileStorage(package_path_ + "parameters.yml", cv::FileStorage::READ);

  // Algorithm parameters:
  std::string svm_filename = package_path_ + "trainedLinearSVMForPeopleDetectionWithHOG.yaml";
  std::cout << svm_filename << std::endl;

  float min_confidence = -1.5;
  float min_height = 1.3;
  float max_height = 2.3;
  float voxel_size = 0.06;
  Eigen::Matrix3f rgb_intrinsics_matrix;
  rgb_intrinsics_matrix << 525, 0.0, 319.5, 0.0, 525, 239.5, 0.0, 0.0, 1.0; // Kinect RGB camera intrinsics
  
  // Read if some parameters are passed from command line:
  pcl::console::parse_argument (argc, argv, "--svm", svm_filename);
  pcl::console::parse_argument (argc, argv, "--conf", min_confidence);
  pcl::console::parse_argument (argc, argv, "--min_h", min_height);
  pcl::console::parse_argument (argc, argv, "--max_h", max_height);


  // Read Kinect live stream:
  PointCloudT::Ptr cloud_people (new PointCloudT);
  cc_->ready_xyzrgb_ = false;
  while ( !cc_->ready_xyzrgb_ )
  {
    ros::spinOnce();
    rate.sleep();
  }
  pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud = cc_->msg_xyzrgb_;

  // Display pointcloud:
  pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud);
  viewer.addPointCloud<PointT> (cloud, rgb, "input_cloud");
  viewer.setCameraPosition(0,0,-2,0,-1,0,0);

  // Add point picking callback to viewer:
  struct callback_args cb_args;
  PointCloudT::Ptr clicked_points_3d (new PointCloudT);
  cb_args.clicked_points_3d = clicked_points_3d;
  cb_args.viewerPtr = pcl::visualization::PCLVisualizer::Ptr(&viewer);
  viewer.registerPointPickingCallback (pp_callback, (void*)&cb_args);
  std::cout << "Shift+click on three floor points, then press 'Q'..." << std::endl;

  // Spin until 'Q' is pressed:
  viewer.spin();
  std::cout << "done." << std::endl;
  
  //cloud_mutex.unlock ();    

  // Ground plane estimation:
  Eigen::VectorXf ground_coeffs;
  ground_coeffs.resize(4);
  std::vector<int> clicked_points_indices;
  for (unsigned int i = 0; i < clicked_points_3d->points.size(); i++)
    clicked_points_indices.push_back(i);
  pcl::SampleConsensusModelPlane<PointT> model_plane(clicked_points_3d);
  model_plane.computeModelCoefficients(clicked_points_indices,ground_coeffs);
  std::cout << "Ground plane: " << ground_coeffs(0) << " " << ground_coeffs(1) << " " << ground_coeffs(2) << " " << ground_coeffs(3) << std::endl;

  // Initialize new viewer:
  pcl::visualization::PCLVisualizer viewer("PCL Viewer");          // viewer initialization
  viewer.setCameraPosition(0,0,-2,0,-1,0,0);

  // Create classifier for people detection:  
  pcl::people::PersonClassifier<pcl::RGB> person_classifier;
  person_classifier.loadSVMFromFile(svm_filename);   // load trained SVM

  // People detection app initialization:
  pcl::people::GroundBasedPeopleDetectionApp<PointT> people_detector;    // people detection object
  people_detector.setVoxelSize(voxel_size);                        // set the voxel size
  people_detector.setIntrinsics(rgb_intrinsics_matrix);            // set RGB camera intrinsic parameters
  people_detector.setClassifier(person_classifier);                // set person classifier
  people_detector.setHeightLimits(min_height, max_height);         // set person classifier
//.........这里部分代码省略.........

int main(int argc,char** argv){
	if (argc < 2){
		std::cout<<"Please enter <input.pcd> file"<<std::endl;
		return 0;
	}
	if (pcl::io::loadPCDFile (argv[1], *model) < 0)
	{
    	std::cout << "Error loading model cloud." << std::endl;
    	return (-1);
  	}
  	model_name = argv[1];
  	model_name = model_name.substr(0,model_name.size()-4);
  	if(pcl::console::find_switch(argc,argv,"-vfh")){
  		vfh = true;
  	}
  	else if(pcl::console::find_switch(argc,argv,"-rv")){
  		std::cout<<"performing Resultant vector feature calculation"<<std::endl;
  		rv = true;
  	}else{
  		std::cout<<"no algorithm specified using default algorithm vfh"<<std::endl;
  		vfh = true;
  	}
  	if (pcl::console::find_switch (argc, argv, "-ds"))
  	{
    	_downsample = true;
    	std::cout<<"performing downsampling on the input file"<<std::endl;
  	}
  	if (pcl::console::find_switch (argc, argv, "-sn"))
  	{
    	show_normals = true;
    	std::cout<<"showing calclated normals"<<std::endl;
  	}
  	if(_downsample){
  		rec.setInputCloud(model,_leaf_size);
  		std::cout<<"saving downsampled file to model_down.pcd"<<std::endl;
  		pcl::io::savePCDFileASCII ("model_down.pcd", *(rec.getCloud()));
  	}
  	else{
  		rec.setInputCloud(model);
  		std::cout<<"setting input model without further downsampling"<<std::endl;
  	}
  	if(pcl::console::find_switch(argc,argv,"--showaxis")){
  		_show_axis = true;
  	}
	if(vfh){
		std::cout<<"estimating VFH features"<<std::endl;
		rec.Estimate_VFH_Features();
	}
	else if(rv){
		std::cout<<"estimating features using the resultant vector"<<std::endl;
		rec.Estimate_RV_Features();
		PointNormalCloudT::Ptr cloud (new PointNormalCloudT);
		pcl::PointCloud<pcl::Normal>::Ptr norm_cloud (new pcl::PointCloud<pcl::Normal>);
		cloud = rec.getPointNormalCloud();
		norm_cloud = rec.getNormalCloud();
		pcl::PointCloud<pcl::PointXYZ>::Ptr plaincloud (new pcl::PointCloud<pcl::PointXYZ>);
		pcl::copyPointCloud(*cloud,*plaincloud);
		//pcl::PointXYZ mass_center(rec.rv_centroid.x,rec.rv_centroid.y,rec.rv_centroid.z);
		pcl::PointXYZ mass_center(0,0,0);
		pcl::PointXYZ kinectZ(0,0,-1);
		pcl::PointXYZ res_vec (rec.rv_resultant.normal_x,rec.rv_resultant.normal_y,rec.rv_resultant.normal_z);
		//pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZ> rgb(plaincloud);
        //viewer.addPointCloud<pcl::PointXYZ> (plaincloud, rgb, "model_cloud");
        
        pcl::visualization::PointCloudColorHandlerCustom<pcl::PointNormal> single_color(cloud, double(0), double(255), double(0));
        viewer.addPointCloud(cloud,single_color,"sample cloud");
        if(_show_axis){
        	viewer.addLine(mass_center,res_vec,1.0f,0.0f,0.0f,"resultantvector");
        	viewer.addLine(mass_center,kinectZ,1.0f,1.0f,0.0f,"KinectZ");
        }
        std::cout<<"resultant vector :"<<res_vec.x<<" i"<<" + "<<res_vec.y<<" j"<<" + "<<res_vec.z<<" k"<<std::endl;
        if(show_normals){
        	std::cout<<"showing 1 in "<<normalsratio<<" normals"<<std::endl;
        	viewer.addPointCloudNormals<pcl::PointNormal,pcl::Normal>(cloud, norm_cloud,normalsratio, 0.05, "normalscloud");
        }
        while(!viewer.wasStopped())
        	viewer.spinOnce();
	}
	std::cout<<"feature calculation complete"<<std::endl;
	
	ofstream myfile;
	
	if (vfh){
		std::stringstream ss;
		ss<<"vfh_"<<model_name<<".txt";
		myfile.open(ss.str().c_str());
		for(size_t k =0 ;k<308;k++){
			if(k != 307)
				myfile << rec._vfh_features->points[0].histogram[k]<<",";
			else 
				myfile << rec._vfh_features->points[0].histogram[k];
		}
		std::cout<<"wrote the histogram to file :" <<ss.str()<<std::endl;
		myfile.close();
	}else if(rv){
		std::stringstream ss;
		ss<<"rv_"<<model_name<<".txt";
		myfile.open(ss.str().c_str());
		for(int k = 0;k<181;k++){
			if(k != 180)
//.........这里部分代码省略.........

//.........这里部分代码省略.........
    vis.addPointCloud (scene_vis, "scene_cloud");
  }

  if (heat_map)
  {
    pcl::PointCloud<pcl::PointXYZI>::Ptr intensity_cloud (new pcl::PointCloud<pcl::PointXYZI>);
    fdrf.getFaceHeatMap (intensity_cloud);

    pcl::visualization::PointCloudColorHandlerGenericField < pcl::PointXYZI > handler_keypoints (intensity_cloud, "intensity");
    vis.addPointCloud < pcl::PointXYZI > (intensity_cloud, handler_keypoints, "heat_map");
  }

  if (show_votes)
  {
    //display votes_
    /*pcl::PointCloud<pcl::PointXYZ>::Ptr votes_cloud(new pcl::PointCloud<pcl::PointXYZ>());
     fdrf.getVotes(votes_cloud);
     pcl::visualization::PointCloudColorHandlerCustom < pcl::PointXYZ > handler_votes(votes_cloud, 255, 0, 0);
     vis.addPointCloud < pcl::PointXYZ > (votes_cloud, handler_votes, "votes_cloud");
     vis.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 14, "votes_cloud");
     vis.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.5, "votes_cloud");
     vis.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.75, "votes_cloud");*/

    pcl::PointCloud<pcl::PointXYZI>::Ptr votes_cloud (new pcl::PointCloud<pcl::PointXYZI> ());
    fdrf.getVotes2 (votes_cloud);
    pcl::visualization::PointCloudColorHandlerGenericField < pcl::PointXYZI > handler_votes (votes_cloud, "intensity");
    vis.addPointCloud < pcl::PointXYZI > (votes_cloud, handler_votes, "votes_cloud");
    vis.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 14, "votes_cloud");
  }

  vis.addCoordinateSystem (0.1, "global");

  std::vector<Eigen::VectorXd> heads;
  fdrf.getDetectedFaces (heads);
  face_detection_apps_utils::displayHeads (heads, vis);

  if (SHOW_GT)
  {
    //check if there is ground truth data
    std::string pose_file (filename);
    boost::replace_all (pose_file, ".pcd", "_pose.txt");

    Eigen::Matrix4d pose_mat;
    pose_mat.setIdentity (4, 4);
    bool result = face_detection_apps_utils::readMatrixFromFile (pose_file, pose_mat);

    if (result)
    {
      Eigen::Vector3d ea = pose_mat.block<3, 3> (0, 0).eulerAngles (0, 1, 2);
      Eigen::Vector3d trans_vector = Eigen::Vector3d (pose_mat (0, 3), pose_mat (1, 3), pose_mat (2, 3));
      std::cout << ea << std::endl;
      std::cout << trans_vector << std::endl;

      pcl::PointXYZ center_point;
      center_point.x = trans_vector[0];
      center_point.y = trans_vector[1];
      center_point.z = trans_vector[2];
      vis.addSphere (center_point, 0.05, 255, 0, 0, "sphere");

      pcl::ModelCoefficients cylinder_coeff;
      cylinder_coeff.values.resize (7); // We need 7 values
      cylinder_coeff.values[0] = center_point.x;
      cylinder_coeff.values[1] = center_point.y;
      cylinder_coeff.values[2] = center_point.z;

      cylinder_coeff.values[3] = ea[0];
      cylinder_coeff.values[4] = ea[1];
      cylinder_coeff.values[5] = ea[2];

      Eigen::Vector3d vec = Eigen::Vector3d::UnitZ () * -1.;
      Eigen::Matrix3d matrixxx;

      matrixxx = Eigen::AngleAxisd (ea[0], Eigen::Vector3d::UnitX ()) * Eigen::AngleAxisd (ea[1], Eigen::Vector3d::UnitY ())
          * Eigen::AngleAxisd (ea[2], Eigen::Vector3d::UnitZ ());

      //matrixxx = pose_mat.block<3,3>(0,0);
      vec = matrixxx * vec;

      cylinder_coeff.values[3] = vec[0];
      cylinder_coeff.values[4] = vec[1];
      cylinder_coeff.values[5] = vec[2];

      cylinder_coeff.values[6] = 0.01;
      vis.addCylinder (cylinder_coeff, "cylinder");
    }
  }

  vis.setRepresentationToSurfaceForAllActors ();

  if (VIDEO)
  {
    vis.spinOnce (50, true);
  } else
  {
    vis.spin ();
  }

  vis.removeAllPointClouds ();
  vis.removeAllShapes ();
}

int 
main (int argc, char** argv)
{
  // Read in the cloud data
  pcl::PCDReader reader;
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
  reader.read ("../bottle.pcd", *cloud);
  std::cout << "PointCloud before filtering has: " << cloud->points.size () << " data points." << std::endl; //*
   parseCommandLine(argc, argv);
   std::cout << "argc:" << argc << " argv:" << *argv << std::endl;
   std::cout << "x_min:" << x_pass_min_ << " x_max:" << x_pass_max_ << " y_min:" << y_pass_min_ << " y_max:"<<y_pass_max_<<std::endl;

	/*apply pass through filter*/
	
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
	// Create the filtering object
	pcl::PassThrough<pcl::PointXYZ> pass;
	//pass.setFilterLimitsNegative (true);
	pass.setInputCloud (cloud);
	pass.setFilterFieldName ("z");
	pass.setFilterLimits (z_pass_min_, z_pass_max_);
	pass.filter (*cloud_filtered);

        pass.setInputCloud (cloud_filtered);
	pass.setFilterFieldName ("y");
	pass.setFilterLimits (y_pass_min_, y_pass_max_);
	pass.filter (*cloud_filtered);
	
        pass.setInputCloud (cloud_filtered);
	pass.setFilterFieldName ("x");
	pass.setFilterLimits (x_pass_min_, x_pass_max_);
	pass.filter (*cloud_filtered);

	viewer.addPointCloud<PointT> (cloud_filtered, "input_cloud");
        #if 0	
       // while (!viewer.wasStopped ())
	{
	//	viewer.spinOnce ();
	} 
        return(0);
        #endif	
        
	viewer.removeAllPointClouds();
        cloud = cloud_filtered;

    
#if 0
#if 0
  // Create the filtering object: downsample the dataset using a leaf size of 1cm
  pcl::VoxelGrid<pcl::PointXYZ> vg;
  //pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
  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; //*
#endif
  // 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;
  }

  viewer.addPointCloud<PointT> (cloud_filtered, "input_cloud");
  while (!viewer.wasStopped ())
	{
		viewer.spinOnce ();
//.........这里部分代码省略.........