当前位置：网站首页>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();
}
边栏推荐
- Erreur de type résolue avec succès: type de données « catégorie» non sous - jacente
- mysql的基础命令
- How much is it to translate Chinese into English for one minute?
- L'Ia dans les nuages rend la recherche géoscientifique plus facile
- [unity] how to export FBX in untiy
- Py06 dictionary mapping dictionary nested key does not exist test key sorting
- [advanced software testing step 1] basic knowledge of automated testing
- Day 248/300 关于毕业生如何找工作的思考
- [brush questions] how can we correctly meet the interview?
- [ 英语 ] 语法重塑 之 英语学习的核心框架 —— 英语兔学习笔记（1）
猜你喜欢
Bitcoinwin (BCW): the lending platform Celsius conceals losses of 35000 eth or insolvency
Classification des verbes reconstruits grammaticalement - - English Rabbit Learning notes (2)
C语言_双创建、前插，尾插，遍历，删除
26岁从财务转行软件测试，4年沉淀我已经是25k的测开工程师...
How to convert flv file to MP4 file? A simple solution
Cobalt strike feature modification
Monotonic stack
How effective is the Chinese-English translation of international economic and trade contracts
女生学软件测试难不难 入门门槛低，学起来还是比较简单的
Chapter 7 - thread pool of shared model
随机推荐
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!
Day 245/300 JS foreach data cannot be updated to the object after multi-layer nesting
【软件测试进阶第1步】自动化测试基础知识
Attributeerror successfully resolved: can only use cat accessor with a ‘category‘ dtype
ROS2安装及基础知识介绍
CS-证书指纹修改
Basic commands of MySQL
Erreur de type résolue avec succès: type de données « catégorie» non sous - jacente
雲上有AI，讓地球科學研究更省力
Biomedical English contract translation, characteristics of Vocabulary Translation
[English] Verb Classification of grammatical reconstruction -- English rabbit learning notes (2)
Leetcode daily question (971. flip binary tree to match preorder traversal)
Leetcode daily question (1870. minimum speed to arrive on time)
SSO流程分析
UNIPRO Gantt chart "first experience": multi scene exploration behind attention to details
Windows Server 2016 standard installing Oracle
Apache dolphin scheduler source code analysis (super detailed)
What are the characteristics of trademark translation and how to translate it?
My creation anniversary