点云裁剪界面qt+pcl+vtk

#include "cloud_crop.h" #include "ui_cloud_crop.h" bool isPickingMode = false; //判断是否为裁剪模式 bool inOrNot1(int poly_sides, double* poly_X, double* poly_Y, double x, double y) { int i, j; j = poly_sides - 1; bool res = false; //对每一条边进行遍历，该边的两个端点，有一个必须在待检测点(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 Cloud_Crop::projectInliers() { double focal[3] = { 0 }; double pos[3] = { 0 }; vtkRenderer* renderer{ viewer->getRendererCollection()->GetFirstRenderer() }; renderer->GetActiveCamera()->GetFocalPoint(focal); renderer->GetActiveCamera()->GetPosition(pos); //获取焦点单位向量 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; cloudIn_Prj->clear(); cloudCiecle_result->clear(); // 创建滤波器对象 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_cropped->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 (ui->radioButton_in->isChecked() && ret)//表示在里面 { cloud_cropped->push_back(cloud_in->points[i]); } if (ui->radioButton_out->isChecked() && !ret) //表示在外面 { cloud_cropped->push_back(cloud_in->points[i]); } } } void Cloud_Crop::getScreentPos(double* displayPos, double* world) { 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]; } } Cloud_Crop::Cloud_Crop(QWidget *parent) : QMainWindow(parent) , ui(new Ui::Cloud_Crop) { ui->setupUi(this); cloud_in.reset(new pcl::PointCloud<pcl::PointXYZ>); cloud_polygon.reset(new pcl::PointCloud<pcl::PointXYZ>); cloud_cropped.reset(new pcl::PointCloud<pcl::PointXYZ>); clouds.reset(new pcl::PointCloud<pcl::PointXYZL>); cloudIn_Prj.reset(new pcl::PointCloud<pcl::PointXYZ>); cloudCiecle_result.reset(new pcl::PointCloud<pcl::PointXYZ>); auto renderer = vtkSmartPointer<vtkRenderer>::New(); auto renderWindow = vtkSmartPointer<vtkGenericOpenGLRenderWindow>::New(); renderWindow->AddRenderer(renderer); viewer.reset(new pcl::visualization::PCLVisualizer(renderer, renderWindow, "viewer", false)); ui->qvtkWidget->setRenderWindow(viewer->getRenderWindow()); viewer->setupInteractor(ui->qvtkWidget->interactor(), ui->qvtkWidget->renderWindow()); connect(ui->qvtkWidget, SIGNAL(mouse(QMouseEvent*)), this, SLOT(QVTKWidgetMouseEvent(QMouseEvent*))); //connect(ui->qvtkWidget, SIGNAL(key(QKeyEvent*)), this, SLOT(QVTKWidgetKeyEvent(QKeyEvent*))); setMouseTracking(true); } Cloud_Crop::~Cloud_Crop() { delete ui; } void Cloud_Crop::on_pushButton_open_cloud_clicked() { QString cloud_path = QFileDialog::getOpenFileName(this, QString("打开.pcd文件"), "", "*.pcd"); pcl::io::loadPCDFile(cloud_path.toStdString(), *cloud_in); qDebug() << "load origin cloud" << cloud_in->size() << "points"; pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZ> fildColor(cloud_in, "x"); viewer->removeAllPointClouds(); viewer->removeAllShapes(); viewer->addPointCloud(cloud_in, fildColor, "cloud"); viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud"); viewer->setupInteractor(ui->qvtkWidget->interactor(), ui->qvtkWidget->renderWindow()); ui->qvtkWidget->setRenderWindow(viewer->getRenderWindow()); ui->qvtkWidget->renderWindow()->Render(); line_id = 0; } void Cloud_Crop::QVTKWidgetMouseEvent(QMouseEvent* e) { //qDebug()<<e->type(); if (e->type() == QEvent::MouseButtonRelease ) { if (isPickingMode) { double world_point[3]; double displayPos[2]; displayPos[0] = double(e->pos().x()); displayPos[1] = ui->qvtkWidget->height() - double(e->pos().y()); getScreentPos(displayPos, world_point); // std::cout << e->pos().x() << ',' << e->pos().y() << endl; // std::cout << 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); ui->qvtkWidget->renderWindow()->Render(); } lastP = curP; //切割点云添加 cloud_polygon->push_back(curP); } } } void Cloud_Crop::on_pushButton_save_cloud_clicked() { if(cloud_cropped->size()) { QString cloud_path = QFileDialog::getSaveFileName(this, QString("保存.pcd文件"), "", "*.pcd"); pcl::io::savePCDFile(cloud_path.toStdString(), *cloud_cropped); qDebug()<<"save cropped cloud" << cloud_cropped->size()<< "points"; } else { qDebug()<<"save cloud is empty"