void
pp_callback (const pcl::visualization::PointPickingEvent& event, void* cookie)
{
int idx = event.getPointIndex ();
if (idx == -1)
return;
if (!cloud)
{
cloud = *reinterpret_cast<pcl::PCLPointCloud2::Ptr*> (cookie);
xyzcloud.reset (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromPCLPointCloud2 (*cloud, *xyzcloud);
search.setInputCloud (xyzcloud);
}
// Return the correct index in the cloud instead of the index on the screen
std::vector<int> indices (1);
std::vector<float> distances (1);
// Because VTK/OpenGL stores data without NaN, we lose the 1-1 correspondence, so we must search for the real point
pcl::PointXYZ picked_pt;
event.getPoint (picked_pt.x, picked_pt.y, picked_pt.z);
//TODO: Look into this.
search.nearestKSearch (picked_pt, 1, indices, distances);
PCL_INFO ("Point index picked: %d (real: %d) - [%f, %f, %f]\n", idx, indices[0], picked_pt.x, picked_pt.y, picked_pt.z);
idx = indices[0];
// If two points were selected, draw an arrow between them
pcl::PointXYZ p1, p2;
if (event.getPoints (p1.x, p1.y, p1.z, p2.x, p2.y, p2.z) && p)
{
std::stringstream ss;
ss << p1 << p2;
p->addArrow<pcl::PointXYZ, pcl::PointXYZ> (p1, p2, 1.0, 1.0, 1.0, ss.str ());
return;
}
// Else, if a single point has been selected
std::stringstream ss;
ss << idx;
// Get the cloud's fields
for (size_t i = 0; i < cloud->fields.size (); ++i)
{
if (!isMultiDimensionalFeatureField (cloud->fields[i]))
continue;
PCL_INFO ("Multidimensional field found: %s\n", cloud->fields[i].name.c_str ());
#if VTK_MAJOR_VERSION==6 || (VTK_MAJOR_VERSION==5 && VTK_MINOR_VERSION>6)
ph_global.addFeatureHistogram (*cloud, cloud->fields[i].name, idx, ss.str ());
ph_global.renderOnce ();
#endif
}
if (p)
{
pcl::PointXYZ pos;
event.getPoint (pos.x, pos.y, pos.z);
p->addText3D<pcl::PointXYZ> (ss.str (), pos, 0.0005, 1.0, 1.0, 1.0, ss.str ());
}
}
void
pp_callback (const pcl::visualization::PointPickingEvent& event, void* cookie)
{
if (event.getPointIndex () == -1)
return;
sensor_msgs::PointCloud2::Ptr cloud = *static_cast<sensor_msgs::PointCloud2::Ptr*>(cookie);
if (!cloud)
return;
// If two points were selected, draw an arrow between them
pcl::PointXYZ p1, p2;
if (event.getPoints (p1.x, p1.y, p1.z, p2.x, p2.y, p2.z) && p)
{
std::stringstream ss;
ss << p1 << p2;
p->addArrow<pcl::PointXYZ, pcl::PointXYZ> (p1, p2, 1.0, 1.0, 1.0, ss.str ());
return;
}
// Else, if a single point has been selected
std::stringstream ss;
ss << event.getPointIndex ();
// Get the cloud's fields
for (size_t i = 0; i < cloud->fields.size (); ++i)
{
if (!isMultiDimensionalFeatureField (cloud->fields[i]))
continue;
ph_global.addFeatureHistogram (*cloud, cloud->fields[i].name, event.getPointIndex (), ss.str ());
}
if (p)
{
pcl::PointXYZ pos;
event.getPoint (pos.x, pos.y, pos.z);
p->addText3D<pcl::PointXYZ> (ss.str (), pos, 0.0005, 1.0, 1.0, 1.0, ss.str ());
}
ph_global.spinOnce ();
}