PCL realizes frame selection and clipping point cloud

PCL Realize frame selection and clipping point cloud

demand ： Click on the screen to draw a polygon , Crop the point cloud in the corresponding box .
Realization ： Press "x" Draw polygon Press again "x" tailoring

Implementation algorithm comes from PCL Polygon clipping of clipping

One 、 Realize screen drawing

1. Realize the conversion of screen coordinates to world coordinates

/** * @brief getScreentPos  Screen coordinates are converted to world coordinates  * @param displayPos  Input ： Screen coordinates  * @param world  Output ： World coordinates  * @param viewer_void  Input ：pclViewer */
void getScreentPos(double* displayPos, double* world,void* viewer_void)
{
    
    pcl::visualization::PCLVisualizer* viewer = static_cast<pcl::visualization::PCLVisualizer*> (viewer_void);
    vtkRenderer* renderer{
    viewer->getRendererCollection()->GetFirstRenderer()};
    // First compute the equivalent of this display point on the focal plane
    double fp[4], tmp1[4],  eventFPpos[4];
    renderer->GetActiveCamera()->GetFocalPoint(fp);
    fp[3] = 0.0;
    renderer->SetWorldPoint(fp);
    renderer->WorldToDisplay();
    renderer->GetDisplayPoint(tmp1);

    tmp1[0] = displayPos[0];
    tmp1[1] = displayPos[1];

    renderer->SetDisplayPoint(tmp1);
    renderer->DisplayToWorld();

    renderer->GetWorldPoint(eventFPpos);
    // Copy the result
    for (int i=0; i<3; i++)
    {
    
        world[i] = eventFPpos[i];
    }
}

2. Bind mouse

void mouseEventOccurred(const pcl::visualization::MouseEvent& event,void* viewer_void)
{
    
    pcl::visualization::PCLVisualizer* viewer = static_cast<pcl::visualization::PCLVisualizer*> (viewer_void);
    if (event.getButton() == pcl::visualization::MouseEvent::LeftButton &&
            event.getType() == pcl::visualization::MouseEvent::MouseButtonRelease)
    {
    
        std::cout << "Left mouse button released at position (" << event.getX() << ", " << event.getY() << ")" << std::endl;

        if(isPickingMode){
    
            double world_point[3];
            double displayPos[2];
            displayPos[0]=double(event.getX()),displayPos[1]=double(event.getY());
            getScreentPos(displayPos, world_point,viewer_void);

            std::cout<<endl<<world_point[0]<<','<<world_point[1]<<','<<world_point[2]<<endl;

            curP=pcl::PointXYZ(world_point[0], world_point[1], world_point[2]);
            if(!flag)flag=true;
            else {
    
                char str1[512];
                sprintf(str1, "line#%03d", line_id++);// The name cannot be repeated 
                viewer->addLine(lastP,curP,str1);
            }
            lastP=curP;
            // Cut point cloud add 
            cloud_polygon->push_back(curP);
        }
    }
}

Two 、 Implement the clipping algorithm

1. Calculate the relationship between a point and the edge of the polygon , Judge whether it is included

/** * @brief inOrNot1 * @param poly_sides  The number of vertices of the polygon drawn on the plane  * @param poly_X  Vertex x Coordinate arrays  * @param poly_Y  Vertex y Coordinate arrays  * @param x  Target point cloud x coordinate  * @param y  Target point cloud y coordinate  * @return */
int inOrNot1(int poly_sides, double *poly_X, double *poly_Y, double x, double y)
{
    
    int i, j;
    j = poly_sides - 1;
    int res = 0;

    // Traverse each edge , The two endpoints of the side , One must be at the point to be tested (x,y) Left side , And two points , There is a point y On the left p.y Small , Another point y Than p.y Big .
    for (i = 0; i < poly_sides; i++) {
    
        if (( (poly_Y[i] < y && poly_Y[j] >= y) || (poly_Y[j] < y && poly_Y[i] >= y) ) && (poly_X[i] <= x || poly_X[j] <= x))
        {
       // Intersect this edge with a horizontal line , The method of finding intersection x coordinate .
            res ^= ((poly_X[i] + (y - poly_Y[i]) / (poly_Y[j] - poly_Y[i])  *(poly_X[j] - poly_X[i])) < x);
        }
        j = i;
    }
    return res;
}

2. Achieve tailoring

Ideas ： Project the point cloud and polygon on a plane and filter out the index of points in the polygon

void projectInliers(void* viewer_void)
{
    
    pcl::visualization::PCLVisualizer* viewer = static_cast<pcl::visualization::PCLVisualizer*> (viewer_void);

    // Get camera focus and position information 
    double focal[3]={
    0}; double pos[3] = {
    0};
    vtkRenderer* renderer{
    viewer->getRendererCollection()->GetFirstRenderer()};
    renderer->GetActiveCamera()->GetFocalPoint(focal);
    renderer->GetActiveCamera()->GetPosition(pos);

    std::cout<<"focal: "<<focal[0]<<','<<focal[1]<<','<<focal[2]<<endl;
    std::cout<<"pos: "<<pos[0]<<','<<pos[1]<<','<<pos[2]<<endl;

    // Get the focus unit vector 
    pcl::PointXYZ eyeLine1 = pcl::PointXYZ(focal[0] - pos[0], focal[1] - pos[1], focal[2] - pos[2]);
    float mochang = sqrt(pow(eyeLine1.x, 2) + pow(eyeLine1.y, 2) + pow(eyeLine1.z, 2));// Die length 
    pcl::PointXYZ eyeLine = pcl::PointXYZ(eyeLine1.x / mochang, eyeLine1.y / mochang, eyeLine1.z / mochang);// Unit vector   The normal vector 

    // Create a plane based on the normal vector 
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());//ax+by+cz+d=0
    coefficients->values.resize(4);
    coefficients->values[0] = eyeLine.x;
    coefficients->values[1] = eyeLine.y;
    coefficients->values[2] = eyeLine.z;
    coefficients->values[3] = 0;

    // Create a point cloud that saves the result projection 
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudIn_Prj(new pcl::PointCloud<pcl::PointXYZ>);// Filter the input point cloud 
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudCiecle_result(new pcl::PointCloud<pcl::PointXYZ>);// Filter the point cloud of the drawn polygon 
    //  Create filter object 
    pcl::ProjectInliers<pcl::PointXYZ> proj;// Create a projection object 
    proj.setModelType(pcl::SACMODEL_PLANE);// Set the projection type 
    proj.setInputCloud(cloud_polygon);// Set the input point cloud 
    proj.setModelCoefficients(coefficients);// Load projection parameters 
    proj.filter(*cloudCiecle_result);// Execution procedure , And save the results 

    //  Create filter object 
    pcl::ProjectInliers<pcl::PointXYZ> projCloudIn;// Create a projection object 
    projCloudIn.setModelType(pcl::SACMODEL_PLANE);// Set the projection type 
    projCloudIn.setInputCloud(cloud_in);// Set the input point cloud 
    projCloudIn.setModelCoefficients(coefficients);// Load projection parameters 
    projCloudIn.filter(*cloudIn_Prj);// Execution procedure , And save the results 

    int ret=-1;
    double *PloyXarr = new double[cloudCiecle_result->points.size()];
    double *PloyYarr = new double[cloudCiecle_result->points.size()];
    for (int i = 0; i < cloudCiecle_result->points.size(); i++)
    {
    
        PloyXarr[i] = cloudCiecle_result->points[i].x;
        PloyYarr[i] = cloudCiecle_result->points[i].y;
    }

    cloud_cliped->clear();
    for (int i = 0; i < cloudIn_Prj->points.size(); i++)
    {
    
        // Judge 
        ret = inOrNot1(cloud_polygon->points.size(), PloyXarr, PloyYarr, cloudIn_Prj->points[i].x, cloudIn_Prj->points[i].y);
        if (1 == ret)// It means inside 
        {
    
            cloud_cliped->points.push_back(cloud_in->points[i]);
        }// It means outside 
    }

    viewer->removeAllPointClouds();
    viewer->addPointCloud(cloud_cliped,"aftercut");

    cloud_in->clear();
    pcl::copyPointCloud(*cloud_cliped,*cloud_in);

    viewer->getRenderWindow()->Render();
}

3、 ... and 、 Bind mouse and keyboard events

1. Static function callback

boost::shared_ptr<pcl::visualization::PCLVisualizer> interactionCustomizationVis()
{
    
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
    viewer->setBackgroundColor(0, 0, 0);
    viewer->addCoordinateSystem(1.0);
    viewer->setWindowName(" Mouse and keyboard events ");

    viewer->addPointCloud(cloud_polygon,"polyline");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR,1,0,0,"polyline");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,8,"polyline");

    viewer->registerKeyboardCallback(keyboardEventOccurred, (void*)viewer.get());
    viewer->registerMouseCallback(mouseEventOccurred, (void*)viewer.get());

    return (viewer);
}

2. When the callback function is a class member

    // Bind mouse and keyboard events 
    std::function<void(const pcl::visualization::KeyboardEvent&)> keyfun =
            std::bind(&PointCloudDlg::keyboardEventOccurred, this, std::placeholders::_1);//std::placeholders  Place holder  _1 For the first parameter 
    std::function<void(const pcl::visualization::MouseEvent&)> mousefun =
            std::bind(&PointCloudDlg::mouseEventOccurred, this, std::placeholders::_1);

    viewer->registerKeyboardCallback(keyfun);
    viewer->registerMouseCallback(mousefun);

Four 、 Load and save

1. load

void load(){
    
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer);
    pcl::PCDReader reader;
    reader.read("in.pcd",*cloud_in);
    viewer->addPointCloud(cloud_in,"cloud_in");
}

2. preservation

Be careful ： If an error occurs in saving , It may be caused by the inability to confirm the size of the point cloud , Need to set up height and width

void save(){
    
    // For disordered point clouds hight The default is 1
    cloud_cliped->height = 1;
    // In point cloud file push_back 了 j A little bit , so width=j
    cloud_cliped->width = cloud_cliped->points.size();

    pcl::io::savePCDFileASCII("out.pcd",*cloud_cliped);
}

5、 ... and 、 Source code

#include <iostream>
#include <boost/thread/thread.hpp>

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

#include <pcl/visualization/cloud_viewer.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/visualization/mouse_event.h> // Mouse events 
#include <pcl/visualization/keyboard_event.h>// Keyboard events 

#include <pcl/filters/project_inliers.h>
#include <pcl/ModelCoefficients.h>

#include "vtkCamera.h"
#include <vtkRenderWindow.h>

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_polygon (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cliped (new pcl::PointCloud<pcl::PointXYZ>);

pcl::PointXYZ curP,lastP;
bool flag=false;// Determine whether it is the first click 
bool isPickingMode = false;
unsigned int line_id = 0;

boost::shared_ptr<pcl::visualization::PCLVisualizer> interactionCustomizationVis();
void getScreentPos(double* displayPos, double* world,void* viewer_void);
void keyboardEventOccurred(const pcl::visualization::KeyboardEvent& event,void* viewer_void);
void mouseEventOccurred(const pcl::visualization::MouseEvent& event,void* viewer_void);
int inOrNot1(int poly_sides, double *poly_X, double *poly_Y, double x, double y);
void projectInliers(void*);


int main(int argc, char** argv)
{
    
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
    viewer = interactionCustomizationVis();
    std::cout<<"read cloud_in"<<endl;
    {
    
        pcl::PCDReader reader;
        reader.read("D:/lesliex/pcl&opencv/pclLearn/pcdFiles/1.pcd",*cloud_in);
        viewer->addPointCloud(cloud_in,"cloud_in");
    }
    std::cout<<"cloud_in size:"<<cloud->size()<<endl;

    while (!viewer->wasStopped())
    {
    
        viewer->spinOnce(100);
        boost::this_thread::sleep(boost::posix_time::microseconds(100000));
    }

    return 0;
}

// initialization 
boost::shared_ptr<pcl::visualization::PCLVisualizer> interactionCustomizationVis()
{
    
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
    viewer->setBackgroundColor(0, 0, 0);
    viewer->addCoordinateSystem(1.0);
    viewer->setWindowName(" Mouse and keyboard events ");

    viewer->addPointCloud(cloud_polygon,"polyline");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR,1,0,0,"polyline");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,8,"polyline");

    viewer->registerKeyboardCallback(keyboardEventOccurred, (void*)viewer.get());
    viewer->registerMouseCallback(mouseEventOccurred, (void*)viewer.get());

    return (viewer);
}

/** * @brief getScreentPos  Screen coordinates are converted to world coordinates  * @param displayPos  Input ： Screen coordinates  * @param world  Output ： World coordinates  * @param viewer_void  Input ：pclViewer */
void getScreentPos(double* displayPos, double* world,void* viewer_void)
{
    
    pcl::visualization::PCLVisualizer* viewer = static_cast<pcl::visualization::PCLVisualizer*> (viewer_void);
    vtkRenderer* renderer{
    viewer->getRendererCollection()->GetFirstRenderer()};
    // First compute the equivalent of this display point on the focal plane
    double fp[4], tmp1[4],  eventFPpos[4];
    renderer->GetActiveCamera()->GetFocalPoint(fp);
    fp[3] = 0.0;
    renderer->SetWorldPoint(fp);
    renderer->WorldToDisplay();
    renderer->GetDisplayPoint(tmp1);

    tmp1[0] = displayPos[0];
    tmp1[1] = displayPos[1];

    renderer->SetDisplayPoint(tmp1);
    renderer->DisplayToWorld();

    renderer->GetWorldPoint(eventFPpos);
    // Copy the result
    for (int i=0; i<3; i++)
    {
    
        world[i] = eventFPpos[i];
    }
}

// Keyboard events 
void keyboardEventOccurred(const pcl::visualization::KeyboardEvent& event,void* viewer_void)
{
    
    pcl::visualization::PCLVisualizer* viewer = static_cast<pcl::visualization::PCLVisualizer*> (viewer_void);

    if (event.getKeySym() == "x" && event.keyDown()){
    
        isPickingMode = !isPickingMode;
        if(isPickingMode){
    
            std::cout<<endl<<"start draw"<<endl;

            line_id = 0;
            cloud_polygon->clear();
            flag=false;
        }
        else{
    
            std::cout<<endl<<"stop draw"<<endl;
            projectInliers(viewer_void);
            viewer->removeAllShapes();
        }
    }
}

// Mouse events 
void mouseEventOccurred(const pcl::visualization::MouseEvent& event,void* viewer_void)
{
    
    pcl::visualization::PCLVisualizer* viewer = static_cast<pcl::visualization::PCLVisualizer*> (viewer_void);
    if (event.getButton() == pcl::visualization::MouseEvent::LeftButton &&
            event.getType() == pcl::visualization::MouseEvent::MouseButtonRelease)
    {
    
        std::cout << "Left mouse button released at position (" << event.getX() << ", " << event.getY() << ")" << std::endl;

        if(isPickingMode){
    
            double world_point[3];
            double displayPos[2];
            displayPos[0]=double(event.getX()),displayPos[1]=double(event.getY());
            getScreentPos(displayPos, world_point,viewer_void);

            std::cout<<endl<<world_point[0]<<','<<world_point[1]<<','<<world_point[2]<<endl;

            curP=pcl::PointXYZ(world_point[0], world_point[1], world_point[2]);
            if(!flag)flag=true;
            else {
    
                char str1[512];
                sprintf(str1, "line#%03d", line_id++);// The name cannot be repeated 
                viewer->addLine(lastP,curP,str1);
            }
            lastP=curP;
            // Cut point cloud add 
            cloud_polygon->push_back(curP);
        }
    }
}

/** * @brief inOrNot1 * @param poly_sides  The number of vertices of the polygon drawn on the plane  * @param poly_X  Vertex x Coordinate arrays  * @param poly_Y  Vertex y Coordinate arrays  * @param x  Target point cloud x coordinate  * @param y  Target point cloud y coordinate  * @return */
int inOrNot1(int poly_sides, double *poly_X, double *poly_Y, double x, double y)
{
    
    int i, j;
    j = poly_sides - 1;
    int res = 0;

    // Traverse each edge , The two endpoints of the side , One must be at the point to be tested (x,y) Left side , And two points , There is a point y On the left p.y Small , Another point y Than p.y Big .
    for (i = 0; i < poly_sides; i++) {
    
        if (( (poly_Y[i] < y && poly_Y[j] >= y) || (poly_Y[j] < y && poly_Y[i] >= y) ) && (poly_X[i] <= x || poly_X[j] <= x))
        {
       // Intersect this edge with a horizontal line , The method of finding intersection x coordinate .
            res ^= ((poly_X[i] + (y - poly_Y[i]) / (poly_Y[j] - poly_Y[i])  *(poly_X[j] - poly_X[i])) < x);
        }
        j = i;
    }
    return res;
}

// tailoring 
void projectInliers(void* viewer_void)
{
    
    pcl::visualization::PCLVisualizer* viewer = static_cast<pcl::visualization::PCLVisualizer*> (viewer_void);
    // Input point cloud 

    double focal[3]={
    0}; double pos[3] = {
    0};
    vtkRenderer* renderer{
    viewer->getRendererCollection()->GetFirstRenderer()};
    renderer->GetActiveCamera()->GetFocalPoint(focal);
    renderer->GetActiveCamera()->GetPosition(pos);

    std::cout<<"focal: "<<focal[0]<<','<<focal[1]<<','<<focal[2]<<endl;
    std::cout<<"pos: "<<pos[0]<<','<<pos[1]<<','<<pos[2]<<endl;

    // Get the focus unit vector 
    pcl::PointXYZ eyeLine1 = pcl::PointXYZ(focal[0] - pos[0], focal[1] - pos[1], focal[2] - pos[2]);

    // pcl::PointXYZ eyeLine1 = pcl::PointXYZ(camera1.focal[0] - camera1.pos[0], camera1.focal[1] - camera1.pos[1], camera1.focal[2] - camera1.pos[2]);
    float mochang = sqrt(pow(eyeLine1.x, 2) + pow(eyeLine1.y, 2) + pow(eyeLine1.z, 2));// Die length 
    pcl::PointXYZ eyeLine = pcl::PointXYZ(eyeLine1.x / mochang, eyeLine1.y / mochang, eyeLine1.z / mochang);// Unit vector   The normal vector 

    // Create a plane 
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());//ax+by+cz+d=0
    coefficients->values.resize(4);
    coefficients->values[0] = eyeLine.x;
    coefficients->values[1] = eyeLine.y;
    coefficients->values[2] = eyeLine.z;
    coefficients->values[3] = 0;

    // Create a point cloud that saves the result projection 
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudIn_Prj(new pcl::PointCloud<pcl::PointXYZ>);// Input point cloud 
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudCiecle_result(new pcl::PointCloud<pcl::PointXYZ>);// Drawn lines 
    //  Create filter object 
    pcl::ProjectInliers<pcl::PointXYZ> proj;// Create a projection object 
    proj.setModelType(pcl::SACMODEL_PLANE);// Set the projection type 
    proj.setInputCloud(cloud_polygon);// Set the input point cloud 
    proj.setModelCoefficients(coefficients);// Load projection parameters 
    proj.filter(*cloudCiecle_result);// Execution procedure , And save the results 

    //  Create filter object 
    pcl::ProjectInliers<pcl::PointXYZ> projCloudIn;// Create a projection object 
    projCloudIn.setModelType(pcl::SACMODEL_PLANE);// Set the projection type 
    projCloudIn.setInputCloud(cloud_in);// Set the input point cloud 
    projCloudIn.setModelCoefficients(coefficients);// Load projection parameters 
    projCloudIn.filter(*cloudIn_Prj);// Execution procedure , And save the results 

    int ret=-1;
    double *PloyXarr = new double[cloudCiecle_result->points.size()];
    double *PloyYarr = new double[cloudCiecle_result->points.size()];
    for (int i = 0; i < cloudCiecle_result->points.size(); i++)
    {
    
        PloyXarr[i] = cloudCiecle_result->points[i].x;
        PloyYarr[i] = cloudCiecle_result->points[i].y;
    }

    cloud_cliped->clear();
    for (int i = 0; i < cloudIn_Prj->points.size(); i++)
    {
    
        ret = inOrNot1(cloud_polygon->points.size(), PloyXarr, PloyYarr, cloudIn_Prj->points[i].x, cloudIn_Prj->points[i].y);
        if (1 == ret)// It means inside 
        {
    
            cloud_cliped->points.push_back(cloud_in->points[i]);
        }// It means outside 
    }

    viewer->removeAllPointClouds();
    viewer->addPointCloud(cloud_cliped,"aftercut");

    cloud_in->clear();
    pcl::copyPointCloud(*cloud_cliped,*cloud_in);

    viewer->getRenderWindow()->Render();
}