如何在PCL中从观众中选取两个点

Question

我想从pointcloud中选择两个点并返回两点的坐标.为了解决这个问题,我使用了PointPickingEventPCL,编写了一个包含pointcloud,visualizer和vector的类来存储选定的点.我的代码:

#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;
}

但是当调试代码时,我什么都没得到.另外我知道当用左按钮拾取点时,应该按下SHIFT按钮.预先感谢您的任何帮助!

Answer 1

我发现 getPoints() 方法没有按我的预期工作。然而，getPoint() 效果很好。下面是打印所选点并将它们存储为向量的代码：

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

一旦选择了点，您还可以很容易地找到点之间的距离。

    if (selectedPoints.size() > 1)
    {
        float distance = pcl::euclideanDistance(selectedPoints[0], selectedPoints[1]);
        std::cout << "Distance is " << distance << std::endl;
    }

如果您想重新开始拾取点，则可以使用键盘事件清空 selectedPoints 向量。