隧道点云轨道提取【C++PCL】

迅卓科技

已于 2023-11-28 15:43:05 修改

阅读量780

点赞数 7

分类专栏：隧道点云分析与检测文章标签： c++

于 2023-11-23 14:08:56 首次发布

本文链接：https://blog.csdn.net/weixin_45196569/article/details/134575990

版权

隧道点云分析与检测专栏收录该内容

12 篇文章 5 订阅 ¥69.90 ¥99.00

订阅专栏

作者：迅卓科技
简介：本人从事过多项点云项目，并且负责的项目均已得到好评！
公众号：迅卓科技，一个可以让您可以学习点云的好地方

1.前言

我们团队注重每一个细节，确保代码的可读性、可维护性和可扩展性达到最高标准。我们严格遵循行业最佳实践，采用模块化和面向对象的设计原则，使得我们的代码结构清晰且易于理解。

我们的点云服务更是引以为傲。我们的技术团队拥有深厚的专业知识和丰富的经验，能够提供高度准确和高效的点云处理服务。无论是点云的获取、存储还是分析，我们都能够满足客户的需求，并提供出色的解决方案。

我们的技术实力也是无与伦比的，拥有广泛的技术背景和深入的专业知识。无论是算法优化、性能调优还是技术创新，我们都能够应对自如，并为客户提供最佳的技术支持。

2.开发环境

下载源代码并解压缩。
在Visual Studio中打开项目。
根据需要配置PCL库。
编译和运行项目。

3.示例（效果图）及说明

PGetMaxPlaneCoe函数:

该函数利用随机采样一致性（RANSAC）算法从点云中估计最大平面模型的系数和内点索引。在实现中，通过迭代随机选择一组点，拟合平面，并统计在一定阈值内符合拟合的点，最终得到平面模型的系数和内点索引。这一步骤是点云处理中常用的平面提取方法，可用于识别地面等平面结构。

PExtracting函数:

该函数根据给定的内点索引，从点云中提取或排除相应的点。内点通常是通过RANSAC等方法得到的，表示符合某种模型的点。PExtracting函数的作用是根据这些内点索引，将它们提取出来，或者在点云中排除掉，得到经过筛选的点云。这一步骤对于后续的分析和处理起到了关键作用。

PGetPlaneCoe函数:

该函数通过迭代地估计平面模型的系数，并提取平面点云，直到找到一个与参考法线角度较小的平面。这一过程是通过迭代优化来寻找与参考法线角度较小的平面模型，以进一步提高平面提取的精度。这种方法对于对地面等结构进行更精细的提取具有重要意义。

PRGrowingSw函数:

函数使用法线估计和区域生长算法对点云进行聚类，将聚类结果保存在cloud_new点云中。该函数通过对点云中的点进行法线估计，然后应用区域生长算法，将相邻点划分到同一簇中，形成聚类结果。聚类通常用于将相似性较高的点分组，对于点云的分割和特征提取非常有用。

PRotateToXOY函数:

该函数用于将点云按照给定的平面模型系数进行旋转，以将该平面旋转到XOY平面。这一步骤的目的是规范化点云的方向，使得后续处理更加方便。通过旋转，将点云的特定平面调整到已知的参考方向，有助于后续分析和可视化。

4.源码如下

main.cpp

#include "Fliter.h"
#include <iostream>
#include <algorithm>
#include <pcl/io/pcd_io.h>
#include <pcl/console/time.h>


int main()
{

	//Initialize data to prevent data from not being empty
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_End(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud_1(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_underside(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_Allside_be(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_Allside(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_Boundary(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_underside_be(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_Ashape(new pcl::PointCloud<pcl::PointXYZ>);


	pcl::ModelCoefficients::Ptr coefficients_underside(new pcl::ModelCoefficients);
	pcl::ModelCoefficients::Ptr coefficients_side(new pcl::ModelCoefficients);
	pcl::ModelCoefficients::Ptr coefficients_Line(new pcl::ModelCoefficients);

	pcl::PointIndices::Ptr inliers(new pcl::PointIndices);


	Eigen::Matrix4f transToXoy;
	Eigen::Matrix4f transT;
	Eigen::Matrix4f transS;
	Eigen::Matrix4f transW;
	
	//Load pointcloud from the inputPath
	pcl::io::loadPCDFile("input/Cloud.pcd", *input_cloud_1);

#pragma region RotateToXOY

	PVoxlGrid(input_cloud_1, input_cloud);

	//Using the PGetMaxPlaneCoe and PExtracting to get underside and the remain pointcloud
	PGetMaxPlaneCoe(input_cloud, coefficients_underside, inliers, 0.2);
	PExtracting(inliers, input_cloud, cloud_underside, false);
	PExtracting(inliers, input_cloud, cloud_Allside, true);
	//Rotate the point cloud to the XOY plane
	PRotateToXOY(cloud_underside, cloud_underside_be, coefficients_underside, transS);
	PRotateToXOY(input_cloud, cloud_Allside_be, coefficients_underside, transS);

	//Get the boundary of the underside
	PAshape(cloud_underside_be, cloud_Boundary);

	//Rotate the point cloud so that it is parallel to the axis
	PGetLineInfo(cloud_Boundary, coefficients_Line);
	cloud_underside->clear();
	cloud_Allside->clear();
	PparallelXYZ(coefficients_Line, cloud_Allside_be, 'y', cloud_Allside, transT);

#pragma endregion

	//here used the tictoc to reckon by time 
	pcl::console::TicToc tt;
	tt.tic();
	//get the whole point cloud after rotate to XOY,all progresses begin from here
	*cloud = *cloud_Allside;
	//here you can load the point cloud to look wrong or right
	pcl::io::savePCDFileBinary("output/cloud.pcd", *cloud);
	//here we will do something to cut the point cloud

	//here we creat a vector to put into the point cloud we will use next
	std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloud_CutVector;

	//cut the point cloud into the vector of cloud_CutVector
	PCutStr(cloud, cloud_CutVector, "y", 0.001);
	//sort the point cloud by the size of the point cloud
	sort(cloud_CutVector.begin(), cloud_CutVector.end(), PsortByPointSize);
	//define n to save the pointcloud

	for (int i = 0; i < cloud_CutVector.size(); ++i)
	{
		pcl::PointXYZ pmin, pmax;
		double ditsance1 = pcl::getMaxSegment(*cloud_CutVector[i], pmin, pmax);
		if (ditsance1 < 10)
		{
			*cloud_End += *cloud_CutVector[i];
		}
	}

	PRGrowingSw(cloud_End, cloud_End);
	pcl::io::savePCDFileBinary("output/cloud_End.pcd", *cloud_End);
	
	std::cout << "Ok ! The progress is finished,and the time of this progress is :" << tt.toc() / 1000 << "s!" << std::endl;
}

Fliter.cpp

#include "Fliter.h"
#include <pcl/io/pcd_io.h>

void PVoxlGrid(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& new_cloud)
{
    pcl::VoxelGrid<pcl::PointXYZ> vg;
    vg.setInputCloud(cloud);             // 输入点云
    vg.setLeafSize(0.3f, 0.3f, 0.3f); // 设置最小体素边长
    vg.filter(*new_cloud);          // 进行滤波
}

void PGetMaxPlaneCoe(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::ModelCoefficients::Ptr& coefficients, pcl::PointIndices::Ptr& inliers, double Threshold)
{
    pcl::SACSegmentation<pcl::PointXYZ> seg;
    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_PLANE);    //分割模型
    seg.setMethodType(pcl::SAC_RANSAC);       //随机参数估计方法
    seg.setMaxIterations(1000);               //最大的迭代的次数
    seg.setDistanceThreshold(Threshold);      //设置阀值
    seg.setInputCloud(cloud);
    seg.segment(*inliers, *coefficients);
}

void PExtracting(pcl::PointIndices::Ptr& inliers, pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_extracted, bool negative)
{
    //cloud_extracted->clear();
    pcl::ExtractIndices<pcl::PointXYZ> extract;
    extract.setInputCloud(cloud);
    extract.setIndices(inliers);
    extract.setNegative(negative);//如果设为true,可以提取指定index之外的点云
    extract.filter(*cloud_extracted);
}
void PGetLineInfo(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::ModelCoefficients::Ptr& coefficients)
{
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);  //inliers用来存储直线上点的索引
    pcl::SACSegmentation<pcl::PointXYZ> seg;//创建一个分割器
    seg.setOptimizeCoefficients(true);      //可选择配置，设置模型系数需要优化
    seg.setModelType(pcl::SACMODEL_LINE);   //设置目标几何形状
    seg.setMethodType(pcl::SAC_RANSAC);     //拟合方法：随机采样法
    seg.setDistanceThreshold(0.2);         //设置误差容忍范围，也就是阈值
    seg.setMaxIterations(500);              //最大迭代次数，不设置的话默认迭代50次
    seg.setInputCloud(cloud);               //输入点云
    seg.segment(*inliers, *coefficients);   //拟合点云

    pcl::PointCloud<pcl::PointXYZ>::Ptr line(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::ExtractIndices<pcl::PointXYZ> extract;  //创建点云提取对象
    extract.setInputCloud(cloud);    //设置输入点云
    extract.setIndices(inliers);     //设置分割后的内点为需要提取的点集
    extract.setNegative(false);      //false提取内点, true提取外点
    extract.filter(*line);        //提取输出存储到c_plane2

    pcl::io::savePCDFileBinary("output/line.pcd", *line);
}

void PparallelXYZ(pcl::ModelCoefficients::Ptr& coefficients, const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, const char ch, const pcl::PointCloud<pcl::PointXYZ>::Ptr& new_cloud, Eigen::Matrix4f& transToXoy)
{
    Eigen::Vector3f InPut_LineDir(coefficients->values[3], coefficients->values[4], coefficients->values[5]);
    Eigen::Vector3f Axis_LineDir;
    //false: 计算的夹角结果为弧度制，true：计算的夹角结果为角度制
    Eigen::Matrix4f T = Eigen::Matrix4f::Identity();

    double theta;

    if (ch == 'x')
    {
        Axis_LineDir.x() = 1;
        Axis_LineDir.y() = 0;
        Axis_LineDir.z() = 0;

        theta = pcl::getAngle3D(InPut_LineDir, Axis_LineDir, false);

        T(0, 0) = cos(theta);
        T(0, 1) = -sin(theta);
        T(1, 0) = sin(theta);
        T(1, 1) = cos(theta);
    }

    else if (ch == 'y')
    {
        Axis_LineDir.x() = 0;
        Axis_LineDir.y() = 1;
        Axis_LineDir.z() = 0;

        theta = pcl::getAngle3D(InPut_LineDir, Axis_LineDir, false);

        T(0, 0) = cos(theta);
        T(0, 1) = -sin(theta);
        T(1, 0) = sin(theta);
        T(1, 1) = cos(theta);
    }

    else if (ch == 'z')
    {
        Axis_LineDir.x() = 0;
        Axis_LineDir.y() = 0;
        Axis_LineDir.z() = 1;

        theta = pcl::getAngle3D(InPut_LineDir, Axis_LineDir, false);

        T(1, 1) = cos(theta);
        T(1, 2) = sin(theta);
        T(2, 1) = -sin(theta);
        T(2, 2) = cos(theta);
    }

    pcl::transformPointCloud(*cloud, *new_cloud, T);

    transToXoy = T;
}

void PAshape(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_hull)
{
    pcl::ConcaveHull<pcl::PointXYZ> chull;
    chull.setInputCloud(cloud); // 输入点云为投影后的点云
    chull.setAlpha(1.0);        // 设置alpha值为0.1
    chull.reconstruct(*cloud_hull);
}

void PRGrowingSw(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_new)
{
    //---------------------------------------法线和表面曲率估计---------------------------------------
    pcl::search::Search<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
    pcl::PointCloud <pcl::Normal>::Ptr normals(new pcl::PointCloud <pcl::Normal>);
    pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> n;
    n.setInputCloud(cloud);   // 设置法线估计对象输入点集
    n.setSearchMethod(tree);  // 设置搜索方法
    n.setNumberOfThreads(20);  // 设置openMP的线程数
    n.setKSearch(20);         // 设置用于法向量估计的k近邻数目
    n.compute(*normals);      // 计算并输出法向量

    //--------------------------------------------区域生长-------------------------------------------
    pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> reg;
    reg.setMinClusterSize(10);                         // 一个聚类需要的最小点数
    reg.setMaxClusterSize(1000000);                    // 一个聚类需要的最大点数
    reg.setSearchMethod(tree);                         // 搜索方法
    reg.setNumberOfNeighbours(20);                     // 搜索的邻域点的个数
    reg.setInputCloud(cloud);                          // 输入点云
    reg.setInputNormals(normals);                      // 输入的法线
    reg.setSmoothnessThreshold(pcl::deg2rad(50.0));     // 设置平滑阈值，即法向量夹角的阈值(这里的3.0是角度制)
    std::vector <pcl::PointIndices> clusters;
    reg.extract(clusters);                             // 获取聚类的结果，分割结果保存在点云索引的向量中。

    //----------------------------------------保存聚类的点云----------------------------------------
    for (std::vector<pcl::PointIndices>::const_iterator it = clusters.begin(); it != clusters.end(); ++it)
    {
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);

        for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); pit++)
            cloud_cluster->points.push_back(cloud->points[*pit]);

        if (cloud_cluster->points.size() > 0.4 * cloud->points.size())
        {
            cloud_new = cloud_cluster;
        }
    }
    cloud_new->width = 1;
    cloud_new->height = cloud_new->points.size();
}

void PRotateToXOY(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_rotated, pcl::ModelCoefficients::Ptr& coefficients, Eigen::Matrix4f& transToXoy)
{
    double A = -coefficients->values[0];
    double B = -coefficients->values[1];
    double C = -coefficients->values[2];
    double D = -coefficients->values[3];
    float theta;
    Eigen::Vector3f rotation_axis;
    Eigen::Vector3f RV;

    Eigen::Vector3f normPlane(A, B, C);
    Eigen::Vector3f ne(0, 0, 1);

    rotation_axis = normPlane.transpose().cross(ne);
    theta = acos(ne.dot(normPlane));
    RV = rotation_axis / rotation_axis.norm() * theta;
    Eigen::Vector3f RVdivAn = RV / theta;
    Eigen::MatrixXf dm3din(4, 3);
    dm3din << 1, 0, 0, 0, 1, 0, 0, 0, 1, RVdivAn(0), RVdivAn(1), RVdivAn(2);

    Eigen::Vector3f omega = RV / theta;
    Eigen::Matrix4f dm2dm3;

    Eigen::Vector3f temp = RV / pow(theta, 2);
    Eigen::Matrix3f RVdivTh2;
    RVdivTh2 << temp(0, 0), temp(0, 1), temp(0, 2), temp(1, 0), temp(1, 1), temp(1, 2), temp(2, 0), temp(2, 1), temp(2, 2);
    Eigen::Matrix3f idendivAn = Eigen::Matrix3f::Identity() / theta - RVdivTh2;

    dm2dm3 << idendivAn(2, 2), 0, 0, idendivAn(0, 2),
        0, idendivAn(2, 2), 0, idendivAn(1, 2),
        0, 0, idendivAn(2, 2), 0,
        0, 0, 0, 1;
    float alpha = cos(theta);
    float beta = sin(theta);
    float gamma = 1 - cos(theta);
    Eigen::Matrix3f omegav;
    omegav << 0, -omega(2), omega(1), omega(2), 0, -omega(0), -omega(1), omega(0), 0;
    Eigen::Matrix3f AA = omega * omega.transpose();

    Eigen::MatrixXf dm1dm2(21, 4);
    dm1dm2 = Eigen::MatrixXf::Zero(21, 4);
    dm1dm2(0, 3) = -sin(theta);
    dm1dm2(1, 3) = cos(theta);
    dm1dm2(2, 3) = sin(theta);
    dm1dm2(8, 0) = 1;
    dm1dm2(11, 0) = -1;
    dm1dm2(5, 1) = -1;
    dm1dm2(9, 1) = 1;
    dm1dm2(4, 2) = 1;
    dm1dm2(6, 2) = -1;

    float w1 = omega(0);
    float w2 = omega(1);
    float w3 = omega(2);
    dm1dm2(12, 0) = 2 * w1;
    dm1dm2(13, 0) = w2;
    dm1dm2(14, 0) = w3;
    dm1dm2(15, 0) = w2;
    dm1dm2(16, 0) = 0;
    dm1dm2(17, 0) = 0;
    dm1dm2(18, 0) = w3;
    dm1dm2(19, 0) = 0;
    dm1dm2(20, 0) = 0;

    dm1dm2(12, 1) = 0;
    dm1dm2(13, 1) = w1;
    dm1dm2(14, 1) = 0;
    dm1dm2(15, 1) = w1;
    dm1dm2(16, 1) = 2 * w2;
    dm1dm2(17, 1) = w3;
    dm1dm2(18, 1) = 0;
    dm1dm2(19, 1) = w3;
    dm1dm2(20, 1) = 0;

    dm1dm2(12, 2) = 0;
    dm1dm2(13, 2) = 0;
    dm1dm2(14, 2) = w1;
    dm1dm2(15, 2) = 0;
    dm1dm2(16, 2) = 0;
    dm1dm2(17, 2) = w2;
    dm1dm2(18, 2) = w1;
    dm1dm2(19, 2) = w2;
    dm1dm2(20, 2) = 2 * w3;

    Eigen::Matrix3f R = Eigen::Matrix3f::Identity() * alpha + omegav * beta + AA * gamma;
    transToXoy << R(0, 0), R(0, 1), R(0, 2), 0,
        R(1, 0), R(1, 1), R(1, 2), 0,
        R(2, 0), R(2, 1), R(2, 2), 0,
        0, 0, 0, 1;
    Eigen::Matrix4f transPan = Eigen::Matrix4f::Identity();
    Eigen::Vector4f tempVec4(0, 0, D / C, 1);
    tempVec4 = transToXoy * tempVec4;
    transPan(2, 3) = tempVec4[2]; // 旋转后平面：z = offSetZ

    transToXoy = transPan * transToXoy;

    pcl::transformPointCloud(*cloud, *cloud_rotated, transToXoy);
}

void PCutStr(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& cloud_Cut, std::string coor_axis, double thick)
{
    for (double i = 0.0; i < 1.0; i += thick)
    {
        pcl::PointCloud<pcl::PointXYZ>::Ptr cutcloud(new pcl::PointCloud<pcl::PointXYZ>);
        PStrThrough(cloud, cutcloud, coor_axis, i, i + thick);
        cloud_Cut.push_back(cutcloud);
    }
}

void PStrThrough(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_Straight_filtered, std::string axis_dir, double start_radio, double end_radio)
{
    double start_range;
    double end_range;
    pcl::PointXYZ minPt, maxPt;
    pcl::PassThrough<pcl::PointXYZ> s_t;
    s_t.setInputCloud(cloud);
    s_t.setFilterFieldName(axis_dir); //滤波字段名被设置为Z轴方向
    pcl::getMinMax3D(*cloud, minPt, maxPt);

    if (axis_dir == "x")
    {
        start_range = start_radio * (maxPt.x - minPt.x);
        end_range = end_radio * (maxPt.x - minPt.x);
        s_t.setFilterLimits(minPt.x + start_range, minPt.x + end_range); //设置在过滤方向上的过滤范围
    }
    else if (axis_dir == "y")
    {
        start_range = start_radio * (maxPt.y - minPt.y);
        end_range = end_radio * (maxPt.y - minPt.y);
        s_t.setFilterLimits(minPt.y + start_range, minPt.y + end_range); //设置在过滤方向上的过滤范围
    }
    else if (axis_dir == "z")
    {
        start_range = start_radio * (maxPt.z - minPt.z);
        end_range = end_radio * (maxPt.z - minPt.z);
        s_t.setFilterLimits(minPt.z + start_range, minPt.z + end_range); //设置在过滤方向上的过滤范围
    }
   
    s_t.setNegative(false); //设置保留范围内的点还是过滤掉范围内的点，标志为false时保留范围内的点
    s_t.filter(*cloud_Straight_filtered);
}

bool PsortByPointSize(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud1, pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud2)
{
    pcl::PointXYZ pmin, pmax;
    pcl::PointXYZ pmin1, pmax1;
    auto ditsance1 = pcl::getMaxSegment(*cloud1, pmin, pmax);
    auto ditsance2 = pcl::getMaxSegment(*cloud2, pmin1, pmax1);
    return ditsance1 > ditsance2;

}

void PMultipleLines(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& cloud_Cut, int n)
{
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices());
    pcl::SACSegmentation<pcl::PointXYZ> seg;   
    seg.setOptimizeCoefficients(true);          
    seg.setModelType(pcl::SACMODEL_LINE);                  
    seg.setMethodType(pcl::SAC_RANSAC);               
    seg.setMaxIterations(1000);                         
    seg.setDistanceThreshold(2);                      

    int i = 0, nr_point = cloud->points.size();
    int k = 0;
    while (k < n && cloud->points.size() > 0.1 * nr_point)// 从0循环到5执行6次，并且每次cloud的点数必须要大于原始总点数的0.1倍
    {
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_line(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::PointCloud<pcl::PointXYZ>::Ptr outside(new pcl::PointCloud<pcl::PointXYZ>);

        pcl::ModelCoefficients coefficients;
        seg.setInputCloud(cloud);                     					 
        seg.segment(*inliers, coefficients);             

        pcl::ExtractIndices<pcl::PointXYZ> extract;   
        extract.setInputCloud(cloud);
        extract.setIndices(inliers);
        extract.setNegative(false);                   
        extract.filter(*cloud_line);
        cloud_Cut.push_back(cloud_line);

        extract.setNegative(true);                    
        extract.filter(*outside);                     
        cloud.swap(outside);                          
        i++;
        k++;
    }
}

Fliter.h

#pragma once
#include <Eigen/Core>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/common/angles.h>
#include <pcl/common/transforms.h>
#include <pcl/common/transforms.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/passthrough.h>
#include <pcl/surface/concave_hull.h>
#include <pcl/features/normal_3d_omp.h>	
#include <pcl/filters/extract_indices.h>	
#include <pcl/filters/project_inliers.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>

void PVoxlGrid(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& new_cloud);

void PGetMaxPlaneCoe(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::ModelCoefficients::Ptr& coefficients, pcl::PointIndices::Ptr& inliers, double Threshold);

void PExtracting(pcl::PointIndices::Ptr& inliers, pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_extracted, bool negative);

void PGetLineInfo(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::ModelCoefficients::Ptr& coefficients);

void PparallelXYZ(pcl::ModelCoefficients::Ptr& coefficients, const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, const char ch, const pcl::PointCloud<pcl::PointXYZ>::Ptr& new_cloud, Eigen::Matrix4f& transToXoy);

void PAshape(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_hull);

void PRGrowingSw(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_new);

void PRotateToXOY(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_rotated, pcl::ModelCoefficients::Ptr& coefficients, Eigen::Matrix4f& transToXoy);

void PCutStr(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& cloud_Cut, std::string coor_axis, double thick);//k is the saved cut number

void PStrThrough(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_Straight_filtered, std::string axis_dir, double start_radio, double end_radio);

bool PsortByPointSize(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud1, pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud2);

void PMultipleLines(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& cloud_Cut, int n);

迅卓科技

关注

7
点赞
踩
17

收藏

觉得还不错? 一键收藏
打赏
5
评论
隧道点云轨道提取【C++PCL】

函数用于通过随机采样一致性（RANSAC）算法从点云中估计最大平面模型的系数和内点索引。函数通过迭代地估计平面模型的系数和提取平面点云，直到找到一个与参考法线角度较小的平面。函数用于将点云按照给定的平面模型系数进行旋转，以将该平面旋转到XOY平面。函数使用法线估计和区域生长算法对点云进行聚类，将聚类结果保存在。函数根据给定的内点索引，从点云中提取或排除相应的点。
复制链接

扫一扫