C++ visualization::CloudViewer类代码示例

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

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

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

  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 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 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 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 show_cloud( pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud)
{
  
  viewer.showCloud(cloud);

}

int main (int argc, char** argv)
{
    // Initialize ROS
    ros::init (argc, argv, "objectMaster");
    ros::NodeHandle n;
    ros::Rate waiting_rate(30);

    //strat a traver and wait for its ready
    cloudTraver ct(n);
    while(!ct.isReady())
    {
        ros::spinOnce();
        waiting_rate.sleep();
    }


    cvNamedWindow("CurrentImage",CV_WINDOW_AUTOSIZE);
    cv::Mat image;
    pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloudP;

    Mat src, dst, color_dst;
    std::string objectNameTemp="TEMP";
    int count=0;
    while(ros::ok())
    {
        while(!ct.isReady())
        {
            ros::spinOnce();
        }

        image=ct.getImage();
        cloudP=ct.getCloud();

        if(cloudP->empty())
        {
            std::cout<<"No pointCloud passed into this process, fuck you no points you play MAOXIAN!"<<std::endl;
            continue;
        }
        pcl::PointCloud<PointType>::Ptr cloud_RGBA(new pcl::PointCloud<PointType>);
        *cloud_RGBA=*cloudP;

        Filter filter;
        cloud_RGBA=filter.PassThrough(cloud_RGBA);
        cloud_RGBA=filter.DeSamping(cloud_RGBA);
        cloud_RGBA=filter.RemovePlane(cloud_RGBA);
        if(cloud_RGBA->empty())
        {
            std::cout<<"No pointCloud left after the samping"<<std::endl;
            continue;
        }

        std::vector<pcl::PointIndices> cluster_indices;
        filter.ExtractionObject(cloud_RGBA,cluster_indices);

        if(cluster_indices.size()!=0)
        {
            std::cout<<cluster_indices.size()<<"clusters extraced"<<std::endl;
            pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZRGBA>);
            pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_filtered (new 	pcl::PointCloud<pcl::PointXYZRGBA>);
            pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud = cloud_RGBA;
            //voxelgrid并不是产生球面空洞的原因
            pcl::VoxelGrid<pcl::PointXYZRGBA> vg;
            vg.setInputCloud (cloud);
            vg.setLeafSize (0.01f, 0.01f, 0.01f);
            vg.filter (*cloud_filtered);
            //Create the segmentation object for the planar model and set all the parameters
            pcl::SACSegmentation<pcl::PointXYZRGBA> seg;
            pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
            pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
            pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZRGBA> ());
            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::PointXYZRGBA> 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
//.........这里部分代码省略.........

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

  //----------------------------------------------------------------------------------
  //Read pcd file
  //---------------------------------------------------------------------------------- 
  if (pcl::io::loadPCDFile<PoinT> ("Cosyslab-0.pcd", *source_cloud) == -1) //* load the file
  {
    PCL_ERROR ("Couldn't read file source_cloud.pcd \n");
    return (-1);
  }
  cout << "Loaded " << source_cloud->width * source_cloud->height << " data points "<< endl;

 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
//.........这里部分代码省略.........