当前位置:网站首页>PCL realizes frame selection and clipping point cloud
PCL realizes frame selection and clipping point cloud
2022-07-06 06:50:00 【Leslie x Xu】
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();
}
边栏推荐
- CS passed (cdn+ certificate) PowerShell online detailed version
- LeetCode每日一题(1870. Minimum Speed to Arrive on Time)
- Cobalt strike feature modification
- Py06 dictionary mapping dictionary nested key does not exist test key sorting
- Apache dolphin scheduler source code analysis (super detailed)
- 接口自动化测试框架:Pytest+Allure+Excel
- Changes in the number of words in English papers translated into Chinese
- What is the difference between int (1) and int (10)? Senior developers can't tell!
- The internationalization of domestic games is inseparable from professional translation companies
- 18.多级页表与快表
猜你喜欢
How much is it to translate Chinese into English for one minute?
The ECU of 21 Audi q5l 45tfsi brushes is upgraded to master special adjustment, and the horsepower is safely and stably increased to 305 horsepower
指尖上的 NFT|在 G2 上评价 Ambire,有机会获得限量版收藏品
Monotonic stack
Financial German translation, a professional translation company in Beijing
AttributeError: Can‘t get attribute ‘SPPF‘ on <module ‘models.common‘ from ‘/home/yolov5/models/comm
前缀和数组系列
Apache dolphin scheduler source code analysis (super detailed)
Blue Bridge Cup zero Foundation National Championship - day 20
How to reconstruct the class explosion caused by m*n strategies?
随机推荐
Attributeerror: can 't get attribute' sppf 'on < module' models. Common 'from' / home / yolov5 / Models / comm
SSO process analysis
Pallet management in SAP SD delivery process
ECS accessKey key disclosure and utilization
Huawei equipment configuration ospf-bgp linkage
UniPro甘特图“初体验”:关注细节背后的多场景探索
Database basics exercise part 2
Thesis abstract translation, multilingual pure human translation
医疗软件检测机构怎么找,一航软件测评是专家
Chinese English comparison: you can do this Best of luck
Phishing & filename inversion & Office remote template
SQL Server manager studio(SSMS)安装教程
Machine learning plant leaf recognition
万丈高楼平地起,每个API皆根基
How much is it to translate Chinese into English for one minute?
The ECU of 21 Audi q5l 45tfsi brushes is upgraded to master special adjustment, and the horsepower is safely and stably increased to 305 horsepower
【刷题】怎么样才能正确的迎接面试?
接口自动化测试实践指导(上):接口自动化需要做哪些准备工作
LeetCode每日一题(1870. Minimum Speed to Arrive on Time)
Redis Foundation