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

void pointsCb(const PointCloud::ConstPtr& Callback_Points)
{
    PointCloud::Ptr Points (new PointCloud);
    *Points = *Callback_Points;
 
    if(image == NULL)
        return;
    //添加rgb信息
    cout<<"Points->width = "<<Points->width<<"  Points->height = "<<Points->height<<"\n";
    for (int j = 0; j < Points->width; j++)
    {
        for (int i = 0; i < Points->height; i++)
        { 
            //float x = (float)(*input_cloud)(j,i).x;
            //float y = (float)(*input_cloud)(j,i).y;
            //float z = (float)(*input_cloud)(j,i).z;
            int b = (int)CV_IMAGE_ELEM(image, uchar, i, j*3+0);
            int g = (int)CV_IMAGE_ELEM(image, uchar, i, j*3+1);
            int r = (int)CV_IMAGE_ELEM(image, uchar, i, j*3+2);
            
            (*Points)(j,i).r = r;
            (*Points)(j,i).g = g;
            (*Points)(j,i).b = b;
        }
    }
    viewer.showCloud(Points);
}

    /***********************************************************************************************************************
     * @BRIEF Callback function for received cloud data
     * @PARAM[in] cloudIn the raw cloud data received by the OpenNI2 device
     * @AUTHOR Christopher D. McMurrough
     **********************************************************************************************************************/
    void cloudCallback(const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr &cloudIn)
    {
        // get the elapsed time since the last callback
        double elapsedTime = m_stopWatch.getTimeSeconds();
        m_stopWatch.reset();
        std::printf("Seconds elapsed since last cloud callback: %f \n", elapsedTime);
        
        // store the cloud save count
        static int saveCount = 0;

        // render cloud if necessary
        if(m_cloudRenderSetting)
        {
            m_viewer.showCloud(cloudIn);
        }

        // save the cloud if necessary
        if(m_cloudSaveSetting)
        {
            std::stringstream ss;
            string str;
            ss << saveCount << ".pcd";
            str = ss.str();
            pcl::io::savePCDFile<pcl::PointXYZRGBA> (str.c_str(), *cloudIn, true);
            std::printf("cloud saved to %s\n", str.c_str());
            saveCount++;
        }
    }

     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 display() {
    // clear screen and depth buffer
    glClear ( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT );
    // Reset the coordinate system before modifying
    glLoadIdentity();
    glEnable(GL_DEPTH_TEST); //「Zバッファ」を有効
    gluLookAt(0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 1.0, 0.0);   //視点の向き設定

    if(GetImage(image,m_hNextImageFrameEvent,m_pImageStreamHandle)==-1)
        return;
    if(GetImage(depth,m_hNextDepthFrameEvent,m_pDepthStreamHandle)==-1)
        return;

    //3次元ポイントクラウドのための座標変換
    //retrievePointCloudMap(depth,pointCloud_XYZ);
    retrieveRGBCloudMap(depth,pointCloud_XYZforRGB);

    //視点の変更
    polarview();

    imshow("image",image);
    imshow("depth",depth);

    //RGBAからBGRAに変換
    cvtColor(image,image,CV_RGBA2BGRA);

    //ポイントクラウド
    //drawPointCloud(image,pointCloud_XYZ,depth);
    drawPointCloud_easy(image,pointCloud_XYZforRGB);


    glFlush();
    glutSwapBuffers();

    //Draw on PCL viewer

    Mat2PCL_XYZRGB_ALL(image,pointCloud_XYZforRGB,*cloud);
    PCLviewer.showCloud(cloud);
}

	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.

	}

//.........这里部分代码省略.........
                    unsigned int tot = cloud_cluster->points.size();
                    red /=tot;
                    blue /=tot;
                    green /=tot;

                    /// compared with standard red !

                    int dis = 0;
                    dis = (red-255)*(red-255)+(blue-0)*(blue-0)+(green-0)*(green-0);

                    if(inlaw > cloud_cluster->points.size()*0.7)
                    {
                        cout <<"This is red ball ! "<<endl;
                    }
                    else
                    {
                        cout <<"Sorry it's not red ball ...."<<endl;
                        cout <<"#Average -> R :"<<red<<" G: "<<green<<" B: "<<blue<<endl;
                    }
                    cout <<"------------------------------------------------------"<<endl;

                    //viewer.showCloud(cloud_cluster);


                }





            }
        }

        viewer.showCloud(cloud_RGBA);



        // --------OpenCV----------

         Mat src, dst, color_dst;



         cvtColor(image, image, CV_BGR2GRAY);

        //sharpenImage1(image, image);

        GaussianBlur(image, image, Size(5, 5), 1.5);

        //sharpenImage1(image, image);
        vector<Vec3f> circles;
        HoughCircles(image, circles, CV_HOUGH_GRADIENT,2,120,220,70,25, 300);

        vector<Vec3f>::iterator itc = circles.begin();
        while(itc != circles.end())
        {
            cout << "x:" << (*itc)[0] << "y:" << (*itc)[1] << endl;
            circle(image,Point((*itc)[0], (*itc)[1]),(*itc)[2],Scalar(255),2);
                           //颜色//厚度

            x_position[count] = (*itc)[0];
            //cout << "x_position" << count << ":" << x_position << endl;
            count++;
            if (count == 5)
            {
                sort(x_position, x_position + 5);

void show_cloud( pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud)
{
  
  viewer.showCloud(cloud);

}

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

     }