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 (); }