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/142742841
int 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 for
for (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 for
for (size_t i = 0; i < cloud->size(); ++i) {
const auto& point = (*cloud)[i];
Point_2 p2(point.x, point.y);
#pragma omp critical
xy_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;
}