C++ CloudViewer::wasStopped方法代码示例

    /***********************************************************************************************************************
     * @BRIEF Starts data acquisition and handling
     * @AUTHOR Christopher D. McMurrough
     **********************************************************************************************************************/
    void run()
    {
        // create a new grabber for OpenNI2 devices
        pcl::Grabber* interface = new pcl::io::OpenNI2Grabber();

        // bind the callbacks to the appropriate member functions
        boost::function<void (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)> f = boost::bind(&OpenNI2Processor::cloudCallback, this, _1);

        // connect callback function for desired signal. In this case its a point cloud with color values
        interface->registerCallback(f);

        // start receiving point clouds
        interface->start();

        // start the timer
        m_stopWatch.reset();

        // wait until user quits program
        while (!m_viewer.wasStopped())
        {
            Sleep(1);
        }

        // stop the grabber
        interface->stop();
    }

     void run ()
     {
       pcl::Grabber* interface = new pcl::OpenNIGrabber();

       boost::function<void (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)> f =
         boost::bind (&SimpleOpenNIViewer::cloud_cb_, this, _1);

       interface->registerCallback (f);

       viewer.registerKeyboardCallback (keyboardEventOccurred, (void*)&viewer);

       interface->start ();

       while (!viewer.wasStopped())
       {
         boost::this_thread::sleep (boost::posix_time::seconds (10));
         if(flag > 0)
         {
         save_cloud();
//         count++;
         }
         if (count >= 2)
        		 {
        	 return;
        		 }
       }

       interface->stop ();
     }

void PbMapVisualizer::Visualize()
{
  cloudViewer.runOnVisualizationThread (boost::bind(&PbMapVisualizer::viz_cb, this, _1), "viz_cb");
  cloudViewer.registerKeyboardCallback ( keyboardEventOccurred );

  while (!cloudViewer.wasStopped() )
    mrpt::system::sleep(10);
}

  void cloud_cb_(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud){		//fonction <> =>classe template 
  
   if(!viewer.wasStopped()){
   
     /*for(int i=0;i<cloud->width;i++){
   for (int j=0;j<cloud->height;j++){ 
     if (i>300 && j>300)
     std::cout <<cloud->width<< cloud->height <<std::endl;
   }}
   */
     nuage=*cloud;
     //viewer.showCloud(nuage);//on montre le viewer tant qu'on ne l'a pas arreté  
   }    
 }

     void cloud_cb_ (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud)
     {
       if (!viewer.wasStopped())
         {
           pcl::PointCloud<pcl::PointXYZRGBA>::Ptr result (new pcl::PointCloud<pcl::PointXYZRGBA>);

           pcl::PassThrough<pcl::PointXYZRGBA> pass;
    	   pass.setFilterFieldName ("z");
    	   pass.setFilterLimits (0.0, 3.0);
    	   pass.setInputCloud (cloud);
    	   pass.filter (*result);
           s_cloud = *result;
           viewer.showCloud (result);
         }
     }

     void cloud_cb_ (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud)
     {
       if (!viewer.wasStopped())
         {
//// Green region detection
//    	   pcl::PointCloud<pcl::PointXYZRGBA>::Ptr final_cloud (new pcl::PointCloud<pcl::PointXYZRGBA>);
//    	   final_cloud->width    = cloud->width;
//    	   final_cloud->height   = cloud->height;
//    	   final_cloud->resize (cloud->width * cloud->height);
//
//    	   size_t i = 0;


    	   viewer.showCloud (cloud);
         }
     }

  void run(){
    
    depth=Mat(480,640,DataType<float>::type);
    
   pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr nuage3(&nuage2);// (new pcl::PointCloud<pcl::PointXYZRGB>);
   pcl::PointXYZRGBA point;
   
   it=1000;
   pcl::OpenNIGrabber* interface =new pcl::OpenNIGrabber();//creation d'un objet interface qui vient de l'include openni_grabber
   //namedWindow( "Display Image", CV_WINDOW_AUTOSIZE );
   namedWindow( "Harris Image", CV_WINDOW_AUTOSIZE );
   //namedWindow( "Depth Image", CV_WINDOW_AUTOSIZE );
  // VideoCapture capture(1);
  // Mat frame;
  // capture >> frame;
  // record=VideoWriter("/home/guerric/Bureau/test.avi", CV_FOURCC('M','J','P','G'), 30, frame.size(), true);
   
   boost::function<void(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)> 
  f = boost::bind (&ImageVIewer::cloud_cb_, this, _1);

  boost::function<void(const boost::shared_ptr<openni_wrapper::Image>&)> 
  g = boost::bind (&ImageVIewer::image_cb_, this, _1);
  
  boost::function<void(const boost::shared_ptr<openni_wrapper::DepthImage>&)> 
  h = boost::bind (&ImageVIewer::depth_cb_, this, _1);


  interface->registerCallback (f);
  interface->registerCallback (g);
  interface->registerCallback (h);
  
   
   interface->start();
   //on reste dans cet état d'acquisition tant qu'on ne stoppe pas dans le viewer

   
   while(!viewer.wasStopped()){
     boost::this_thread::sleep(boost::posix_time::seconds(1));	//met la fonction en attente pendant une seconde <=> sleep(1) mais plus précis pour les multicores     
     viewer.showCloud(nuage3);     
  }
   
  interface->stop();
  record.release();
  destroyAllWindows();  

  }

     void run ()
     {
       pcl::Grabber* interface = new pcl::OpenNIGrabber();

       boost::function<void (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)> f =
         boost::bind (&SimpleOpenNIViewer::cloud_cb_, this, _1);

       interface->registerCallback (f);
       viewer.registerPointPickingCallback (pointpickingEventOccurred, (void*)&viewer);
       interface->start ();

       while (!viewer.wasStopped())
       {
         boost::this_thread::sleep (boost::posix_time::seconds (1));
       }

       interface->stop ();
     }

	void cloud_cb(const boost::shared_ptr<const sensor_msgs::PointCloud2>& rototranslatedpcBoostPtr){
		boost::shared_ptr <pcl::PointCloud <pcl::PointXYZ> > pclCloudBoostPtr (new pcl::PointCloud<pcl::PointXYZ> );


		pcl::fromROSMsg( *rototranslatedpcBoostPtr , *pclCloudBoostPtr ); // ORIG WORKING

		//		// Perform voxel filter
		//		boost::shared_ptr <pcl::PointCloud <pcl::PointXYZ> > filteredCloudBoostPtr (new pcl::PointCloud<pcl::PointXYZ> ); // Uncomment to use filtering
		//		pcl::VoxelGrid<pcl::PointXYZ> sor;
		//		  sor.setInputCloud (pclCloudBoostPtr);
		//		  sor.setLeafSize (0.01f, 0.01f, 0.01f);
		//		  sor.filter (*filteredCloudBoostPtr);
		//
		//		// Prints filtered pointcloud
		//		for (size_t i = 0; i < filteredCloudBoostPtr->points.size (); ++i)
		//			std::cout << filteredCloudBoostPtr->points[i].x
		//			<< "     "<< filteredCloudBoostPtr->points[i].y
		//			<< "     "<< filteredCloudBoostPtr->points[i].z << std::endl;
		//
		//		  std::cout<<filteredCloudBoostPtr->points.size ()<<std::endl;
		// if (!viewer.wasStopped())	viewer.showCloud (filteredCloudBoostPtr, "sample cloud");
		if (!viewer.wasStopped())	viewer.showCloud ( pclCloudBoostPtr, "sample cloud"); // IGNORE ECLIPSE ERROR HERE. COMPILER WORKS.

	}

  void cloud_cb_(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloud){		//fonction <> =>classe template  
 
   if(!viewer.wasStopped()){
     viewer.showCloud(cloud);//on montre le viewer tant qu'on ne l'a pas arreté
   }    
 }

//.........这里部分代码省略.........
 if (pcl::io::loadPCDFile<PoinT> ("Cosyslab-1.pcd", *target_cloud) == -1) //* load the file
  {
    PCL_ERROR ("Couldn't read file target_test_nonoise.pcd \n");
    return (-1);
  }
  cout << "Loaded " << target_cloud->width * target_cloud->height << " data points "<< endl;

  //----------------------------------------------------------------------------------
  //remove NAN points from the cloud
  //----------------------------------------------------------------------------------
  std::vector<int> indices_src, indices_tgt;
  pcl::removeNaNFromPointCloud(*source_cloud,*source_cloud, indices_src);
  pcl::removeNaNFromPointCloud(*target_cloud,*target_cloud, indices_tgt);

  //----------------------------------------------------------------------------------
  //Reduce number of points
  //----------------------------------------------------------------------------------
  pcl::VoxelGrid<PoinT> grid, grid1;
  grid.setLeafSize (0.01, 0.01, 0.05);
  grid.setInputCloud (source_cloud);
  grid.filter(*source_cloud);
  cout << "source cloud number of point after voxelgrid: " << source_cloud->points.size() << endl;
  
  grid1.setLeafSize (0.01, 0.01, 0.05);
  grid1.setInputCloud (target_cloud);
  grid1.filter(*target_cloud);
  cout << "target cloud number of point after voxelgrid: " << target_cloud->points.size() << endl;

  //----------------------------------------------------------------------------------
  //Make source cloud blue
  //----------------------------------------------------------------------------------
  for (int i = 0; i < source_cloud->points.size(); ++i) {
	source_cloud->points[i].r = 0;
	source_cloud->points[i].g = 0;
	source_cloud->points[i].b = 255;
  }
  //----------------------------------------------------------------------------------
  //Make Target cloud Red
  //----------------------------------------------------------------------------------
  for (int i = 0; i < target_cloud->points.size(); ++i) {
	target_cloud->points[i].r = 255;
	target_cloud->points[i].g = 0;
	target_cloud->points[i].b = 0;
  }
	
  //----------------------------------------------------------------------------------
  //Apply PCA transformation from target to source
  //----------------------------------------------------------------------------------
  if(PCAregistration == true){
	Eigen::Vector4f centroid_source, centroid_target;
	Eigen::Matrix4f transformationm_source, transformationm_target, PCAtransformation;

	applyPCAregistration(source_cloud, centroid_source, transformationm_source);
	applyPCAregistration(target_cloud, centroid_target, transformationm_target);
	PCAtransformation = transformationm_source * transformationm_target.transpose();

	//Apply rotation transformation 
	pcl::transformPointCloud(*source_cloud, *transformed_cloud, PCAtransformation);
	
	//calculate fitnesscore
	FitnesscorePCA = calculateFitnesscore(target_cloud,source_cloud, PCAtransformation);
	cout << "FitnesscorePCA is : " << FitnesscorePCA  << " meter "<< endl;
  }

  //----------------------------------------------------------------------------------
  //Apply SVD transformation from target to source
  //----------------------------------------------------------------------------------
  if(SVDregistration == true){	
	Eigen::Matrix4f trans_matrix_svd;
	applySVDregistration(source_cloud, target_cloud, trans_matrix_svd);
	pcl::transformPointCloud (*source_cloud, *transformed_cloud, trans_matrix_svd);

	//calculate fitnesscore
 	FitnesscoreSVD = calculateFitnesscore(target_cloud,source_cloud, trans_matrix_svd);
	cout << "FitnesscoreSVD is : " << FitnesscoreSVD  << " meter "<< endl;
  }
  //----------------------------------------------------------------------------------
  //Make transformed cloud White
  //----------------------------------------------------------------------------------
  for (int i = 0; i < transformed_cloud->points.size(); ++i) {
	transformed_cloud->points[i].r = 255;
	transformed_cloud->points[i].g = 255;
	transformed_cloud->points[i].b = 255;
  }
  	
  //----------------------------------------------------------------------------------
  //Show cloud in viewer
  //----------------------------------------------------------------------------------
   viewer.showCloud (source_cloud, "source");
   viewer.showCloud (transformed_cloud, "transformed");
   viewer.showCloud (target_cloud, "target");

  while (!viewer.wasStopped ())
  {
	//while keypress "q" is pressed
  }
  

  return (0);
}

     void cloud_cb_ (const sensor_msgs::PointCloud2ConstPtr& input)
     {

    	 if (!viewer.wasStopped()){

    		// if(!cloud_received){


    			 // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
    			     pcl::PointCloud<pcl::PointXYZRGB>::Ptr in_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
    			     //pcl::PointCloud<pcl::PointXYZ> cloud;
    			     pcl::fromROSMsg (*input, *in_cloud);
    			     //downsample
    			     pcl::PointCloud<pcl::PointXYZRGB>::Ptr downsampled_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
    			     downSample(in_cloud, downsampled_cloud);
    			     //crop
    			     pcl::PointCloud<pcl::PointXYZRGB>::Ptr cropped_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
    			     pcl::IndicesPtr indices (new std::vector <int>);
    			     cropCloud(downsampled_cloud, cropped_cloud, indices);
    			     //rotate

    			     pcl::PointCloud<pcl::PointXYZRGB>::Ptr rotated_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
    			     rotateCloud(cropped_cloud, rotated_cloud);
    			     pcl::PointCloud<pcl::PointXYZRGB>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
    			     colorSegment(rotated_cloud, filtered_cloud);



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

    		  seg.setInputCloud (msg);
    		  seg.segment (*inliers, *coefficients);

    		  if (inliers->indices.size () == 0)
    		  {
    		    PCL_ERROR ("Could not estimate a planar model for the given dataset.");
    		    return;
    		    //return (-1);
    		  }

    		  std::cerr << "Model coefficients: " << coefficients->values[0] << " "
    		                                      << coefficients->values[1] << " "
    		                                      << coefficients->values[2] << " "
    		                                      << coefficients->values[3] << std::endl;

    		  std::cerr << "Model inliers: " << inliers->indices.size () << std::endl;

    		  cloud.points.resize(inliers->indices.size ());
    		  cloud.width=1;
    		  cloud.height= inliers->indices.size ();
    		  for (size_t i = 0; i < inliers->indices.size (); ++i){
    			  cloud.points[i].x = (*msg).points[inliers->indices[i]].x;
    			  cloud.points[i].y = (*msg).points[inliers->indices[i]].y;
    			  cloud.points[i].z = (*msg).points[inliers->indices[i]].z;

    			  //std::cerr << inliers->indices[i] << "    " << (*msg).points[inliers->indices[i]].x << " "
    		      //                                         << (*msg).points[inliers->indices[i]].y << " "
    		      //                                         << (*msg).points[inliers->indices[i]].z << std::endl;
    		  }
              //cloud_received = true;
    		}
    		*/
    		  //return (0);
    	     cv::waitKey(3);
             viewer.showCloud(filtered_cloud);
             //viewer.showCloud(msg);
    		 }

     }