コード例 #1
0
void writePCD(std::string path, PCLPointCloud::Ptr cloud)
{
	cloud->width = 1;
	cloud->height = cloud->points.size();
	static pcl::PCDWriter writer;
	if (not cloud->empty()) writer.writeASCII(path, *cloud);
}
コード例 #2
0
ファイル: openni_klt.cpp プロジェクト: BITVoyager/pcl
 void
 keyboard_callback (const pcl::visualization::KeyboardEvent& event, void*)
 {
   static pcl::PCDWriter writer;
   static std::ostringstream frame;
   if (event.keyUp ())
   {
     if ((event.getKeyCode () == 's') || (event.getKeyCode () == 'S'))
     {
       boost::mutex::scoped_lock lock (cloud_mutex_);
       frame.str ("frame-");
       frame << boost::posix_time::to_iso_string (boost::posix_time::microsec_clock::local_time ()) << ".pcd";
       writer.writeBinaryCompressed (frame.str (), *cloud_);
       PCL_INFO ("Written cloud %s.\n", frame.str ().c_str ());
     }
   }
 }
コード例 #3
0
ファイル: pcMerger2.cpp プロジェクト: aa755/scene_labelling
/* ---[ */
int
main(int argc, char** argv) {
       fn = "transformed_"  + std::string(argv[1])+".pcd";

  ros::init(argc, argv, "pcmerger");
  char *topic="/camera/rgb/points";
  if(argc>2)
    topic=argv[2];

  if (!reader.open (argv[1], topic))
  {
    cout <<"Couldn't read bag file on topic" <<(topic);
    return (-1);
  }
//  skipNum = atoi(argv[2]);


   viewer.createViewPort(0.0, 0.0, 1.0, 1.0, viewportOrig);
   viewer.addCoordinateSystem (1);

    //viewer.createViewPort(0.5, 0.0, 1.0, 1.0, viewportPred);
    //for (int i = 1 ; i <= max_segment_num ; i++ ){
    srv = new dynamic_reconfigure::Server < scene_processing::pcmergerConfig > (global_mutex);
    dynamic_reconfigure::Server < scene_processing::pcmergerConfig >::CallbackType f = boost::bind(&reconfig, _1, _2);


    srv->setCallback(f);
    conf.done = false;
    while (true) {
        viewer.spinOnce();
        ros::spinOnce();
        if (conf.done) {
            conf.done = false;
            srv->updateConfig(conf);
            //savePCDAndLabels ();
            break;
        }
        if (doUpdate) {
            doUpdate = false;
            srv->updateConfig(conf);
        }
    }
    pcl::PCDWriter writer;
      writer.write ( fn,*cloud_merged_ptr, true);
    transformFile.close();

    

   // viewer.removePointCloud("pred");


    return (0);
}
コード例 #4
0
  ////////////////////////////////////////////////////////////////////////////////
  // cloud_cb (!)
  void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& pc)
  {
    bool found_transform = tf_.waitForTransform(pc->header.frame_id, to_frame_,
                                                pc->header.stamp, ros::Duration(10.0));
    if (found_transform)
    {
      //ROS_ASSERT_MSG(found_transform, "Could not transform to camera frame");
      tf::StampedTransform transform;
      tf_.lookupTransform(to_frame_,pc->header.frame_id, pc->header.stamp, transform);
      pcl_ros::transformPointCloud(to_frame_, transform, *pc, output_cloud_);
      ROS_DEBUG("[TransformPointcloudNode:] Point cloud published in frame %s", output_cloud_.header.frame_id.c_str());
      pub_.publish (output_cloud_);
      if (save_point_cloud_)
	{
	  pcl::PointCloud<pcl::PointXYZ> pcl_cloud;
	  pcl::fromROSMsg(output_cloud_, pcl_cloud);
	  pcd_writer_.write (point_cloud_name_ + ".pcd", pcl_cloud);
      ROS_INFO("Point cloud saved as %s.pcd", point_cloud_name_.c_str());
	}
    }
  }
コード例 #5
0
ファイル: pcMerger2.cpp プロジェクト: aa755/scene_labelling
void reconfig(scene_processing::pcmergerConfig & config, uint32_t level) {
    conf = config;
    boost::recursive_mutex::scoped_lock lock(global_mutex);
 //   pcl::PointCloud<PointT>::Ptr prev_cloud_ptr(new pcl::PointCloud<PointT > ());
  //  pcl::PointCloud<PointT>::Ptr new_cloud_ptr(new pcl::PointCloud<PointT > ());


    if(conf.add_pc) {
       //conf.add_pc = false;
        doUpdate = true;
        *cloud_new_ptr = *cloud_mod_ptr;
        *cloud_merged_backup_ptr=*cloud_merged_ptr;
        if(Merged)
        {
            *cloud_merged_ptr += *cloud_new_ptr;
            Matrix4f globalTrans=computeTransformXYZYPR(config.x, config.y, config.z, config.yaw/180.0*PI, config.pitch/180.0*PI, config.roll/180.0*PI);
            writeMatrixToFile(globalTrans);

        }
        else
            *cloud_merged_ptr = *cloud_new_ptr;
        //*cloud_merged_ptr =*cloud_new_ptr;
        
        viewer.removePointCloud("new");
        if(Merged)
            viewer.removePointCloud("merged");
        Merged = true;
        pcl::toROSMsg(*cloud_merged_ptr, cloud_blobc_merged);
        color_handler_merged.reset(new pcl_visualization::PointCloudColorHandlerRGBField<sensor_msgs::PointCloud2 > (cloud_blobc_merged));
        viewer.addPointCloud(*cloud_merged_ptr, color_handler_merged, "merged", viewportOrig);
        ROS_INFO("displaying mergered pointcloud");

    }

    if(conf.setIT) {

        std::string transformsFileName=fn+".transforms.txt";
            transformFile.open(transformsFileName.data());

conf.setIT = false;
        doUpdate = true;
        InitialTransformConfig = conf;
        
            Matrix4f globalTrans=computeTransformXYZYPR(InitialTransformConfig.x, InitialTransformConfig.y, InitialTransformConfig.z, InitialTransformConfig.yaw/180.0*PI, InitialTransformConfig.pitch/180.0*PI, InitialTransformConfig.roll/180.0*PI);
            writeMatrixToFile(globalTrans);

        *cloud_new_ptr = *cloud_mod_ptr;
        ITpresent = true;
    }
    if(config.undo)
    {
        if(noMoreUndo)
        {
            conf.undo=false;
            doUpdate=true;
            return;
        }

        noMoreUndo=true;
        transformFile<<"endo"<<endl;
        *cloud_merged_ptr=*cloud_merged_backup_ptr;
        if(Merged)
            viewer.removePointCloud("merged");
        Merged = true;
        pcl::toROSMsg(*cloud_merged_ptr, cloud_blobc_merged);
        color_handler_merged.reset(new pcl_visualization::PointCloudColorHandlerRGBField<sensor_msgs::PointCloud2 > (cloud_blobc_merged));
        viewer.addPointCloud(*cloud_merged_ptr, color_handler_merged, "merged", viewportOrig);
        ROS_INFO("undo:displaying mergered pointcloud");
        conf.undo=false;
        doUpdate=true;
  if (pcl::io::loadPCDFile (std::string("tempAppend.pcd"), cloud_blobc_new) == -1)
  {
    ROS_ERROR ("Couldn't read file ");
    return ;
  }
//  ROS_INFO ("Loaded %d data points from %s with the following fields: %s", (int)(cloud_blob.width * cloud_blob.height), argv[1] ,pcl::getFieldsList (cloud_blob).c_str ());

  // Convert to the templated message type
   pcl::fromROSMsg (cloud_blobc_new, *cloud_new_ptr);
//   pcl::PointCloud<PointT>::Ptr cloud_ptr (new pcl::PointCloud<PointT> (cloud));

            if(ITpresent){
                cout<<"inside IT"<<endl;
                transformXYZYPR<PointT>(*cloud_new_ptr, *cloud_mod_ptr, InitialTransformConfig.x, InitialTransformConfig.y, InitialTransformConfig.z, InitialTransformConfig.yaw/180.0*PI, InitialTransformConfig.pitch/180.0*PI, InitialTransformConfig.roll/180.0*PI);
                *cloud_new_ptr = *cloud_mod_ptr;
                conf.pitch=0;
                conf.yaw=0;
                conf.roll=0;
            }
            //ROS_INFO("PointCloud with %d data points and frame %s (%f) received.", (int) cloud_new_ptr->points.size(), cloud_new_ptr->header.frame_id.c_str(), cloud_new_ptr->header.stamp.toSec());
            viewer.removePointCloud("new");
           // pcl::toROSMsg<PointT>(*cloud_new_ptr, cloud_blobc_new);
            color_handler_new.reset(new pcl_visualization::PointCloudColorHandlerRGBField<sensor_msgs::PointCloud2 > (cloud_blobc_new));
            viewer.addPointCloud(*cloud_new_ptr, color_handler_new, "new", viewportOrig);
            ROS_INFO("undo:displaying new point cloud");
            conf.x=0;
            conf.y=0;
            conf.z=0;
            conf.yaw=0;
            conf.pitch=0;
            conf.roll=0;

        
    }
    if(conf.skip_pc || conf.add_pc)
    {
        if (conf.skip_pc)
        {
            conf.x = -1;
            conf.y = -0.3;
            conf.z = 0.78;
            conf.yaw = 0;
            conf.pitch = 30;
            conf.roll = 0;
        }
        else
        {
            conf.x = -0.1;
            conf.y = -0.8;
            conf.z = 0.019;
            conf.yaw = 175;
            conf.pitch = 0;
            conf.roll = 0;
        }

        noMoreUndo=false;
        conf.skip_pc = false;
        conf.add_pc =false;
        
        doUpdate = true;
        int count = 0;
        cloud_blob_prev = cloud_blob_new;
        cloud_blob_new = reader.getNextCloud();
        cout<<"header"<<cloud_blob_new->header<<endl;
//        cloud_blob_new->
        ros::M_string::iterator iter;
        //for(iter=cloud_blob_new->__connection_header->begin ();iter!=cloud_blob_new->__connection_header->end ();iter++)
         // cout<<iter->first<<","<<iter->second<<endl;
        
        
        while(count < skipNum && cloud_blob_prev != cloud_blob_new)
        {
            cloud_blob_prev = cloud_blob_new;
            cloud_blob_new = reader.getNextCloud();
            count ++;
        }
        if (cloud_blob_prev != cloud_blob_new) {
            pcl::fromROSMsg(*cloud_blob_new, *cloud_temp_ptr);
            cloud_temp_ptr->header;
            cout<<"numPoints="<<cloud_temp_ptr->size ()<<endl;
            appendCamIndexAndDistance (cloud_temp_ptr,cloud_new_ptr,globalFrameCount,VectorG(0,0,0));
            globalFrameCount++;
            cloud_new_ptr->width=1;
            cloud_new_ptr->height=cloud_new_ptr->size();
            writer.write (std::string("tempAppend.pcd"),*cloud_new_ptr,true);
  if (pcl::io::loadPCDFile (std::string("tempAppend.pcd"), cloud_blobc_new) == -1)
  {
    ROS_ERROR ("Couldn't read temp file ");
    return ;
  }
//  ROS_INFO ("Loaded %d data points from %s with the following fields: %s", (int)(cloud_blob.width * cloud_blob.height), argv[1] ,pcl::getFieldsList (cloud_blob).c_str ());

  // Convert to the templated message type
   pcl::fromROSMsg (cloud_blobc_new, *cloud_new_ptr);
//   pcl::PointCloud<PointT>::Ptr cloud_ptr (new pcl::PointCloud<PointT> (cloud));
            
            if(ITpresent){
                cout<<"inside IT"<<endl;
                transformXYZYPR<PointT>(*cloud_new_ptr, *cloud_mod_ptr, InitialTransformConfig.x, InitialTransformConfig.y, InitialTransformConfig.z, InitialTransformConfig.yaw/180.0*PI, InitialTransformConfig.pitch/180.0*PI, InitialTransformConfig.roll/180.0*PI);
                *cloud_new_ptr = *cloud_mod_ptr;
             //   conf.pitch=0;
             //   conf.yaw=0;
             //   conf.roll=0;
            }
            //ROS_INFO("PointCloud with %d data points and frame %s (%f) received.", (int) cloud_new_ptr->points.size(), cloud_new_ptr->header.frame_id.c_str(), cloud_new_ptr->header.stamp.toSec());
            viewer.removePointCloud("new");
           // pcl::toROSMsg<PointT>(*cloud_new_ptr, cloud_blobc_new);
            color_handler_new.reset(new pcl_visualization::PointCloudColorHandlerRGBField<sensor_msgs::PointCloud2 > (cloud_blobc_new));
            viewer.addPointCloud(*cloud_new_ptr, color_handler_new, "new", viewportOrig);
            ROS_INFO("displaying new point cloud");
          /*  if(Merged){
                viewer.removePointCloud("merged");
                pcl::toROSMsg(*cloud_merged_ptr, cloud_blobc_merged);
                color_handler_merged.reset(new pcl_visualization::PointCloudColorHandlerRGBField<sensor_msgs::PointCloud2 > (cloud_blobc_merged));
                viewer.addPointCloud(*cloud_merged_ptr, color_handler_merged, "merged", viewportOrig);
            }*/
        }else {
            ROS_INFO("Finised reading all pointclouds! Select done to save.");
        }
    }
    
    if(conf.set_skip){
        conf.set_skip = false;
        doUpdate = true;
        skipNum = (conf.skipNum);
    }
    if(conf.update)
    {
        conf.update = false;
        doUpdate = true;
        updateUI();
    }
}