how to pick two points from viewer in PCL - point-cloud-library

Question

I would like to pick two points from pointcloud and return coordinates of the two points. In order to get down to the problem, I have used the PointPickingEvent of PCL, and written a class containing pointcloud, visualizer, and a vector to store selected points. My code:
#include <pcl/point_cloud.h>
#include <pcl/PCLPointCloud2.h>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/common/io.h>
#include <pcl/io/ply_io.h>
#include <pcl/io/vtk_lib_io.h>
#include <pcl/visualization/pcl_visualizer.h>
using namespace pcl;
using namespace std;
class pickPoints {
public:
pickPoints::pickPoints () {
viewer.reset (new pcl::visualization::PCLVisualizer ("Viewer", true));
viewer->registerPointPickingCallback (&pickPoints::pickCallback, *this);
}
~pickPoints () {}
void setInputCloud (PointCloud<PointXYZ>::Ptr cloud)
{
cloudTemp = cloud;
}
vector<float> getpoints() {
return p;
}
void simpleViewer ()
{
// Visualizer
viewer->addPointCloud<pcl::PointXYZ>(cloudTemp, "Cloud");
viewer->resetCameraViewpoint ("Cloud");
viewer->spin();
}
protected:
void pickCallback (const pcl::visualization::PointPickingEvent& event, void*)
{
if (event.getPointIndex () == -1)
return;
PointXYZ picked_point1,picked_point2;
event.getPoints(picked_point1.x,picked_point1.y,picked_point1.z,
picked_point2.x,picked_point2.y,picked_point2.z);
p.push_back(picked_point1.x); // store points
p.push_back(picked_point1.y);
p.push_back(picked_point1.z);
p.push_back(picked_point2.x);
p.push_back(picked_point2.y);
p.push_back(picked_point2.z);
//cout<<"first selected point: "<<p[0]<<" "<<p[1]<<" "<<p[2]<<endl;
//cout<<"second selected point: "<<p[3]<<" "<<p[4]<<" "<<p[5]<<endl;
}
private:
// Point cloud data
PointCloud<pcl::PointXYZ>::Ptr cloudTemp;
// The visualizer
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
// The picked point
vector<float> p;
};
int main()
{
//LOAD;
PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ> ());
pcl::PolygonMesh mesh;
pcl::io::loadPolygonFilePLY("test.ply", mesh);
pcl::fromPCLPointCloud2(mesh.cloud, *cloud);
pickPoints pickViewer;
pickViewer.setInputCloud(cloud); // A pointer to a cloud
pickViewer.simpleViewer();
vector<float> pointSelected;
pointSelected= pickViewer.getpoints();
cout<<pointSelected[0]<<" "<<pointSelected[1]<<" "<<pointSelected[2]<<endl;
cout<<pointSelected[3]<<" "<<pointSelected[4]<<" "<<pointSelected[5]<<endl;
cin.get();
return 0;
}
But when the code was debugged, I got nothing. Also I know that when picking points with the left button, the SHIFT button should be pressed. Thank you in advance for any help!

Answer 1

I found that the getPoints() method does not work as I expected. However, getPoint() worked well. Here is code to print out the selected points and store them is a vector:
std::vector<pcl::PointXYZ> selectedPoints;
void pointPickingEventOccurred(const pcl::visualization::PointPickingEvent& event, void* viewer_void)
{
float x, y, z;
if (event.getPointIndex() == -1)
{
return;
}
event.getPoint(x, y, z);
std::cout << "Point coordinate ( " << x << ", " << y << ", " << z << ")" << std::endl;
selectedPoints.push_back(pcl::PointXYZ(x, y, z));
}
void displayCloud(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud, const std::string& window_name)
{
if (cloud->size() < 1)
{
std::cout << window_name << " display failure. Cloud contains no points\n";
return;
}
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer(window_name));
pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZI> point_cloud_color_handler(cloud, "intensity");
viewer->addPointCloud< pcl::PointXYZI >(cloud, point_cloud_color_handler, "id");
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "id");
viewer->registerKeyboardCallback(keyboardEventOccurred, (void*)viewer.get());
viewer->registerPointPickingCallback(pointPickingEventOccurred, (void*)&viewer);
while (!viewer->wasStopped() && !close_window){
viewer->spinOnce(50);
}
close_window = false;
viewer->close();
}
You can also find distances between the points pretty easily once they are selected.
if (selectedPoints.size() > 1)
{
float distance = pcl::euclideanDistance(selectedPoints[0], selectedPoints[1]);
std::cout << "Distance is " << distance << std::endl;
}
The selectedPoints vector can be emptied with a keyboardEvent if you want to start over picking points.