当前位置:网站首页>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();
}
边栏推荐
- Financial German translation, a professional translation company in Beijing
- 因高额网络费用,Arbitrum 奥德赛活动暂停,Nitro 发行迫在眉睫
- Simple use of MySQL database: add, delete, modify and query
- Market segmentation of supermarket customers based on purchase behavior data (RFM model)
- Bitcoinwin (BCW): 借贷平台Celsius隐瞒亏损3.5万枚ETH 或资不抵债
- Basic commands of MySQL
- Latex文字加颜色的三种办法
- AttributeError: Can‘t get attribute ‘SPPF‘ on <module ‘models.common‘ from ‘/home/yolov5/models/comm
- AttributeError: Can‘t get attribute ‘SPPF‘ on <module ‘models.common‘ from ‘/home/yolov5/models/comm
- Py06 dictionary mapping dictionary nested key does not exist test key sorting
猜你喜欢
Wish Dragon Boat Festival is happy
Biomedical English contract translation, characteristics of Vocabulary Translation
C语言_双创建、前插,尾插,遍历,删除
How do programmers remember code and programming language?
Market segmentation of supermarket customers based on purchase behavior data (RFM model)
利用快捷方式-LNK-上线CS
《从0到1:CTFer成长之路》书籍配套题目(周更)
Thesis abstract translation, multilingual pure human translation
When my colleague went to the bathroom, I helped my product sister easily complete the BI data product and got a milk tea reward
[ 英语 ] 语法重塑 之 动词分类 —— 英语兔学习笔记(2)
随机推荐
将ue4程序嵌入qt界面显示
ECS accessKey key disclosure and utilization
E-book CHM online CS
基于购买行为数据对超市顾客进行市场细分(RFM模型)
In English translation of papers, how to do a good translation?
AttributeError: Can‘t get attribute ‘SPPF‘ on <module ‘models. common‘ from ‘/home/yolov5/models/comm
MySQL high frequency interview 20 questions, necessary (important)
成功解决AttributeError: Can only use .cat accessor with a ‘category‘ dtype
Huawei equipment configuration ospf-bgp linkage
[Yu Yue education] Dunhuang Literature and art reference materials of Zhejiang Normal University
Phishing & filename inversion & Office remote template
基于PyTorch和Fast RCNN快速实现目标识别
[brush questions] how can we correctly meet the interview?
How to translate professional papers and write English abstracts better
BUU的MISC(不定时更新)
LeetCode - 152 乘积最大子数组
[ 英语 ] 语法重塑 之 动词分类 —— 英语兔学习笔记(2)
Thesis abstract translation, multilingual pure human translation
SQL Server Manager studio (SSMS) installation tutorial
前缀和数组系列