jak wybrać dwa punkty z przeglądarki w PCL

Question

Chciałbym wybrać dwa punkty z Pointcloud i powrócić współrzędne dwóch punktów. Aby poradzić sobie z tym problemem, użyłem PointPickingEvent języka PCL i napisałem klasę zawierającą pointcloud, wizualizator i wektor do przechowywania wybranych punktów. Mój kod:

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

Ale po debugowaniu kodu nic nie dostałem. Wiem również, że podczas wybierania punktów lewym klawiszem należy nacisnąć przycisk SHIFT. Z góry dziękuję za pomoc!

Answer 1

Okazało się, że metoda getPoints() nie działa zgodnie z oczekiwaniami. Jednak getPoint() działał dobrze. Oto kod, aby wydrukować wybrane punkty i zapisać je jest wektorem:

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

Można również znaleźć odległości między punktami dość łatwo po ich wybraniu.

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

Wektor selectedPoints można opróżnić za pomocą klawiaturyEvent, jeśli chcesz rozpocząć od wybrania punktów.