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 ();
     }
Exemplo n.º 2
0
void PbMapVisualizer::Visualize()
{
  cloudViewer.runOnVisualizationThread (boost::bind(&PbMapVisualizer::viz_cb, this, _1), "viz_cb");
  cloudViewer.registerKeyboardCallback ( keyboardEventOccurred );

  while (!cloudViewer.wasStopped() )
    mrpt::system::sleep(10);
}
Exemplo n.º 3
0
     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);
         }
     }
Exemplo n.º 5
0
    /***********************************************************************************************************************
     * @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();
    }
Exemplo n.º 6
0
    /***********************************************************************************************************************
     * @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++;
        }
    }
Exemplo n.º 7
0
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);
}
Exemplo n.º 8
0
  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 ();
     }
Exemplo n.º 10
0
  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é  
   }    
 }
Exemplo n.º 11
0
	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.

	}
Exemplo n.º 12
0
//描画
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);
}
Exemplo n.º 13
0
void show_cloud( pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud)
{
  
  viewer.showCloud(cloud);

}
Exemplo n.º 14
0
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, f**k 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
                extract.setNegative (true);
                extract.filter (*cloud_f);
                *cloud_filtered = *cloud_f;
            }


            //cloud filter !
            pcl::search::KdTree<pcl::PointXYZRGBA>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBA>);
            if ( !cloud_filtered->empty())tree->setInputCloud (cloud_filtered);

            std::vector<pcl::PointIndices> cluster_indices;
            pcl::EuclideanClusterExtraction<pcl::PointXYZRGBA> ec;
            ec.setClusterTolerance (0.04); // 4cm
            ec.setMinClusterSize (200);
            ec.setMaxClusterSize (1000);
            ec.setSearchMethod (tree);
            ec.setInputCloud (cloud_filtered);
            ec.extract (cluster_indices);
            int j = 0;
            int cnt = 0,inlaw = 0;

            //start detect
            for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
            {
                pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZRGBA>);
                for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
                    cloud_cluster->points.push_back (cloud_filtered->points[*pit]); //*
                cloud_cluster->width = cloud_cluster->points.size ();
                cloud_cluster->height = 1;
                cloud_cluster->is_dense = true;

                //std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size () << " data points." << std::endl;
                //std::stringstream ss;
                //ss << "cloud_cluster_" << j << ".pcd";
                //writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false); //*
                j++;
                cout <<"......................................................."<<endl;
                cout << "cloud_cluster_" << j << endl;
                if ( match(cloud_cluster))
                {
                    cout << ++cnt << " sphere have been detected " <<endl;
                    unsigned int red,green,blue;
                    for (int i=0,red = 0,green =0,blue = 0; i < cloud_cluster->points.size(); i++)
                    {
                        red =cloud_cluster->points[i].r;
                        green =cloud_cluster->points[i].g;
                        blue =cloud_cluster->points[i].b;
                        if(getDistance(red,green,blue))inlaw++;



                    }
                    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);
                int mid_value = x_position[2];//每5帧的中位数
                cout << "mid_value:" << mid_value << endl;
                cout << "............................." << endl;
                double r = (*itc)[2];
                //double d = 640 * 230/(2 * r) - 271;
                int l = (mid_value - 320);
                double theta = (double)52 / 640 * l;
                cout << "theta:" << theta << endl;
                if (mid_value > 410)
                {
                    cout << "need to turn right" << endl;
                    direction = 1;
                }
                else if (mid_value < 230)
                {
                    cout << "need to turn left" << endl;
                    direction = -1;
                }
                else
                {
                    cout << "succeed" << endl;
                    direction = 0;
                }

                count = 0;
                mid_value = 0;
            }
            ++itc;
        }









        // viewerS.showCloud(cloudP);
        cv::imshow("CurrentImage", image);
        //U must wait a second to let the image show on the screen
        char temp=cvWaitKey(0);
        if (temp ==27)break;
        //wait for new datas
    }
}
Exemplo n.º 15
0
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
  //----------------------------------------------------------------------------------
   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);
}
Exemplo n.º 16
0
  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é
   }    
 }
Exemplo n.º 17
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);
    		 }

     }