思路
PCL拟合出来的圆柱为圆柱轴线的法向量和轴线过的一个点，本身和半径，不包含高度和起始点以及中止点，不过会返回使用到的点云信息。
- 通过一点和法向量可以计算出轴线方程
- 将用到的点全部投影到这条直线上，获取新的在直线上的点云
- 直线上的点两个端点就是起始点，通过起始点可以计算出高度
- 步骤3的计算过程可以将直线点旋转到与某一轴平行进行计算，计算完后将结果的两个点再旋转回去
核心代码
#include "PCLFit.h"
#include <vtkGenericOpenGLRenderWindow.h>
#include <vtkAxesActor.h>
#include <vtkOBJReader.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/io/vtk_io.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/passthrough.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d.h>
#include <pcl/surface/gp3.h>
#include <pcl/surface/poisson.h>
#include <pcl/surface/mls.h>
#include <pcl/surface/marching_cubes_hoppe.h>
#include <pcl/surface/marching_cubes_rbf.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/common/transforms.h>
#include <vtkPlaneSource.h>
#include <vtkPolyData.h>
#include <vtkActor.h>
#include <vtkCylinderSource.h>
#include <vtkTubeFilter.h>
PCLFit::PCLFit(QWidget* parent)
: QMainWindow(parent)
{
vtkWidget = new QVTKOpenGLNativeWidget(this);
this->resize(600, 400);
this->setCentralWidget(vtkWidget);
auto renderWindow = vtkSmartPointer<vtkGenericOpenGLRenderWindow>::New();
auto renderer = vtkSmartPointer<vtkRenderer>::New();
renderWindow->AddRenderer(renderer);
// PCL 点云显示到VTK
viewer = pcl::visualization::PCLVisualizer::Ptr(new pcl::visualization::PCLVisualizer(
renderer
, renderWindow, "", false));
// 拟合圆柱体
// 读取PCB
cylinderCloud.reset(new pcl::PointCloud<pcl::PointXYZ>);
pcl::io::loadPCDFile("D:/TEST/3D/pcb/cylinder.pcd", *cylinderCloud);
// 点云缩放
// cylinderCloud乘系数
//pcl::transformPointCloud(*cylinderCloud,
// *cylinderCloud,
// Eigen::Affine3f(Eigen::Scaling(.1f)));
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> o_color(cylinderCloud, 0, 0, 255);
viewer->addPointCloud<pcl::PointXYZ>(cylinderCloud, o_color, "o_point_cloud");
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "o_point_cloud");
auto&& cylinder = FitCylinder(cylinderCloud);
auto&& cloud_cylinder = std::get<3>(cylinder);
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> set_color(cloud_cylinder, 0, 255, 0);
viewer->addPointCloud<pcl::PointXYZ>(cloud_cylinder, set_color, "segment_point_cloud");
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "segment_point_cloud");
// 圆柱体法向量一点
auto&& normal_point = std::get<0>(cylinder);
// 圆柱体轴线法向量
auto&& normal = std::get<1>(cylinder);
// 圆柱体半径
auto&& radius = std::get<2>(cylinder);
// 点投影到直线的位置
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_projection(new pcl::PointCloud<pcl::PointXYZ>);
// 遍历cloud_cylinder
for (auto&& point : *cloud_cylinder)
{
// 点投影到直线
double t = -(
normal.x * (normal_point.x - point.x) +
normal.y * (normal_point.y - point.y) +
normal.z * (normal_point.z - point.z)) /
(normal.x * normal.x + normal.y * normal.y + normal.z * normal.z);
// 点投影到直线的位置
cloud_projection->push_back({
(float)(normal_point.x + normal.x * t),
(float)(normal_point.y + normal.y * t),
(float)(normal_point.z + normal.z * t)
});
}
// 旋转到X轴
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_rotation(new pcl::PointCloud<pcl::PointXYZ>);
// 获取normal绕Z轴角度
double angle_z = atan2(normal.y, normal.x);
// 获取normal需要绕Y旋转角度
double angle_y = atan2(normal.z, normal.x);
// 绕Z轴旋转
pcl::transformPointCloud(*cloud_projection,
*cloud_rotation,
Eigen::Affine3f(Eigen::AngleAxisf(angle_z, Eigen::Vector3f::UnitZ())));
// 绕Y轴旋转
pcl::transformPointCloud(*cloud_rotation,
*cloud_rotation,
Eigen::Affine3f(Eigen::AngleAxisf(angle_y, Eigen::Vector3f::UnitY())));
// 获取X最大值索引
auto&& max_x_point = std::max_element(cloud_rotation->begin(), cloud_rotation->end(), [](const pcl::PointXYZ& a, const pcl::PointXYZ& b) {
return a.x < b.x;
}).operator->();
//qDebug输出点
qDebug() << "max_x_point: " << max_x_point->x << " " << max_x_point->y << " " << max_x_point->z;
// 获取X最小值索引
auto&& min_x_point = std::min_element(cloud_rotation->begin(), cloud_rotation->end(), [](const pcl::PointXYZ& a, const pcl::PointXYZ& b) {
return a.x < b.x;
}).operator->();
//qDebug输出点
qDebug() << "min_x_point: " << min_x_point->x << " " << min_x_point->y << " " << min_x_point->z;
// 结果点云
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_result(new pcl::PointCloud<pcl::PointXYZ>);
cloud_result->push_back(*max_x_point);
cloud_result->push_back(*min_x_point);
// 反旋转
pcl::transformPointCloud(*cloud_result,
*cloud_result,
Eigen::Affine3f(Eigen::AngleAxisf(-angle_y, Eigen::Vector3f::UnitY())));
pcl::transformPointCloud(*cloud_result, *cloud_result,
Eigen::Affine3f(Eigen::AngleAxisf(-angle_z, Eigen::Vector3f::UnitZ())));
auto&& end_point = cloud_result->at(0);
auto&& start_point = cloud_result->at(1);
// 距离
auto&& distance = sqrt(
pow(start_point.x - end_point.x, 2) +
pow(start_point.y - end_point.y, 2) +
pow(start_point.z - end_point.z, 2)
);
// 获取start_point，end_point中心点
auto&& center_point = pcl::PointXYZ{ (start_point.x + end_point.x) / 2, (start_point.y + end_point.y) / 2, (start_point.z + end_point.z) / 2 };
// 显示圆柱体
vtkSmartPointer<vtkLineSource> lineSource =
vtkSmartPointer<vtkLineSource>::New();
lineSource->SetPoint1(start_point.x, start_point.y, start_point.z);
lineSource->SetPoint2(end_point.x, end_point.y, end_point.z);
vtkSmartPointer<vtkTubeFilter> tubeFilter = vtkSmartPointer<vtkTubeFilter>::New();
tubeFilter->SetInputConnection(lineSource->GetOutputPort());
tubeFilter->SetRadius(radius);
tubeFilter->SetNumberOfSides(50);
tubeFilter->CappingOn();
tubeFilter->Update();
vtkSmartPointer<vtkPolyData> polydata = tubeFilter->GetOutput();
vtkSmartPointer<vtkPolyDataMapper> mapper = vtkSmartPointer<vtkPolyDataMapper>::New();
mapper->SetInputData(polydata);
vtkSmartPointer<vtkActor> actor = vtkSmartPointer<vtkActor>::New();
actor->SetMapper(mapper);
actor->GetProperty()->SetColor(255, 0, 0);
renderer->AddActor(actor);
// VTK 点云显示到VTK
vtkWidget->setRenderWindow(viewer->getRenderWindow());
viewer->setupInteractor(vtkWidget->interactor(), vtkWidget->renderWindow());
}
PCLFit::~PCLFit()
{}
std::tuple<double, double, double, double> PCLFit::FitPlane(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
//选择拟合点云与几何模型
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
//创建随机采样一致性对象
pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr model_p(new pcl::SampleConsensusModelPlane<pcl::PointXYZ>(cloud));
//设置距离阈值
pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_p);
ransac.setDistanceThreshold(0.01);
//执行模型估计
ransac.computeModel();
// 根据索引提取内点
///方法1
//std::vector<int> inliers; //存储内点索引的向量
//ransac.getInliers(inliers); //提取内点对应的索引
//pcl::copyPointCloud(*cloud, inliers, *cloud_plane);
///方法2,需要pcl/filters/extract_indices.h头文件，较为繁琐。
//pcl::PointIndices pi;
//ransac.getInliers(pi.indices);
//pcl::IndicesPtr index_ptr(new vector<int>(pi.indices));/// 将自定义的点云索引数组pi进行智能指针的转换
//pcl::ExtractIndices<pcl::PointXYZ> extract;
//extract.setInputCloud(cloud);
//extract.setIndices(index_ptr);
//extract.setNegative(false); /// 提取索引外的点云，若设置为true,则与copyPointCloud提取结果相同
//extract.filter(*cloud_plane);
//= 根据索引提取内点 =
// 输出模型参数Ax+By+Cz+D=0
Eigen::VectorXf coefficient;
ransac.getModelCoefficients(coefficient);
qDebug() << "平面方程为：\n"
<< coefficient[0] << "x + "
<< coefficient[1] << "y + "
<< coefficient[2] << "z + "
<< coefficient[3] << " = 0";
return { coefficient[0] ,coefficient[1],coefficient[2],coefficient[3] };
}
std::tuple<pcl::PointXYZ, pcl::PointXYZ,
double, pcl::PointCloud<pcl::PointXYZ>::Ptr>
PCLFit::FitCylinder(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
// 创建法向量估计对象
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
// 设置搜索方式
ne.setSearchMethod(tree);
// 设置输入点云
ne.setInputCloud(cloud);
// 设置K近邻搜索点的个数
ne.setKSearch(50);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
// 计算法向量，并将结果保存到cloud_normals中
ne.compute(*cloud_normals);
//圆柱体分割
// 创建圆柱体分割对象
pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> seg;
// 设置输入点云：待分割点云
seg.setInputCloud(cloud);
// 设置对估计的模型系数需要进行优化
seg.setOptimizeCoefficients(true);
// 设置分割模型为圆柱体模型
seg.setModelType(pcl::SACMODEL_CYLINDER);
// 设置采用RANSAC算法进行参数估计
seg.setMethodType(pcl::SAC_RANSAC);
// 设置表面法线权重系数
seg.setNormalDistanceWeight(0.1);
// 设置迭代的最大次数
seg.setMaxIterations(10000);
// 设置内点到模型距离的最大值
seg.setDistanceThreshold(0.05);
// 设置圆柱模型半径的范围
seg.setRadiusLimits(3.0, 4.0);
// 设置输入法向量
seg.setInputNormals(cloud_normals);
// 保存分割结果
pcl::PointIndices::Ptr inliers_cylinder(new pcl::PointIndices);
// 保存圆柱体模型系数
pcl::ModelCoefficients::Ptr coefficients_cylinder(new pcl::ModelCoefficients);
// 执行分割，将分割结果的索引保存到inliers_cylinder中，同时存储模型系数coefficients_cylinder
seg.segment(*inliers_cylinder, *coefficients_cylinder);
qDebug() << "\n\t\t -圆柱体系数 -";
qDebug() << "轴线一点坐标：(" <<
coefficients_cylinder->values[0] << ", "
<< coefficients_cylinder->values[1] << ", "
<< coefficients_cylinder->values[2] << ")";
qDebug() << "轴线方向向量：(" <<
coefficients_cylinder->values[3] << ", "
<< coefficients_cylinder->values[4] << ", "
<< coefficients_cylinder->values[5] << ")";
qDebug() << "圆柱体半径：" << coefficients_cylinder->values[6];
//提取分割结果
// 创建索引提取点对象
pcl::ExtractIndices<pcl::PointXYZ> extract;
// 设置输入点云：待分割点云
extract.setInputCloud(cloud);
// 设置内点索引
extract.setIndices(inliers_cylinder);
// 默认false，提取圆柱体内点；true，提取圆柱体外点
extract.setNegative(false);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cylinder(new pcl::PointCloud<pcl::PointXYZ>());
// 执行滤波，并将结果点云保存到cloud_cylinder中
extract.filter(*cloud_cylinder);
return {
{
coefficients_cylinder->values[0],
coefficients_cylinder->values[1],
coefficients_cylinder->values[2]
},
{
coefficients_cylinder->values[3],
coefficients_cylinder->values[4],
coefficients_cylinder->values[5]
},
coefficients_cylinder->values[6],
cloud_cylinder };
}
效果
扫一扫
1233
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
