void PickingEventOccurred_(const visualization::PointPickingEvent& event, void* viewer_void)
{
  

  boost::shared_ptr<visualization::PCLVisualizer> viewer = *static_cast<boost::shared_ptr<

  visualization::PCLVisualizer> *>(viewer_void);


  idx = event.getPointIndex ();

  if (idx == -1)

    return;

  

  vector<int> indices (1);

  vector<float> distances (1);

 
  // Use mutices to make sure we get the right cloud

  //boost::mutex::scoped_lock lock1 (cloud_mutex_);

  

  pcl::PointXYZ   PointAux;

  

  event.getPoint ( PointAux.x,  PointAux.y,  PointAux.z);

  

  char str[512];

  sprintf(str, "sphere_%d",idx);

  

  std::cout << " (" << PointAux.x << ", " << PointAux.y <<", " << PointAux.z<< "......)"

  << str<<std::endl;

  

  cloud_p->points.push_back (PointAux);

  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud_p, 255, 0, 0);

  //viewer_void->addSphere(PointAux, str);

  viewer->addPointCloud<pcl::PointXYZ>(cloud_p,single_color, str,0);

  viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,10,str);

  

}
    /** \brief Point picking callback. This gets called when the user selects
      * a 3D point on screen (in the PCLVisualizer window) using Shift+click.
      *
      * \param[in] event the event that triggered the call
      */
    void 
    pp_callback (const visualization::PointPickingEvent& event, void*)
    {
      // Check to see if we got a valid point. Early exit.
      int idx = event.getPointIndex ();
      if (idx == -1)
        return;

      vector<int> indices (1);
      vector<float> distances (1);

      // Get the point that was picked
      PointT picked_pt;
      event.getPoint (picked_pt.x, picked_pt.y, picked_pt.z);

      print_info (stderr, "Picked point with index %d, and coordinates %f, %f, %f.\n", idx, picked_pt.x, picked_pt.y, picked_pt.z);

      // Add a sphere to it in the PCLVisualizer window
      stringstream ss;
      ss << "sphere_" << idx;
      cloud_viewer_->addSphere (picked_pt, 0.01, 1.0, 0.0, 0.0, ss.str ());

      // Because VTK/OpenGL stores data without NaN, we lose the 1-1 correspondence, so we must search for the real point
      search_->nearestKSearch (picked_pt, 1, indices, distances);

      // Add some marker to the image
      if (image_viewer_)
      {
        // Get the [u, v] in pixel coordinates for the ImageViewer. Remember that 0,0 is bottom left.
        uint32_t width  = search_->getInputCloud ()->width,
                 height = search_->getInputCloud ()->height;
        int v = height - indices[0] / width,
            u = indices[0] % width;

        image_viewer_->addCircle (u, v, 5, 1.0, 0.0, 0.0, "circles", 1.0);
        image_viewer_->addFilledRectangle (u-5, u+5, v-5, v+5, 0.0, 1.0, 0.0, "boxes", 0.5);
        image_viewer_->markPoint (u, v, visualization::red_color, visualization::blue_color, 10);
      }

      // Segment the region that we're interested in, by employing a two step process:
      //  * first, segment all the planes in the scene, and find the one closest to our picked point
      //  * then, use euclidean clustering to find the object that we clicked on and return it
      PlanarRegion<PointT> region;
      segment (picked_pt, indices[0], region, object_);

      // If no region could be determined, exit
      if (region.getContour ().empty ())
      {
        PCL_ERROR ("No planar region detected. Please select another point or relax the thresholds and continue.\n");
        return;
      }
      // Else, draw it on screen
      else
      {
        cloud_viewer_->addPolygon (region, 0.0, 0.0, 1.0, "region");
        cloud_viewer_->setShapeRenderingProperties (visualization::PCL_VISUALIZER_LINE_WIDTH, 10, "region");

        // Draw in image space
        if (image_viewer_)
        {
          image_viewer_->addPlanarPolygon (search_->getInputCloud (), region, 0.0, 0.0, 1.0, "refined_region", 1.0);
        }
      }

      // If no object could be determined, exit
      if (!object_)
      {
        PCL_ERROR ("No object detected. Please select another point or relax the thresholds and continue.\n");
        return;
      }
      else
      {
        // Visualize the object in 3D...
        visualization::PointCloudColorHandlerCustom<PointT> red (object_, 255, 0, 0);
        if (!cloud_viewer_->updatePointCloud (object_, red, "object"))
          cloud_viewer_->addPointCloud (object_, red, "object");
        // ...and 2D
        if (image_viewer_)
        {
          image_viewer_->removeLayer ("object");
          image_viewer_->addMask (search_->getInputCloud (), *object_, "object");
        }

        // ...and 2D
        if (image_viewer_)
          image_viewer_->addRectangle (search_->getInputCloud (), *object_);
      }
    }
Beispiel #3
0
    /** \brief Point picking callback. This gets called when the user selects
      * a 3D point on screen (in the PCLVisualizer window) using Shift+click.
      *
      * \param[in] event the event that triggered the call
      */
    void 
    pp_callback (const visualization::PointPickingEvent& event, void*)
    {
      // Check to see if we got a valid point. Early exit.
      int idx = event.getPointIndex ();
      if (idx == -1)
        return;

      vector<int> indices (1);
      vector<float> distances (1);

      // Use mutices to make sure we get the right cloud
      boost::mutex::scoped_lock lock1 (cloud_mutex_);

      // Get the point that was picked
      PointT picked_pt;
      event.getPoint (picked_pt.x, picked_pt.y, picked_pt.z);

      // Add a sphere to it in the PCLVisualizer window
      stringstream ss;
      ss << "sphere_" << idx;
      cloud_viewer_.addSphere (picked_pt, 0.01, 1.0, 0.0, 0.0, ss.str ());

      // Check to see if we have access to the actual cloud data. Use the previously built search object.
      if (!search_.isValid ())
        return;

      // Because VTK/OpenGL stores data without NaN, we lose the 1-1 correspondence, so we must search for the real point
      search_.nearestKSearch (picked_pt, 1, indices, distances);

      // Get the [u, v] in pixel coordinates for the ImageViewer. Remember that 0,0 is bottom left.
      uint32_t width  = search_.getInputCloud ()->width,
               height = search_.getInputCloud ()->height;
      int v = height - indices[0] / width,
          u = indices[0] % width;

      // Add some marker to the image
      image_viewer_.addCircle (u, v, 5, 1.0, 0.0, 0.0, "circles", 1.0);
      image_viewer_.addFilledRectangle (u-5, u+5, v-5, v+5, 0.0, 1.0, 0.0, "boxes", 0.5);
      image_viewer_.markPoint (u, v, visualization::red_color, visualization::blue_color, 10);

      // Segment the region that we're interested in, by employing a two step process:
      //  * first, segment all the planes in the scene, and find the one closest to our picked point
      //  * then, use euclidean clustering to find the object that we clicked on and return it
      PlanarRegion<PointT> region;
      CloudPtr object;
      PointIndices region_indices;
      segment (picked_pt, indices[0], region, region_indices, object);

      // If no region could be determined, exit
      if (region.getContour ().empty ())
      {
        PCL_ERROR ("No planar region detected. Please select another point or relax the thresholds and continue.\n");
        return;
      }
      // Else, draw it on screen
      else
      {
        //cloud_viewer_.addPolygon (region, 1.0, 0.0, 0.0, "region");
        //cloud_viewer_.setShapeRenderingProperties (visualization::PCL_VISUALIZER_LINE_WIDTH, 10, "region");

        PlanarRegion<PointT> refined_region;
        pcl::approximatePolygon (region, refined_region, 0.01, false, true);
        PCL_INFO ("Planar region: %zu points initial, %zu points after refinement.\n", region.getContour ().size (), refined_region.getContour ().size ());
        cloud_viewer_.addPolygon (refined_region, 0.0, 0.0, 1.0, "refined_region");
        cloud_viewer_.setShapeRenderingProperties (visualization::PCL_VISUALIZER_LINE_WIDTH, 10, "refined_region");

        // Draw in image space
        image_viewer_.addPlanarPolygon (search_.getInputCloud (), refined_region, 0.0, 0.0, 1.0, "refined_region", 1.0);
      }

      // If no object could be determined, exit
      if (!object)
      {
        PCL_ERROR ("No object detected. Please select another point or relax the thresholds and continue.\n");
        return;
      }
      else
      {
        // Visualize the object in 3D...
        visualization::PointCloudColorHandlerCustom<PointT> red (object, 255, 0, 0);
        if (!cloud_viewer_.updatePointCloud (object, red, "object"))
          cloud_viewer_.addPointCloud (object, red, "object");
        // ...and 2D
        image_viewer_.removeLayer ("object");
        image_viewer_.addMask (search_.getInputCloud (), *object, "object");

        // Compute the min/max of the object
        PointT min_pt, max_pt;
        getMinMax3D (*object, min_pt, max_pt);
        stringstream ss;
        ss << "cube_" << idx;
        // Visualize the bounding box in 3D...
        cloud_viewer_.addCube (min_pt.x, max_pt.x, min_pt.y, max_pt.y, min_pt.z, max_pt.z, 0.0, 1.0, 0.0, ss.str ());
        cloud_viewer_.setShapeRenderingProperties (visualization::PCL_VISUALIZER_LINE_WIDTH, 10, ss.str ());

        // ...and 2D
        image_viewer_.addRectangle (search_.getInputCloud (), *object);
      }
    }