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