コード例 #1
0
void initViewer(pcl::visualization::PCLVisualizer &viewer) {
	viewer.setBackgroundColor(0, 0, 0);
	viewer.addCoordinateSystem(1.0, "reference");
	viewer.initCameraParameters();
	viewer.setRepresentationToPointsForAllActors();
	viewer.setCameraPosition(0, 0, -1, 0, 0, 0, 0, -1, 0);
	viewer.registerKeyboardCallback(keyboardCallback);
}
コード例 #2
0
ファイル: main.cpp プロジェクト: vaheta/face_recognition
void setViewerPose (pcl::visualization::PCLVisualizer& viewer, const Eigen::Affine3f& viewer_pose) {
  Eigen::Vector3f pos_vector = viewer_pose * Eigen::Vector3f(10, 10, 10);
  Eigen::Vector3f look_at_vector = viewer_pose.rotation () * Eigen::Vector3f(0, 0, 0);
  Eigen::Vector3f up_vector = viewer_pose.rotation () * Eigen::Vector3f(0, -1, 0);
  viewer.setCameraPosition (pos_vector[0], pos_vector[1], pos_vector[2],
                            look_at_vector[0], look_at_vector[1], look_at_vector[2],
                            up_vector[0], up_vector[1], up_vector[2]);
}
コード例 #3
0
int
  main (int argc, char** argv)
{
  //pcl::PointCloud<pcl::PointXYZ> cloud;

  // Read Kinect live stream:
  PointCloudT cloud_obj;
  PointCloudT::Ptr cloud (new PointCloudT);
  bool new_cloud_available_flag = false;
  pcl::Grabber* interface = new pcl::OpenNIGrabber();
  boost::function<void (const PointCloudT::ConstPtr&)> f =
        boost::bind (&cloud_cb_, _1, &cloud_obj, &new_cloud_available_flag);
  interface->registerCallback (f);
  interface->start ();

  // Wait for the first frame:
  while(!new_cloud_available_flag) 
    boost::this_thread::sleep(boost::posix_time::milliseconds(1));
  pcl::copyPointCloud<PointT, PointT>(cloud_obj, *cloud);
  new_cloud_available_flag = false;

  // Display pointcloud:
  pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud);
  viewer.addPointCloud<PointT> (cloud, rgb, "input_cloud");
  viewer.setCameraPosition(0,0,-2,0,-1,0,0);

  /*
  // Add point picking callback to viewer:
  struct callback_args cb_args;
  PointCloudT::Ptr clicked_points_3d (new PointCloudT);
  cb_args.clicked_points_3d = clicked_points_3d;
  cb_args.viewerPtr = pcl::visualization::PCLVisualizer::Ptr(&viewer);
  viewer.registerPointPickingCallback (pp_callback, (void*)&cb_args);
  std::cout << "Shift+click on three floor points, then press 'Q'..." << std::endl;
  */

  // Spin until 'Q' is pressed (to allow ground manual initialization):
  viewer.spin();
  std::cout << "done." << std::endl;

  pcl::io::savePCDFileASCII ("/home/igor/pcds/pcd_grabber_out_1.pcd", *cloud);
  std::cerr << "Saved " << (*cloud).points.size () << " data points to pcd_grabber_out_1.pcd." << std::endl;

  /*
  for (size_t i = 0; i < cloud.points.size (); ++i)
    std::cerr << "    " << cloud.points[i].x << " " << cloud.points[i].y << " " << cloud.points[i].z << std::endl;
  */

  while (!viewer.wasStopped ())
  {
    boost::this_thread::sleep (boost::posix_time::microseconds (100));
  }

  return (0);
}
コード例 #4
0
int main (int argc, char** argv)
{
  if(pcl::console::find_switch (argc, argv, "--help") || pcl::console::find_switch (argc, argv, "-h"))
        return print_help();

  // Algorithm parameters:
  std::string svm_filename = "../../people/data/trainedLinearSVMForPeopleDetectionWithHOG.yaml";
  float min_confidence = -1.5;
  float min_height = 1.3;
  float max_height = 2.3;
  float voxel_size = 0.06;
  Eigen::Matrix3f rgb_intrinsics_matrix;
  rgb_intrinsics_matrix << 525, 0.0, 319.5, 0.0, 525, 239.5, 0.0, 0.0, 1.0; // Kinect RGB camera intrinsics

  // Read if some parameters are passed from command line:
  pcl::console::parse_argument (argc, argv, "--svm", svm_filename);
  pcl::console::parse_argument (argc, argv, "--conf", min_confidence);
  pcl::console::parse_argument (argc, argv, "--min_h", min_height);
  pcl::console::parse_argument (argc, argv, "--max_h", max_height);

  // Read Kinect live stream:
  PointCloudT::Ptr cloud (new PointCloudT);
  bool new_cloud_available_flag = false;
  pcl::Grabber* interface = new pcl::OpenNIGrabber();
  boost::function<void (const pcl::PointCloud<pcl::PointXYZRGBA>::ConstPtr&)> f =
      boost::bind (&cloud_cb_, _1, cloud, &new_cloud_available_flag);
  interface->registerCallback (f);
  interface->start ();

  // Wait for the first frame:
  while(!new_cloud_available_flag) 
    boost::this_thread::sleep(boost::posix_time::milliseconds(1));
  new_cloud_available_flag = false;

  cloud_mutex.lock ();    // for not overwriting the point cloud

  // Display pointcloud:
  pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud);
  viewer.addPointCloud<PointT> (cloud, rgb, "input_cloud");
  viewer.setCameraPosition(0,0,-2,0,-1,0,0);

  // Add point picking callback to viewer:
  struct callback_args cb_args;
  PointCloudT::Ptr clicked_points_3d (new PointCloudT);
  cb_args.clicked_points_3d = clicked_points_3d;
  cb_args.viewerPtr = pcl::visualization::PCLVisualizer::Ptr(&viewer);
  viewer.registerPointPickingCallback (pp_callback, (void*)&cb_args);
  std::cout << "Shift+click on three floor points, then press 'Q'..." << std::endl;

  // Spin until 'Q' is pressed:
  viewer.spin();
  std::cout << "done." << std::endl;
  
  cloud_mutex.unlock ();    

  // Ground plane estimation:
  Eigen::VectorXf ground_coeffs;
  ground_coeffs.resize(4);
  std::vector<int> clicked_points_indices;
  for (unsigned int i = 0; i < clicked_points_3d->points.size(); i++)
    clicked_points_indices.push_back(i);
  pcl::SampleConsensusModelPlane<PointT> model_plane(clicked_points_3d);
  model_plane.computeModelCoefficients(clicked_points_indices,ground_coeffs);
  std::cout << "Ground plane: " << ground_coeffs(0) << " " << ground_coeffs(1) << " " << ground_coeffs(2) << " " << ground_coeffs(3) << std::endl;

  // Initialize new viewer:
  pcl::visualization::PCLVisualizer viewer("PCL Viewer");          // viewer initialization
  viewer.setCameraPosition(0,0,-2,0,-1,0,0);

  // Create classifier for people detection:  
  pcl::people::PersonClassifier<pcl::RGB> person_classifier;
  person_classifier.loadSVMFromFile(svm_filename);   // load trained SVM

  // People detection app initialization:
  pcl::people::GroundBasedPeopleDetectionApp<PointT> people_detector;    // people detection object
  people_detector.setVoxelSize(voxel_size);                        // set the voxel size
  people_detector.setIntrinsics(rgb_intrinsics_matrix);            // set RGB camera intrinsic parameters
  people_detector.setClassifier(person_classifier);                // set person classifier
  people_detector.setHeightLimits(min_height, max_height);         // set person classifier
//  people_detector.setSensorPortraitOrientation(true);             // set sensor orientation to vertical

  // For timing:
  static unsigned count = 0;
  static double last = pcl::getTime ();

  // Main loop:
  while (!viewer.wasStopped())
  {
    if (new_cloud_available_flag && cloud_mutex.try_lock ())    // if a new cloud is available
    {
      new_cloud_available_flag = false;

      // Perform people detection on the new cloud:
      std::vector<pcl::people::PersonCluster<PointT> > clusters;   // vector containing persons clusters
      people_detector.setInputCloud(cloud);
      people_detector.setGround(ground_coeffs);                    // set floor coefficients
      people_detector.compute(clusters);                           // perform people detection

      ground_coeffs = people_detector.getGround();                 // get updated floor coefficients

      // Draw cloud and people bounding boxes in the viewer:
      viewer.removeAllPointClouds();
      viewer.removeAllShapes();
      pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud);
      viewer.addPointCloud<PointT> (cloud, rgb, "input_cloud");
      unsigned int k = 0;
      for(std::vector<pcl::people::PersonCluster<PointT> >::iterator it = clusters.begin(); it != clusters.end(); ++it)
      {
        if(it->getPersonConfidence() > min_confidence)             // draw only people with confidence above a threshold
        {
          // draw theoretical person bounding box in the PCL viewer:
          it->drawTBoundingBox(viewer, k);
          k++;
        }
      }
      std::cout << k << " people found" << std::endl;
      viewer.spinOnce();

      // Display average framerate:
      if (++count == 30)
      {
        double now = pcl::getTime ();
        std::cout << "Average framerate: " << double(count)/double(now - last) << " Hz" <<  std::endl;
        count = 0;
        last = now;
      }
      cloud_mutex.unlock ();
    }
  }

  return 0;
}
コード例 #5
0
int main (int argc, char** argv)
{

  //ROS Initialization
  ros::init(argc, argv, "detecting_people");
  ros::NodeHandle nh;
  ros::Rate rate(13);

  ros::Subscriber state_sub = nh.subscribe("followme_state", 5, &stateCallback);
  ros::Publisher people_pub = nh.advertise<frmsg::people>("followme_people", 5);
  frmsg::people pub_people_;

  CloudConverter* cc_ = new CloudConverter();

  while (!cc_->ready_xyzrgb_)
  {
    ros::spinOnce();
    rate.sleep();
    if (!ros::ok())
    {
      printf("Terminated by Control-c.\n");
      return -1;
    }
  }

  // Input parameter from the .yaml
  std::string package_path_ = ros::package::getPath("detecting_people") + "/";
  cv::FileStorage* fs_ = new cv::FileStorage(package_path_ + "parameters.yml", cv::FileStorage::READ);

  // Algorithm parameters:
  std::string svm_filename = package_path_ + "trainedLinearSVMForPeopleDetectionWithHOG.yaml";
  std::cout << svm_filename << std::endl;

  float min_confidence = -1.5;
  float min_height = 1.3;
  float max_height = 2.3;
  float voxel_size = 0.06;
  Eigen::Matrix3f rgb_intrinsics_matrix;
  rgb_intrinsics_matrix << 525, 0.0, 319.5, 0.0, 525, 239.5, 0.0, 0.0, 1.0; // Kinect RGB camera intrinsics
  
  // Read if some parameters are passed from command line:
  pcl::console::parse_argument (argc, argv, "--svm", svm_filename);
  pcl::console::parse_argument (argc, argv, "--conf", min_confidence);
  pcl::console::parse_argument (argc, argv, "--min_h", min_height);
  pcl::console::parse_argument (argc, argv, "--max_h", max_height);


  // Read Kinect live stream:
  PointCloudT::Ptr cloud_people (new PointCloudT);
  cc_->ready_xyzrgb_ = false;
  while ( !cc_->ready_xyzrgb_ )
  {
    ros::spinOnce();
    rate.sleep();
  }
  pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr cloud = cc_->msg_xyzrgb_;

  // Display pointcloud:
  pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud);
  viewer.addPointCloud<PointT> (cloud, rgb, "input_cloud");
  viewer.setCameraPosition(0,0,-2,0,-1,0,0);

  // Add point picking callback to viewer:
  struct callback_args cb_args;
  PointCloudT::Ptr clicked_points_3d (new PointCloudT);
  cb_args.clicked_points_3d = clicked_points_3d;
  cb_args.viewerPtr = pcl::visualization::PCLVisualizer::Ptr(&viewer);
  viewer.registerPointPickingCallback (pp_callback, (void*)&cb_args);
  std::cout << "Shift+click on three floor points, then press 'Q'..." << std::endl;

  // Spin until 'Q' is pressed:
  viewer.spin();
  std::cout << "done." << std::endl;
  
  //cloud_mutex.unlock ();    

  // Ground plane estimation:
  Eigen::VectorXf ground_coeffs;
  ground_coeffs.resize(4);
  std::vector<int> clicked_points_indices;
  for (unsigned int i = 0; i < clicked_points_3d->points.size(); i++)
    clicked_points_indices.push_back(i);
  pcl::SampleConsensusModelPlane<PointT> model_plane(clicked_points_3d);
  model_plane.computeModelCoefficients(clicked_points_indices,ground_coeffs);
  std::cout << "Ground plane: " << ground_coeffs(0) << " " << ground_coeffs(1) << " " << ground_coeffs(2) << " " << ground_coeffs(3) << std::endl;

  // Initialize new viewer:
  pcl::visualization::PCLVisualizer viewer("PCL Viewer");          // viewer initialization
  viewer.setCameraPosition(0,0,-2,0,-1,0,0);

  // Create classifier for people detection:  
  pcl::people::PersonClassifier<pcl::RGB> person_classifier;
  person_classifier.loadSVMFromFile(svm_filename);   // load trained SVM

  // People detection app initialization:
  pcl::people::GroundBasedPeopleDetectionApp<PointT> people_detector;    // people detection object
  people_detector.setVoxelSize(voxel_size);                        // set the voxel size
  people_detector.setIntrinsics(rgb_intrinsics_matrix);            // set RGB camera intrinsic parameters
  people_detector.setClassifier(person_classifier);                // set person classifier
  people_detector.setHeightLimits(min_height, max_height);         // set person classifier
//  people_detector.setSensorPortraitOrientation(true);             // set sensor orientation to vertical

  // For timing:
  static unsigned count = 0;
  static double last = pcl::getTime ();
  
  int people_count = 0;
  histogram* first_hist;

  int max_people_num = (int)fs_->getFirstTopLevelNode()["max_people_num"];
  // Main loop:
  while (!viewer.wasStopped() && ros::ok() )
  {
    if ( current_state == 1 )
    {
      if ( cc_->ready_xyzrgb_ )    // if a new cloud is available
      {
    //    std::cout << "In state 1!!!!!!!!!!" << std::endl;

        std::vector<float> x;
        std::vector<float> y;
        std::vector<float> depth;

        cloud = cc_->msg_xyzrgb_;
        PointCloudT::Ptr cloud_new(new PointCloudT(*cloud));
        cc_->ready_xyzrgb_ = false;

        // Perform people detection on the new cloud:
        std::vector<pcl::people::PersonCluster<PointT> > clusters;   // vector containing persons clusters
        people_detector.setInputCloud(cloud_new);
        people_detector.setGround(ground_coeffs);                    // set floor coefficients
        people_detector.compute(clusters);                           // perform people detection

        ground_coeffs = people_detector.getGround();                 // get updated floor coefficients

        // Draw cloud and people bounding boxes in the viewer:
        viewer.removeAllPointClouds();
        viewer.removeAllShapes();
        pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud);
        viewer.addPointCloud<PointT> (cloud, rgb, "input_cloud");
        unsigned int k = 0;
        std::vector<pcl::people::PersonCluster<PointT> >::iterator it;
        std::vector<pcl::people::PersonCluster<PointT> >::iterator it_min;

        float min_z = 10.0;

        float histo_dist_min = 2.0;
        int id = -1;

        for(it = clusters.begin(); it != clusters.end(); ++it)
        {
          if(it->getPersonConfidence() > min_confidence)             // draw only people with confidence above a threshold
          {

            x.push_back((it->getTCenter())[0]);
            y.push_back((it->getTCenter())[1]);
            depth.push_back(it->getDistance());
            // draw theoretical person bounding box in the PCL viewer:
            /*pcl::copyPointCloud( *cloud, it->getIndices(), *cloud_people);
            if ( people_count == 0 )
            {
              first_hist = calc_histogram_a( cloud_people );
              people_count++;
              it->drawTBoundingBox(viewer, k);
            }
            else if ( people_count <= 10 )
            {
              histogram* hist_tmp = calc_histogram_a( cloud_people );
              add_hist( first_hist, hist_tmp );
              it->drawTBoundingBox(viewer, k);
              people_count++;
            }*/
            pcl::copyPointCloud( *cloud, it->getIndices(), *cloud_people);
            if ( people_count < 11 )
            {
              if ( it->getDistance() < min_z )
              {
                it_min = it;
                min_z = it->getDistance();
              }
            }
            else if ( people_count == 11 )
            {
              normalize_histogram( first_hist );
              people_count++;
              histogram* hist_tmp = calc_histogram( cloud_people );
              float tmp = histo_dist_sq( first_hist, hist_tmp );
              std::cout << "The histogram distance is " << tmp << std::endl;
              histo_dist_min = tmp;
              it_min = it;
              id = k;
            }
            else
            {
              histogram* hist_tmp = calc_histogram( cloud_people );
              float tmp = histo_dist_sq( first_hist, hist_tmp );
              std::cout << "The histogram distance is " << tmp << std::endl;
              if ( tmp < histo_dist_min )
              {
                histo_dist_min = tmp;
                it_min = it;
                id = k;
              }
            }
            k++;
            //std::cout << "The data of the center is " << cloud->points [(cloud->width >> 1) * (cloud->height + 1)].x << "  " << cloud->points [(cloud->width >> 1) * (cloud->height + 1)].y << " " << cloud->points [(cloud->width >> 1) * (cloud->height + 1)].z << std::endl;
            //std::cout << "The size of the people cloud is " <<  cloud_people->points.size() << std::endl;
            std::cout << "The " << k << " person's distance is " << it->getDistance() << std::endl;
          }
        }
        pub_people_.x = x;
        pub_people_.y = y;
        pub_people_.depth = depth;
        if ( k > 0 )
        {
          if ( people_count <= 11 )
          {
            pcl::copyPointCloud( *cloud, it_min->getIndices(), *cloud_people);
            if ( people_count == 0)
            {
              first_hist = calc_histogram_a( cloud_people );
              people_count++;
              it_min->drawTBoundingBox(viewer, 1);
            }
            else if ( people_count < 11 )
            {
              histogram* hist_tmp = calc_histogram_a( cloud_people );
              add_hist( first_hist, hist_tmp );
              it_min->drawTBoundingBox(viewer, 1);
              people_count++;
            }
          }
          else
          {
            pub_people_.id = k-1;
            if ( histo_dist_min < 1.3 )
            {
              it_min->drawTBoundingBox(viewer, 1);
              std::cout << "The minimum distance of the histogram is " << histo_dist_min << std::endl;
              std::cout << "The vector is " << it_min->getTCenter() << std::endl << "while the elements are " << (it_min->getTCenter())[0] << " " << (it_min->getTCenter())[1] << std::endl;
            }
            else
            {
              pub_people_.id = -1;
            }
          }
        }
        else
        {
          pub_people_.id = -1;
        }
        pub_people_.header.stamp = ros::Time::now();
        people_pub.publish(pub_people_);
        
        std::cout << k << " people found" << std::endl;
        viewer.spinOnce();
        ros::spinOnce();

        // Display average framerate:
        if (++count == 30)
        {
          double now = pcl::getTime ();
          std::cout << "Average framerate: " << double(count)/double(now - last) << " Hz" <<  std::endl;
          count = 0;
          last = now;
        }
        //cloud_mutex.unlock ();
      }
    }
    else
    {
      viewer.spinOnce();
      ros::spinOnce();
      // std::cout << "In state 0!!!!!!!!!" << std::endl;
    }
  }

  return 0;
}
コード例 #6
0
ファイル: show_pts.cpp プロジェクト: fxia22/tinker
int main(int argc, char** argv)
{
    int total_frame = 0;

    ros::init(argc, argv, "pub_pcl");
    ros::NodeHandle nh;
    ros::Publisher pub = nh.advertise<PointCloudT>("/openni/points2", 1);

    // Read
    PointCloudT::Ptr cloud(new PointCloudT);
    bool new_cloud_avaliable_flag = false;
    pcl::Grabber* interface = new pcl::OpenNIGrabber();
    boost::function<void (const PointCloudT::ConstPtr&)> f = boost::bind(&cloud_cb_, _1, cloud, &new_cloud_avaliable_flag);
    interface->registerCallback(f);
    interface->start();

    // Wait for the first frame
    while (!new_cloud_avaliable_flag)
    {
        boost::this_thread::sleep(boost::posix_time::milliseconds(1));
    }
    cloud_mutex.lock();     // for not overwriting the point cloud

    // Display
    printf("frame %d\n", ++total_frame);
    pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud);
    viewer.addPointCloud<PointT>(cloud, rgb, "input_cloud");
    viewer.setCameraPosition(0, 0, -2, 0, -1, 0, 0);

    cloud_mutex.unlock();

    ros::Rate loop_rate(4);
    while (nh.ok())
    {
        ros::Time scan_time = ros::Time::now();
        
        // Get & publish new cloud
        if (new_cloud_avaliable_flag && cloud_mutex.try_lock())
        {
            new_cloud_avaliable_flag = false;
            pub.publish(cloud);

            printf("frame %d\n", ++total_frame);
            viewer.removeAllPointClouds();
            viewer.removeAllShapes();
            pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud);
            viewer.addPointCloud<PointT>(cloud, rgb, "input_cloud");

            cloud_mutex.unlock();
        }
        else
        {
            printf("no %d\n", total_frame);
        }

        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}