当前位置： 首页 > news >正文

PCL统计点云Volume

news 2025/8/5 12:08:20

1. 指定参考平面；

2. 将原始点云变换到已参考平面作为xOy平面的坐标系下；

3. 对投影到参考平面的2D点云进行三角化，高度为三角形各顶点到平面距离的平均值；

4. 累加所有三棱柱体积作为点云Volume

Coord2DTransformer.h

#pragma once
#include<vector>
#include<glm/glm.hpp>
#include<ransac_plane.h>
#include<Eigen/Dense>
#include"common.h"
#include<pcl/point_cloud.h>
#include <pcl/common/common.h>// 返回值为角度值
double angle_between_vectors(const glm::dvec3& v1, const glm::dvec3& v2);float angleBetweenVectors(const Eigen::Vector3f& u, const Eigen::Vector3f& v, bool in_degrees = false);
class Coord2DTransformer
{
public:pcl::PointCloud<pcl::PointXYZ>::Ptr rectify(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, Eigen::Vector4f plane, const Eigen::Vector3f& centroid);void SetData(std::vector<Point3d>& input, const std::vector<int>& indices = std::vector<int>()){std::vector<Point3d> srcPoints;if (indices.empty()) {srcPoints = input;}else {srcPoints.resize(indices.size());for (int i = 0; i < indices.size(); i++) {srcPoints[i] = input[indices[i]];}}// fit_plane参见博客《CGAL散乱点拟合最小二乘平面（3D平面拟合，基于Eigen）》// https://blog.csdn.net/xmyzqs1212/article/details/142742841int minInliers = std::floor(srcPoints.size() * 0.66666);Plane3D plane = parallelRansacFitPlane(srcPoints, 10000, 0.1, minInliers, 4, 0.8).param;centroid = calculateCenter(srcPoints);// 使用并行RANSAC拟合平面//PlaneModel plane = parallelRansacFitPlane(points, 10000, 0.1, 300, 4, 0.8);Vec3d norm = plane.normal();Vec3d arbitrary(1, 0, 0);auto angle = angle_between_vectors(norm, arbitrary);// 避免任意选取的向量与平面法向量方向一致（方向一致没办法通过向量积计算正交向量）if (angle < 1.0 || angle > 179) {arbitrary = Vec3d(0, 1, 0);}basis1 = glm::normalize(glm::cross(norm, arbitrary));basis2 = glm::normalize(glm::cross(norm, basis1));points2d.resize(srcPoints.size());for (int i = 0; i < srcPoints.size(); i++){Vec3d v = srcPoints[i] - centroid;points2d[i][0] = glm::dot(v, basis1);points2d[i][1] = glm::dot(v, basis2);}}Point3d calcCoord3D(double x, double y){auto v = x * basis1 + y * basis2;return Point3d(centroid.x + v.x, centroid.y + v.y, centroid.z + v.z);}Point3d calcCoord3D(const Point2d& pt){return calcCoord3D(pt.x, pt.y);}void calcCoord3D(const std::vector<Vec2d>& pts2d, std::vector<Point3d>& result){result.resize(pts2d.size());for (int i = 0; i < pts2d.size(); i++){result[i] = calcCoord3D(pts2d[i][0], pts2d[i][1]);}}std::vector<Point2d> points2d;Vec3d basis1;Vec3d basis2;Point3d centroid;
};

Coord2DTransformer.cpp

#include "Coord2DTransformer.h"
#include<glm/ext.hpp>double angle_between_vectors(const glm::dvec3& v1, const glm::dvec3& v2)
{double dot_product = glm::dot(glm::normalize(v1), glm::normalize(v2));double magnitude_v1 = glm::length(v1);double magnitude_v2 = glm::length(v2);double cos_angle = dot_product / (magnitude_v1 * magnitude_v2);// 确保cos_angle在[-1, 1]范围内，避免浮点数误差cos_angle = std::max(-1.0, std::min(1.0, cos_angle));return std::acos(cos_angle) / glm::pi<double>() * 180.0; // 返回角度
}float angleBetweenVectors(const Eigen::Vector3f& u, const Eigen::Vector3f& v, bool in_degrees/* = false*/) {// 计算点积float dot_product = u.dot(v);// 计算向量长度float norm_u = u.norm();float norm_v = v.norm();// 避免除以零（如果任一向量是零向量，返回 NaN）if (norm_u == 0 || norm_v == 0) {return std::numeric_limits<float>::quiet_NaN();}// 计算夹角的余弦值（限制在 [-1, 1] 范围内，避免数值误差）float cos_theta = dot_product / (norm_u * norm_v);cos_theta = std::clamp(cos_theta, -1.0f, 1.0f);// 计算弧度角float angle_rad = std::acos(cos_theta);// 转换为度数（可选）if (in_degrees) {return angle_rad * (180.0f / static_cast<float>(M_PI));}return angle_rad;
}pcl::PointCloud<pcl::PointXYZ>::Ptr Coord2DTransformer::rectify(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, Eigen::Vector4f plane, const Eigen::Vector3f& centroid)
{//PlaneModel plane = parallelRansacFitPlane(points, 10000, 0.1, 300, 4, 0.8);Eigen::Vector3f norm_eigen = plane.head<3>();norm_eigen.normalize();glm::dvec3 norm(norm_eigen[0], norm_eigen[1], norm_eigen[2]);glm::dvec3 arbitrary(1, 0, 0);auto angle = angle_between_vectors(norm, arbitrary);// 避免任意选取的向量与平面法向量方向一致（方向一致没办法通过向量积计算正交向量）if (angle < 1.0 || angle > 179) {arbitrary = glm::dvec3(0, 1, 0);}this->centroid = Point3d(centroid[0], centroid[1], centroid[2]);basis1 = glm::normalize(glm::cross(norm, arbitrary));basis2 = glm::normalize(glm::cross(norm, basis1));pcl::PointCloud<pcl::PointXYZ>::Ptr result(new pcl::PointCloud<pcl::PointXYZ>);result->width = cloud->size();result->height = 1;result->is_dense = true;result->points.resize(cloud->size());//points2d.resize(cloud->points.size());
#pragma omp parallel forfor (int i = 0; i < cloud->size(); i++){glm::dvec3 tmpPt(cloud->points[i].x, cloud->points[i].y, cloud->points[i].z);Vec3d v = tmpPt - this->centroid;result->points[i].x = glm::dot(v, basis1);result->points[i].y = glm::dot(v, basis2);result->points[i].z = glm::dot(v, norm);}return result;
}

main.cpp

#include <iostream>
#include <fstream>
#include <sstream>
#include <string>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h> // 用于保存点云（可选）
#include<pcl/io/ply_io.h>
#include <pcl/console/print.h>  // 用于调试输出
//#include <pcl/gpu/containers/initialization.h>
//#include <pcl/gpu/features/features.hpp>
#include <pcl/common/centroid.h>
#include <pcl/common/common.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_sphere.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/segmentation/conditional_euclidean_clustering.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/statistical_outlier_removal.h> // 包含StatisticalOutlierRemoval头文件
#include <pcl/filters/random_sample.h> // 包含RandomSample头文件
#include"Coord2DTransformer.h"
#include <pcl/visualization/pcl_visualizer.h>#include<opencv2/opencv.hpp>#include <chrono>
#include <iomanip>  // 用于格式化输出
#include <ctime>    // 用于将时间转换为可读格式
#include<thread>#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
#include <CGAL/Delaunay_triangulation_2.h>typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef CGAL::Delaunay_triangulation_2<K> Delaunay;
typedef K::Point_2 Point_2;// 读取.xyz文件并存储到pcl::PointCloud<pcl::PointXYZ>::Ptr
pcl::PointCloud<pcl::PointXYZ>::Ptr readXYZFile(const std::string& filename) {// 创建点云对象pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);// 打开文件std::ifstream file(filename);if (!file.is_open()) {std::cerr << "Error: Could not open file " << filename << std::endl;return cloud;}std::string line;while (std::getline(file, line)) {std::istringstream iss(line);std::string token;pcl::PointXYZ point;// 读取X坐标if (std::getline(iss, token, ';')) {point.x = std::stof(token);}else {std::cerr << "Error: Invalid X coordinate in line: " << line << std::endl;continue;}// 读取Y坐标if (std::getline(iss, token, ';')) {point.y = std::stof(token);}else {std::cerr << "Error: Invalid Y coordinate in line: " << line << std::endl;continue;}// 读取Z坐标if (std::getline(iss, token, ';')) {point.z = std::stof(token);}else {std::cerr << "Error: Invalid Z coordinate in line: " << line << std::endl;continue;}// 将点添加到点云中cloud->push_back(point);}file.close();return cloud;
}void print_time(const std::string& str)
{// 获取当前时间点auto now = std::chrono::system_clock::now();// 转换为时间戳（精确到毫秒）auto milliseconds = std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()).count();// 获取当前时间的秒级表示auto now_time_t = std::chrono::system_clock::to_time_t(now);std::tm* now_tm = std::localtime(&now_time_t);// 打印时间（精确到毫秒）std::cout << std::put_time(now_tm, "%Y-%m-%d %H:%M:%S") << "." << std::setw(3) << std::setfill('0') << (milliseconds % 1000);printf("   %s\n", str.c_str());
}// 自定义函数：生成随机颜色
pcl::RGB getRandomColor()
{pcl::RGB color;color.r = rand() % 256;color.g = rand() % 256;color.b = rand() % 256;return color;
}Eigen::Vector3f computeCentroidPCL(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud) {Eigen::Vector4f centroid_homogeneous; // 四维齐次坐标 (x, y, z, 1)pcl::compute3DCentroid(*cloud, centroid_homogeneous);return centroid_homogeneous.head<3>(); // 返回前三维 (x, y, z)
}// 哈希函数用于 Point_2
struct Point2Hash {size_t operator()(const Point_2& p) const {size_t h1 = std::hash<double>()(CGAL::to_double(p.x()));size_t h2 = std::hash<double>()(CGAL::to_double(p.y()));return h1 ^ (h2 << 1);}
};// 判断两个 Point_2 是否相等
struct Point2Equal {bool operator()(const Point_2& p1, const Point_2& p2) const {return CGAL::to_double(p1.x()) == CGAL::to_double(p2.x()) &&CGAL::to_double(p1.y()) == CGAL::to_double(p2.y());}
};double computeVolumeFromHeightMapOptimized(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud) {if (cloud->empty()) return 0.0;// 1. 构建哈希表（XY坐标到Z值的映射）std::unordered_map<Point_2, double, Point2Hash, Point2Equal> xy_to_z;
#pragma omp parallel forfor (size_t i = 0; i < cloud->size(); ++i) {const auto& point = (*cloud)[i];Point_2 p2(point.x, point.y);
#pragma omp criticalxy_to_z[p2] = point.z;  // 假设每个XY坐标唯一}// 2. Delaunay 三角剖分（单线程，CGAL不支持并行）std::vector<Point_2> points_2d;for (const auto& point : *cloud) {points_2d.push_back(Point_2(point.x, point.y));}print_time("开始Delaunay三角剖分");Delaunay dt(points_2d.begin(), points_2d.end());print_time("Delaunay三角剖分 done");// 3. 并行计算每个三角面片的体积double total_volume = 0.0;
//#pragma omp parallel for reduction(+:total_volume)for (auto face = dt.finite_faces_begin(); face != dt.finite_faces_end(); ++face) {// 获取三角面片的三个顶点auto p1 = face->vertex(0)->point();auto p2 = face->vertex(1)->point();auto p3 = face->vertex(2)->point();// 从哈希表快速查找Z值（无需遍历点云）double z1 = xy_to_z[p1];double z2 = xy_to_z[p2];double z3 = xy_to_z[p3];// 计算三角面片的面积（2D）double area = CGAL::area(p1, p2, p3);// 体积 = 面积 × 平均高度total_volume += area * ((z1 + z2 + z3) / 3.0);}return total_volume;
}bool extractPlane(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, Eigen::Vector4f& plane_coeff, Eigen::Vector3f& centroid, int sampleCnt = 100000)
{//pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);//// 数量降到足够少，然后RANSAC提取底部平面//pcl::RandomSample<pcl::PointXYZ> rs;//rs.setInputCloud(cloud);//rs.setSample(sampleCnt); // 设置采样点数//rs.filter(*cloud_filtered);centroid = computeCentroidPCL(cloud);//// 创建统计离群值去除滤波器对象//pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;//sor.setInputCloud(cloud);  // 设置输入点云//sor.setMeanK(20);          // 设置用于计算平均距离的最近邻点数//sor.setStddevMulThresh(1.0); // 设置标准差倍数阈值，这里设置为1.0//sor.filter(*cloud); // 执行滤波操作// 创建分割对象pcl::SACSegmentation<pcl::PointXYZ> seg;pcl::PointIndices::Ptr inliers(new pcl::PointIndices); // 平面内点索引pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); // 平面模型系数// 设置分割参数seg.setOptimizeCoefficients(true); // 优化模型系数seg.setModelType(pcl::SACMODEL_PLANE); // 模型类型为平面seg.setMethodType(pcl::SAC_RANSAC); // 使用 RANSAC 方法seg.setMaxIterations(1000); // 最大迭代次数seg.setDistanceThreshold(0.2); // 距离阈值，用于判断点是否为内点// 设置输入点云seg.setInputCloud(cloud);// 执行分割seg.segment(*inliers, *coefficients);if (inliers->indices.empty()) {std::cerr << "Error: Could not estimate a planar model for the given dataset." << std::endl;return false;}// 提取平面点云pcl::PointCloud<pcl::PointXYZ>::Ptr plane_cloud(new pcl::PointCloud<pcl::PointXYZ>);pcl::ExtractIndices<pcl::PointXYZ> extract;extract.setInputCloud(cloud);extract.setIndices(inliers);extract.setNegative(false); // 提取内点extract.filter(*plane_cloud);//// 保存提取的平面点云//pcl::io::savePCDFile("plane_cloud.pcd", *plane_cloud);//std::cout << "Plane point cloud saved to plane_cloud.pcd" << std::endl;plane_coeff = Eigen::Map<const Eigen::Vector4f>(coefficients->values.data());print_time("extractPlane done");return true;
}// 保存Mat到二值文件
void saveMatInBinaryFloat(const cv::Mat& mat, std::string filename)
{FILE* saveF;saveF = fopen(filename.c_str(), "wb");if (saveF == NULL){std::printf("保存Mat到二值文件出错!\n");return;}int width = mat.cols;int height = mat.rows;int nChannels = mat.channels();printf("%s  -- %d %d %d\n", filename.c_str(), width, height, nChannels);fwrite(&width, sizeof(int), 1, saveF);fwrite(&height, sizeof(int), 1, saveF);fwrite(&nChannels, sizeof(int), 1, saveF);int cnt = width * height * nChannels;float* data = (float*)mat.data;for (int i = 0; i < cnt; i++)fwrite(&data[i], sizeof(float), 1, saveF);fclose(saveF);
}/*** @brief 读取 ASCII 格式的 .xyz 文件（包含 XYZ 和 RGB 值），并提取 XYZ 坐标* @param filename 输入文件名（.xyz）* @param cloud 输出的点云（仅 XYZ 坐标）* @return 是否成功读取*/
bool readXYZFile(const std::string& filename, pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud)
{// 打开文件std::ifstream file(filename);if (!file.is_open()){std::cerr << "Error: Could not open file " << filename << std::endl;return false;}// 清空并初始化点云cloud->clear();cloud->width = 0;cloud->height = 1;  // 无序点云cloud->is_dense = true;  // 假设没有 NaN 或 Inf 值std::string line;while (std::getline(file, line)){// 跳过空行和注释行（可选）if (line.empty() || line[0] == '#')continue;std::istringstream iss(line);pcl::PointXYZ point;// 读取 XYZ 坐标（前 3 个值）if (!(iss >> point.x >> point.y >> point.z)){std::cerr << "Warning: Invalid line format: " << line << std::endl;continue;}// 忽略 RGB 值（后 3 个值）float r, g, b;iss >> r >> g >> b;  // 读取但不存储// 添加到点云cloud->points.push_back(point);}// 更新点云宽度（点数）cloud->width = cloud->points.size();file.close();return true;
}int main() {//  // 读取.xyz文件std::string filename = "q.xyz";const std::string file_name = "q.ply";////std::string filename = "q.xyz";//pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2 = std::make_shared<pcl::PointCloud<pcl::PointXYZ>>();//  readXYZFile(filename, cloud2);//  print_time("load data done");//  if (pcl::io::savePLYFileBinary(file_name, *cloud2) != 0)//  {//    PCL_ERROR("Failed to save ply!\n");//    return -1;//  }//  //std::cout << "Saved " << cloud->size() << " points to " << file_name << std::endl;//  print_time("save done");//------------------------------------------------------// 3. 重新读入//------------------------------------------------------pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);pcl::PLYReader reader;//reader.setVerbosity(true);  // 启用详细日志if (reader.read(file_name, *cloud) == -1) {pcl::console::print_error("Failed to load PLY file!\n");return -1;}//std::cout << "Loaded " << cloud->size() << " points from " << file_name << std::endl;print_time("load data done");Eigen::Vector4f plane_coeff;Eigen::Vector3f centroid;if (!extractPlane(cloud, plane_coeff, centroid)){printf("Plane extract failed!\n");return -1;}//erasePtsByDistToPlane(cloud, plane_coeff);Coord2DTransformer transformer;auto transformed_cloud = transformer.rectify(cloud, plane_coeff, centroid);print_time("rectify done");auto volume = computeVolumeFromHeightMapOptimized(transformed_cloud);printf("Volume of the point cloud: %.2f cubic meters\n", volume);print_time("compute volume done");return 0;
}

http://www.xdnf.cn/news/1242181.html

相关文章：

  • ArcGIS的字段计算器生成随机数
  • 配置Mybatis环境
  • 【多智能体cooragent】CoorAgent 系统中 5 个核心系统组件分析
  • 一起学springAI系列一：流式返回
  • 【实战】Dify从0到100进阶--中药科普助手（1）
  • 嵌入式硬件中三极管原理分析与控制详解
  • 零售消费行业研究系列报告
  • 微帧GPU视频硬编优化引擎：面向人工智能大时代的AI算法与硬编协同优化方案
  • [特殊字符]️ 整个键盘控制无人机系统框架
  • 【AI 加持下的 Python 编程实战 2_13】第九章：繁琐任务的自动化（中）——自动批量合并 PDF 文档
  • 【银河麒麟服务器系统】自定义ISO镜像更新内核版本
  • 数据结构---配置网络步骤、单向链表额外应用
  • 从物理扇区到路径访问：Linux文件抽象的全景解析
  • 深入剖析RT-Thread串口驱动：基于STM32H750的FinSH Shell全链路Trace分析与实战解密(上)
  • 深度学习TR3周：Pytorch复现Transformer
  • OpenCV轻松入门_面向python（第二章图像处理基础）
  • JS--获取事件的子元素与父元素
  • 思途Spring学习 0804
  • 【Keras学习笔记】手写识别
  • C++-异常
  • Linux84 SHELL编程:流程控制 前瞻（1）
  • 贯穿全生命周期，生成式AI正在重塑游戏行业
  • Coze Loop：开源智能体自动化流程编排平台原理与实践
  • k8s集群
  • 案件线索展示与交付项目
  • 数据结构：如何判断一个链表中是否存在环（Check for LOOP in Linked List）
  • 深度学习图像处理篇之AlexNet模型详解
  • 【PHP】对比两张图片的相似度
  • WPF 按钮背景色渐变
  • 服务器的Mysql 集群技术