当前位置：网站首页>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();
}
边栏推荐
- LeetCode每日一题(1997. First Day Where You Have Been in All the Rooms)
- 攻防世界 MISC中reverseMe简述
- Monotonic stack
- Phishing & filename inversion & Office remote template
- 女生学软件测试难不难 入门门槛低，学起来还是比较简单的
- 基于购买行为数据对超市顾客进行市场细分（RFM模型）
- [Yu Yue education] flower cultivation reference materials of Weifang Vocational College
- 云上有AI，让地球科学研究更省力
- 接口自动化测试框架：Pytest+Allure+Excel
- Wish Dragon Boat Festival is happy
猜你喜欢
MySQL5.72. MSI installation failed
同事上了个厕所，我帮产品妹子轻松完成BI数据产品顺便得到奶茶奖励
How to translate professional papers and write English abstracts better
CS passed (cdn+ certificate) PowerShell online detailed version
Biomedical English contract translation, characteristics of Vocabulary Translation
After working for 10 years, I changed to a programmer. Now I'm 35 + years old and I'm not anxious
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
My seven years with NLP
ML之shap：基于adult人口普查收入二分类预测数据集(预测年收入是否超过50k)利用Shap值对XGBoost模型实现可解释性案例之详细攻略
![[brush questions] how can we correctly meet the interview?](/img/89/a5b874ba4db97fbb3d330af59c387a.png)
[brush questions] how can we correctly meet the interview?
随机推荐
接口自动化测试实践指导（上）：接口自动化需要做哪些准备工作
Bitcoinwin (BCW): 借贷平台Celsius隐瞒亏损3.5万枚ETH 或资不抵债
SSO流程分析
Successfully solved typeerror: data type 'category' not understood
Chinese English comparison: you can do this Best of luck
After sharing the clone remote project, NPM install reports an error - CB () never called! This is an error with npm itself.
SQL Server manager studio(SSMS)安装教程
《从0到1：CTFer成长之路》书籍配套题目（周更）
The internationalization of domestic games is inseparable from professional translation companies
How to do a good job in financial literature translation?
Is it difficult for girls to learn software testing? The threshold for entry is low, and learning is relatively simple
C语言_双创建、前插，尾插，遍历，删除
Brief introduction to the curriculum differences of colleges and universities at different levels of machine human major -ros1/ros2-
Fedora/REHL 安装 semanage
AttributeError: Can‘t get attribute ‘SPPF‘ on ＜module ‘models.common‘ from ‘/home/yolov5/models/comm
Blue Bridge Cup zero Foundation National Championship - day 20
GET 和 POST 请求类型的区别
How much is it to translate Chinese into English for one minute?
AttributeError: Can‘t get attribute ‘SPPF‘ on ＜module ‘models. common‘ from ‘/home/yolov5/models/comm
Distributed system basic (V) protocol (I)