how to pick two points from viewer in PCL

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!

Going

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.

Pensive