当前位置：网站首页>PCL实现选框裁剪点云
PCL实现选框裁剪点云
2022-07-06 06:43:00 【Leslie X徐】
PCL实现选框裁剪点云
需求：在屏幕上点击画出多边形，裁剪对应框内的点云。
实现：按"x" 绘制多边形 再按"x"裁剪
实现算法来自 PCL裁剪之多边形裁剪
一、实现屏幕画线
1.实现屏幕坐标转换至世界坐标
/** * @brief getScreentPos 屏幕坐标转换至世界坐标 * @param displayPos 输入：屏幕坐标 * @param world 输出：世界坐标 * @param viewer_void 输入：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.绑定鼠标
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++);//名字不能重复
viewer->addLine(lastP,curP,str1);
}
lastP=curP;
//切割点云添加
cloud_polygon->push_back(curP);
}
}
}
二、实现裁剪算法
1.计算某点与多边形的边的关系，判断是否在内
/** * @brief inOrNot1 * @param poly_sides 平面上绘制多边形的顶点数 * @param poly_X 顶点的x坐标数组 * @param poly_Y 顶点的y坐标数组 * @param x 目标点云的x坐标 * @param y 目标点云的y坐标 * @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;
//对每一条边进行遍历，该边的两个端点，有一个必须在待检测点(x,y)的左边，且两个点中，有一个点的y左边比p.y小，另一个点的y比p.y大。
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))
{
//用水平的直线与该边相交，求交点的x坐标。
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.实现裁剪
思路：将点云和多边形投影在一个平面上过滤出在多边形内的点的索引
void projectInliers(void* viewer_void)
{
pcl::visualization::PCLVisualizer* viewer = static_cast<pcl::visualization::PCLVisualizer*> (viewer_void);
//获取相机焦点和位置信息
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;
//获取焦点单位向量
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));//模长
pcl::PointXYZ eyeLine = pcl::PointXYZ(eyeLine1.x / mochang, eyeLine1.y / mochang, eyeLine1.z / mochang);//单位向量 法向量
//根据法向量创建一个平面
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;
//创建保存结果投影的点云
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudIn_Prj(new pcl::PointCloud<pcl::PointXYZ>);//过滤输入的点云
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudCiecle_result(new pcl::PointCloud<pcl::PointXYZ>);//过滤绘制的多边形的点云
// 创建滤波器对象
pcl::ProjectInliers<pcl::PointXYZ> proj;//建立投影对象
proj.setModelType(pcl::SACMODEL_PLANE);//设置投影类型
proj.setInputCloud(cloud_polygon);//设置输入点云
proj.setModelCoefficients(coefficients);//加载投影参数
proj.filter(*cloudCiecle_result);//执行程序，并将结果保存
// 创建滤波器对象
pcl::ProjectInliers<pcl::PointXYZ> projCloudIn;//建立投影对象
projCloudIn.setModelType(pcl::SACMODEL_PLANE);//设置投影类型
projCloudIn.setInputCloud(cloud_in);//设置输入点云
projCloudIn.setModelCoefficients(coefficients);//加载投影参数
projCloudIn.filter(*cloudIn_Prj);//执行程序，并将结果保存
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)//表示在里面
{
cloud_cliped->points.push_back(cloud_in->points[i]);
}//表示在外面
}
viewer->removeAllPointClouds();
viewer->addPointCloud(cloud_cliped,"aftercut");
cloud_in->clear();
pcl::copyPointCloud(*cloud_cliped,*cloud_in);
viewer->getRenderWindow()->Render();
}
三、绑定鼠标键盘事件
1.静态函数回调
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("鼠标键盘事件");
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.当回调函数为类成员
//绑定鼠标键盘事件
std::function<void(const pcl::visualization::KeyboardEvent&)> keyfun =
std::bind(&PointCloudDlg::keyboardEventOccurred, this, std::placeholders::_1);//std::placeholders 占位符 _1为第一个参数
std::function<void(const pcl::visualization::MouseEvent&)> mousefun =
std::bind(&PointCloudDlg::mouseEventOccurred, this, std::placeholders::_1);
viewer->registerKeyboardCallback(keyfun);
viewer->registerMouseCallback(mousefun);
四、加载和保存
1.加载
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.保存
注意：如果保存发生错误，可能是无法确认点云大小引起，需要设置height和width
void save(){
//对于无序点云hight默认是1
cloud_cliped->height = 1;
//点云文件中push_back了j个点，故width=j
cloud_cliped->width = cloud_cliped->points.size();
pcl::io::savePCDFileASCII("out.pcd",*cloud_cliped);
}
五、源代码
#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> //鼠标事件
#include <pcl/visualization/keyboard_event.h>//键盘事件
#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;//判断是不是第一次点击
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;
}
//初始化
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("鼠标键盘事件");
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 屏幕坐标转换至世界坐标 * @param displayPos 输入：屏幕坐标 * @param world 输出：世界坐标 * @param viewer_void 输入：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];
}
}
//键盘事件
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();
}
}
}
//鼠标事件
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++);//名字不能重复
viewer->addLine(lastP,curP,str1);
}
lastP=curP;
//切割点云添加
cloud_polygon->push_back(curP);
}
}
}
/** * @brief inOrNot1 * @param poly_sides 平面上绘制多边形的顶点数 * @param poly_X 顶点的x坐标数组 * @param poly_Y 顶点的y坐标数组 * @param x 目标点云的x坐标 * @param y 目标点云的y坐标 * @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;
//对每一条边进行遍历，该边的两个端点，有一个必须在待检测点(x,y)的左边，且两个点中，有一个点的y左边比p.y小，另一个点的y比p.y大。
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))
{
//用水平的直线与该边相交，求交点的x坐标。
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;
}
//裁剪
void projectInliers(void* viewer_void)
{
pcl::visualization::PCLVisualizer* viewer = static_cast<pcl::visualization::PCLVisualizer*> (viewer_void);
//输入的点云
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;
//获取焦点单位向量
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));//模长
pcl::PointXYZ eyeLine = pcl::PointXYZ(eyeLine1.x / mochang, eyeLine1.y / mochang, eyeLine1.z / mochang);//单位向量 法向量
//创建一个平面
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;
//创建保存结果投影的点云
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudIn_Prj(new pcl::PointCloud<pcl::PointXYZ>);//输入的点云
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudCiecle_result(new pcl::PointCloud<pcl::PointXYZ>);//绘制的线
// 创建滤波器对象
pcl::ProjectInliers<pcl::PointXYZ> proj;//建立投影对象
proj.setModelType(pcl::SACMODEL_PLANE);//设置投影类型
proj.setInputCloud(cloud_polygon);//设置输入点云
proj.setModelCoefficients(coefficients);//加载投影参数
proj.filter(*cloudCiecle_result);//执行程序，并将结果保存
// 创建滤波器对象
pcl::ProjectInliers<pcl::PointXYZ> projCloudIn;//建立投影对象
projCloudIn.setModelType(pcl::SACMODEL_PLANE);//设置投影类型
projCloudIn.setInputCloud(cloud_in);//设置输入点云
projCloudIn.setModelCoefficients(coefficients);//加载投影参数
projCloudIn.filter(*cloudIn_Prj);//执行程序，并将结果保存
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)//表示在里面
{
cloud_cliped->points.push_back(cloud_in->points[i]);
}//表示在外面
}
viewer->removeAllPointClouds();
viewer->addPointCloud(cloud_cliped,"aftercut");
cloud_in->clear();
pcl::copyPointCloud(*cloud_cliped,*cloud_in);
viewer->getRenderWindow()->Render();
}
边栏推荐
- Fedora/rehl installation semanage
- MySQL high frequency interview 20 questions, necessary (important)
- [brush questions] how can we correctly meet the interview?
- Attributeerror successfully resolved: can only use cat accessor with a ‘category‘ dtype
- 详解SQL中Groupings Sets 语句的功能和底层实现逻辑
- mysql的基础命令
- How to translate biomedical instructions in English
- Suspended else
- How effective is the Chinese-English translation of international economic and trade contracts
- 基于购买行为数据对超市顾客进行市场细分（RFM模型）
猜你喜欢
Chinese English comparison: you can do this Best of luck
![[unity] how to export FBX in untiy](/img/03/b7937a1ac1a677f52616186fb85ab3.jpg)
[unity] how to export FBX in untiy
How to translate professional papers and write English abstracts better
机器人类专业不同层次院校课程差异性简述-ROS1/ROS2-
电子书-CHM-上线CS
Office-DOC加载宏-上线CS
Summary of leetcode's dynamic programming 4
CS-证书指纹修改
Today's summer solstice
Cobalt Strike特征修改
随机推荐
一文读懂简单查询代价估算
mysql的基础命令
ECS accessKey key disclosure and utilization
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
What are the characteristics of trademark translation and how to translate it?
My seven years with NLP
How to do a good job in financial literature translation?
Day 248/300 thoughts on how graduates find jobs
Bitcoinwin (BCW): 借贷平台Celsius隐瞒亏损3.5万枚ETH 或资不抵债
MySQL5.72.msi安装失败
26岁从财务转行软件测试，4年沉淀我已经是25k的测开工程师...
Leetcode daily question (1870. minimum speed to arrive on time)
女生学软件测试难不难 入门门槛低，学起来还是比较简单的
基于PyTorch和Fast RCNN快速实现目标识别
Day 246/300 SSH connection prompt "remote host identification has changed!"
Fledgling Xiao Li's 103rd blog CC2530 resource introduction
Summary of leetcode's dynamic programming 4
C语言_双创建、前插，尾插，遍历，删除
翻译生物医学说明书，英译中怎样效果佳
Difference between backtracking and recursion