点云数据曲率计算算法实现
1. 基础数据结构和数学库
1.1 点云数据结构 (point_cloud.h)
#ifndef POINT_CLOUD_H
#define POINT_CLOUD_H
#include <vector>
#include <Eigen/Dense>
#include <memory>
#include <queue>
#include <cmath>
// 点结构体
struct Point3D {
float x, y, z;
float nx, ny, nz; // 法向量
float curvature; // 曲率
float principal_k1; // 主曲率1
float principal_k2; // 主曲率2
float gaussian_k; // 高斯曲率
float mean_k; // 平均曲率
Point3D() : x(0), y(0), z(0),
nx(0), ny(0), nz(0),
curvature(0), principal_k1(0), principal_k2(0),
gaussian_k(0), mean_k(0) {}
Point3D(float x_, float y_, float z_) :
x(x_), y(y_), z(z_),
nx(0), ny(0), nz(0),
curvature(0), principal_k1(0), principal_k2(0),
gaussian_k(0), mean_k(0) {}
// 转换为Eigen向量
Eigen::Vector3f toEigen() const {
return Eigen::Vector3f(x, y, z);
}
// 计算两点间距离
float distanceTo(const Point3D& p) const {
float dx = x - p.x;
float dy = y - p.y;
float dz = z - p.z;
return sqrt(dx*dx + dy*dy + dz*dz);
}
};
// 点云类
class PointCloud {
private:
std::vector<Point3D> points;
std::shared_ptr<std::vector<std::vector<int>>> neighbors; // 邻域索引
public:
PointCloud() = default;
// 添加点
void addPoint(const Point3D& p) { points.push_back(p); }
void addPoint(float x, float y, float z) { points.emplace_back(x, y, z); }
// 获取点
const std::vector<Point3D>& getPoints() const { return points; }
std::vector<Point3D>& getPoints() { return points; }
// 获取点数量
size_t size() const { return points.size(); }
// 清空
void clear() { points.clear(); }
// 获取点的Eigen矩阵表示
Eigen::MatrixXf toMatrix() const {
Eigen::MatrixXf mat(points.size(), 3);
for (size_t i = 0; i < points.size(); i++) {
mat(i, 0) = points[i].x;
mat(i, 1) = points[i].y;
mat(i, 2) = points[i].z;
}
return mat;
}
// 计算包围盒
void computeBoundingBox(Eigen::Vector3f& min_pt, Eigen::Vector3f& max_pt) const {
if (points.empty()) return;
min_pt = Eigen::Vector3f(points[0].x, points[0].y, points[0].z);
max_pt = min_pt;
for (const auto& p : points) {
min_pt[0] = std::min(min_pt[0], p.x);
min_pt[1] = std::min(min_pt[1], p.y);
min_pt[2] = std::min(min_pt[2], p.z);
max_pt[0] = std::max(max_pt[0], p.x);
max_pt[1] = std::max(max_pt[1], p.y);
max_pt[2] = std::max(max_pt[2], p.z);
}
}
};
1.2 数学工具类 (math_utils.h)
#ifndef MATH_UTILS_H
#define MATH_UTILS_H
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <vector>
#include <cmath>
namespace MathUtils {
// 计算协方差矩阵
inline Eigen::Matrix3f computeCovarianceMatrix(const std::vector<Eigen::Vector3f>& points,
const Eigen::Vector3f& centroid) {
Eigen::Matrix3f covariance = Eigen::Matrix3f::Zero();
for (const auto& p : points) {
Eigen::Vector3f diff = p - centroid;
covariance += diff * diff.transpose();
}
if (points.size() > 0) {
covariance /= static_cast<float>(points.size());
}
return covariance;
}
// 计算质心
inline Eigen::Vector3f computeCentroid(const std::vector<Eigen::Vector3f>& points) {
if (points.empty()) return Eigen::Vector3f::Zero();
Eigen::Vector3f centroid = Eigen::Vector3f::Zero();
for (const auto& p : points) {
centroid += p;
}
centroid /= static_cast<float>(points.size());
return centroid;
}
// 计算质心（从Point3D）
inline Eigen::Vector3f computeCentroid(const std::vector<Point3D>& points) {
if (points.empty()) return Eigen::Vector3f::Zero();
Eigen::Vector3f centroid = Eigen::Vector3f::Zero();
for (const auto& p : points) {
centroid[0] += p.x;
centroid[1] += p.y;
centroid[2] += p.z;
}
centroid /= static_cast<float>(points.size());
return centroid;
}
// PCA主成分分析
inline bool computePCA(const Eigen::Matrix3f& covariance,
Eigen::Vector3f& eigenvalues,
Eigen::Matrix3f& eigenvectors) {
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> solver(covariance);
if (solver.info() != Eigen::Success) {
return false;
}
eigenvalues = solver.eigenvalues();
eigenvectors = solver.eigenvectors();
// 确保特征值从小到大排序
// Eigen默认从大到小，我们需要从小到大
for (int i = 0; i < 2; i++) {
for (int j = i + 1; j < 3; j++) {
if (eigenvalues[i] > eigenvalues[j]) {
std::swap(eigenvalues[i], eigenvalues[j]);
eigenvectors.col(i).swap(eigenvectors.col(j));
}
}
}
return true;
}
// 计算曲面的最小二乘拟合平面
inline bool fitPlane(const std::vector<Eigen::Vector3f>& points,
Eigen::Vector3f& normal,
Eigen::Vector3f& centroid) {
if (points.size() < 3) return false;
centroid = computeCentroid(points);
Eigen::Matrix3f covariance = computeCovarianceMatrix(points, centroid);
// PCA找到最小特征值对应的特征向量（法向量）
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> solver(covariance);
if (solver.info() != Eigen::Success) {
return false;
}
// 最小特征值对应的特征向量
normal = solver.eigenvectors().col(0);
// 确保法向量指向正确方向（可以统一指向视点）
return true;
}
// 计算向量夹角
inline float angleBetween(const Eigen::Vector3f& v1, const Eigen::Vector3f& v2) {
float cos_angle = v1.dot(v2) / (v1.norm() * v2.norm());
cos_angle = std::max(-1.0f, std::min(1.0f, cos_angle));
return std::acos(cos_angle);
}
};
2. 曲率计算核心算法
2.1 基于PCA的曲率计算 (pca_curvature.h)
#ifndef PCA_CURVATURE_H
#define PCA_CURVATURE_H
#include "point_cloud.h"
#include "math_utils.h"
class PCACurvature {
private:
// 邻域搜索方法
enum NeighborhoodType {
K_NEAREST, // K最近邻
RADIUS_SEARCH // 半径搜索
};
public:
// 计算点云的曲率
static bool compute(PointCloud& cloud,
int k_neighbors = 30,
float radius = 0.0f,
NeighborhoodType type = K_NEAREST) {
if (cloud.size() < k_neighbors) {
std::cerr << "点数量少于K近邻数" << std::endl;
return false;
}
// 建立KD树加速搜索
KdTree kdtree(cloud);
for (size_t i = 0; i < cloud.size(); i++) {
std::vector<int> neighbor_indices;
std::vector<float> distances;
if (type == K_NEAREST) {
kdtree.knnSearch(cloud.getPoints()[i], k_neighbors,
neighbor_indices, distances);
} else {
kdtree.radiusSearch(cloud.getPoints()[i], radius,
neighbor_indices, distances);
}
if (neighbor_indices.size() < 3) {
cloud.getPoints()[i].curvature = 0;
continue;
}
// 计算PCA曲率
float curvature = computePCACurvature(cloud, neighbor_indices, i);
cloud.getPoints()[i].curvature = curvature;
}
return true;
}
private:
// 基于PCA的曲率计算
static float computePCACurvature(const PointCloud& cloud,
const std::vector<int>& neighbor_indices,
int point_idx) {
std::vector<Eigen::Vector3f> points;
points.reserve(neighbor_indices.size());
// 包含中心点
const Point3D& center = cloud.getPoints()[point_idx];
points.push_back(center.toEigen());
for (int idx : neighbor_indices) {
if (idx != point_idx) { // 不重复添加中心点
const Point3D& p = cloud.getPoints()[idx];
points.push_back(p.toEigen());
}
}
if (points.size() < 4) return 0.0f;
// 计算质心
Eigen::Vector3f centroid = MathUtils::computeCentroid(points);
// 计算协方差矩阵
Eigen::Matrix3f covariance = MathUtils::computeCovarianceMatrix(points, centroid);
// PCA分解
Eigen::Vector3f eigenvalues;
Eigen::Matrix3f eigenvectors;
if (!MathUtils::computePCA(covariance, eigenvalues, eigenvectors)) {
return 0.0f;
}
// 计算曲率：最小特征值 / 特征值之和
float sum_eigenvalues = eigenvalues.sum();
if (sum_eigenvalues < 1e-9f) return 0.0f;
float curvature = eigenvalues[0] / sum_eigenvalues;
// 保存法向量（最小特征值对应的特征向量）
Eigen::Vector3f normal = eigenvectors.col(0);
cloud.getPoints()[point_idx].nx = normal[0];
cloud.getPoints()[point_idx].ny = normal[1];
cloud.getPoints()[point_idx].nz = normal[2];
return curvature;
}
// 计算主曲率和高斯曲率
static bool computePrincipalCurvatures(const PointCloud& cloud,
const std::vector<int>& neighbor_indices,
int point_idx,
float& k1, float& k2,
float& gaussian_k, float& mean_k) {
const Point3D& center = cloud.getPoints()[point_idx];
// 收集邻域点
std::vector<Eigen::Vector3f> points;
for (int idx : neighbor_indices) {
const Point3D& p = cloud.getPoints()[idx];
points.push_back(p.toEigen());
}
if (points.size() < 6) return false; // 需要足够的点拟合曲面
// 使用二次曲面拟合
return fitQuadricSurface(points, center.toEigen(),
k1, k2, gaussian_k, mean_k);
}
// 二次曲面拟合
static bool fitQuadricSurface(const std::vector<Eigen::Vector3f>& points,
const Eigen::Vector3f& center,
float& k1, float& k2,
float& gaussian_k, float& mean_k) {
// 构建法向量估计
Eigen::Vector3f normal, centroid;
if (!MathUtils::fitPlane(points, normal, centroid)) {
return false;
}
// 建立局部坐标系
Eigen::Vector3f u, v, w = normal;
// 选择u轴（与w垂直）
if (std::abs(w[0]) > std::abs(w[1])) {
u = Eigen::Vector3f(-w[2], 0, w[0]);
} else {
u = Eigen::Vector3f(0, w[2], -w[1]);
}
u.normalize();
// 计算v轴
v = w.cross(u);
v.normalize();
// 转换到局部坐标系
Eigen::MatrixXf A(points.size(), 6);
Eigen::VectorXf b(points.size());
for (size_t i = 0; i < points.size(); i++) {
Eigen::Vector3f p_local = points[i] - center;
float x = p_local.dot(u);
float y = p_local.dot(v);
float z = p_local.dot(w);
// 二次曲面方程：z = a*x² + b*y² + c*x*y + d*x + e*y + f
A(i, 0) = x * x;
A(i, 1) = y * y;
A(i, 2) = x * y;
A(i, 3) = x;
A(i, 4) = y;
A(i, 5) = 1.0f;
b[i] = z;
}
// 最小二乘法求解
Eigen::VectorXf coeffs = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
float a = coeffs[0];
float b_coef = coeffs[1];
float c = coeffs[2];
float d = coeffs[3];
float e = coeffs[4];
// 计算曲率
float E = 1.0f + d * d;
float F = d * e;
float G = 1.0f + e * e;
float D = std::sqrt(d * d + e * e + 1.0f);
float L = 2.0f * a / D;
float M = c / D;
float N = 2.0f * b_coef / D;
// 计算第一基本形式和第二基本形式的系数
float coeff = 1.0f / (E * G - F * F);
// 计算主曲率
float H = 0.5f * coeff * (E * N - 2.0f * F * M + G * L);
float K = coeff * (L * N - M * M);
// 计算主曲率
float discriminant = H * H - K;
if (discriminant < 0) discriminant = 0;
k1 = H + std::sqrt(discriminant);
k2 = H - std::sqrt(discriminant);
gaussian_k = K;
mean_k = H;
return true;
}
};
2.2 KD树加速结构 (kd_tree.h)
#ifndef KD_TREE_H
#define KD_TREE_H
#include "point_cloud.h"
#include <queue>
#include <algorithm>
#include <limits>
struct KdTreeNode {
Point3D point;
int index;
KdTreeNode* left;
KdTreeNode* right;
int axis; // 分割轴: 0=x, 1=y, 2=z
KdTreeNode(const Point3D& p, int idx) :
point(p), index(idx), left(nullptr), right(nullptr), axis(0) {}
};
class KdTree {
private:
KdTreeNode* root;
std::vector<Point3D> points;
public:
KdTree(const PointCloud& cloud) {
points = cloud.getPoints();
root = buildTree(0, points.size(), 0);
}
~KdTree() {
deleteTree(root);
}
// K近邻搜索
void knnSearch(const Point3D& query, int k,
std::vector<int>& indices,
std::vector<float>& distances) {
indices.clear();
distances.clear();
std::priority_queue<std::pair<float, KdTreeNode*>> pq;
searchKnn(root, query, k, pq);
while (!pq.empty()) {
indices.push_back(pq.top().second->index);
distances.push_back(pq.top().first);
pq.pop();
}
std::reverse(indices.begin(), indices.end());
std::reverse(distances.begin(), distances.end());
}
// 半径搜索
void radiusSearch(const Point3D& query, float radius,
std::vector<int>& indices,
std::vector<float>& distances) {
indices.clear();
distances.clear();
searchRadius(root, query, radius, indices, distances);
}
private:
// 递归构建KD树
KdTreeNode* buildTree(int start, int end, int depth) {
if (start >= end) return nullptr;
int axis = depth % 3;
// 按当前轴排序
if (axis == 0) {
std::sort(points.begin() + start, points.begin() + end,
const Point3D& a, const Point3D& b { return a.x < b.x; });
} else if (axis == 1) {
std::sort(points.begin() + start, points.begin() + end,
const Point3D& a, const Point3D& b { return a.y < b.y; });
} else {
std::sort(points.begin() + start, points.begin() + end,
const Point3D& a, const Point3D& b { return a.z < b.z; });
}
int mid = start + (end - start) / 2;
KdTreeNode* node = new KdTreeNode(points[mid], mid);
node->axis = axis;
node->left = buildTree(start, mid, depth + 1);
node->right = buildTree(mid + 1, end, depth + 1);
return node;
}
// 递归删除树
void deleteTree(KdTreeNode* node) {
if (!node) return;
deleteTree(node->left);
deleteTree(node->right);
delete node;
}
// K近邻搜索递归
void searchKnn(KdTreeNode* node, const Point3D& query, int k,
std::priority_queue<std::pair<float, KdTreeNode*>>& pq) {
if (!node) return;
float dist = query.distanceTo(node->point);
if (pq.size() < k) {
pq.push({dist, node});
} else if (dist < pq.top().first) {
pq.pop();
pq.push({dist, node});
}
int axis = node->axis;
float diff = 0;
if (axis == 0) diff = query.x - node->point.x;
else if (axis == 1) diff = query.y - node->point.y;
else diff = query.z - node->point.z;
KdTreeNode* first = (diff < 0) ? node->left : node->right;
KdTreeNode* second = (diff < 0) ? node->right : node->left;
searchKnn(first, query, k, pq);
if (pq.size() < k || diff * diff < pq.top().first) {
searchKnn(second, query, k, pq);
}
}
// 半径搜索递归
void searchRadius(KdTreeNode* node, const Point3D& query, float radius,
std::vector<int>& indices, std::vector<float>& distances) {
if (!node) return;
float dist = query.distanceTo(node->point);
if (dist <= radius) {
indices.push_back(node->index);
distances.push_back(dist);
}
int axis = node->axis;
float diff = 0;
if (axis == 0) diff = query.x - node->point.x;
else if (axis == 1) diff = query.y - node->point.y;
else diff = query.z - node->point.z;
KdTreeNode* first = (diff < 0) ? node->left : node->right;
KdTreeNode* second = (diff < 0) ? node->right : node->left;
searchRadius(first, query, radius, indices, distances);
if (std::abs(diff) <= radius) {
searchRadius(second, query, radius, indices, distances);
}
}
};
3. 高级曲率计算方法
3.1 基于法向变化的曲率 (normal_variation_curvature.h)
#ifndef NORMAL_VARIATION_CURVATURE_H
#define NORMAL_VARIATION_CURVATURE_H
#include "point_cloud.h"
#include "math_utils.h"
class NormalVariationCurvature {
public:
// 基于法向量变化的曲率估计
static float compute(const PointCloud& cloud,
const std::vector<int>& neighbor_indices,
int point_idx) {
if (neighbor_indices.size() < 3) return 0.0f;
const Point3D& center = cloud.getPoints()[point_idx];
Eigen::Vector3f center_normal(center.nx, center.ny, center.nz);
if (center_normal.norm() < 1e-6f) {
return 0.0f; // 法向量未计算
}
// 计算法向量变化的平均角度
float total_angle = 0.0f;
int valid_count = 0;
for (int idx : neighbor_indices) {
const Point3D& neighbor = cloud.getPoints()[idx];
Eigen::Vector3f neighbor_normal(neighbor.nx, neighbor.ny, neighbor.nz);
if (neighbor_normal.norm() < 1e-6f) continue;
float angle = MathUtils::angleBetween(center_normal, neighbor_normal);
total_angle += angle;
valid_count++;
}
if (valid_count == 0) return 0.0f;
float avg_angle = total_angle / valid_count;
// 角度越大，曲率越大
return avg_angle;
}
};
3.2 基于曲率张量的方法 (curvature_tensor.h)
#ifndef CURVATURE_TENSOR_H
#define CURVATURE_TENSOR_H
#include "point_cloud.h"
#include "math_utils.h"
class CurvatureTensor {
public:
// 计算曲率张量
static bool compute(const PointCloud& cloud,
const std::vector<int>& neighbor_indices,
int point_idx,
Eigen::Matrix3f& curvature_tensor,
float& principal_curvature1,
float& principal_curvature2,
Eigen::Vector3f& principal_dir1,
Eigen::Vector3f& principal_dir2) {
if (neighbor_indices.size() < 6) return false;
const Point3D& center = cloud.getPoints()[point_idx];
Eigen::Vector3f normal(center.nx, center.ny, center.nz);
if (normal.norm() < 1e-6f) return false;
// 构建投影平面
Eigen::Vector3f u, v, w = normal;
// 选择u轴
if (std::abs(w[0]) > std::abs(w[1])) {
u = Eigen::Vector3f(-w[2], 0, w[0]);
} else {
u = Eigen::Vector3f(0, w[2], -w[1]);
}
u.normalize();
v = w.cross(u);
v.normalize();
// 收集邻域点
std::vector<Eigen::Vector3f> local_points;
for (int idx : neighbor_indices) {
const Point3D& p = cloud.getPoints()[idx];
Eigen::Vector3f diff(p.x - center.x, p.y - center.y, p.z - center.z);
float x_local = diff.dot(u);
float y_local = diff.dot(v);
float z_local = diff.dot(w);
local_points.emplace_back(x_local, y_local, z_local);
}
// 构建法向变化矩阵
Eigen::MatrixXf A(local_points.size(), 2);
Eigen::VectorXf b(local_points.size());
for (size_t i = 0; i < local_points.size(); i++) {
float x = local_points[i][0];
float y = local_points[i][1];
float z = local_points[i][2];
A(i, 0) = x;
A(i, 1) = y;
b[i] = z;
}
// 最小二乘法拟合平面
Eigen::Vector2f coeffs = (A.transpose() * A).inverse() * A.transpose() * b;
float a = coeffs[0];
float b_coef = coeffs[1];
// 构造曲率张量
curvature_tensor = Eigen::Matrix3f::Zero();
curvature_tensor(0, 0) = 2.0f * a;
curvature_tensor(1, 1) = 2.0f * b_coef;
curvature_tensor(0, 1) = curvature_tensor(1, 0) = 0.0f;
// 提取主曲率和方向
Eigen::SelfAdjointEigenSolver<Eigen::Matrix2f> solver;
solver.compute(curvature_tensor.block<2,2>(0,0));
principal_curvature1 = solver.eigenvalues()[0];
principal_curvature2 = solver.eigenvalues()[1];
principal_dir1 = solver.eigenvectors().col(0)[0] * u +
solver.eigenvectors().col(0)[1] * v;
principal_dir2 = solver.eigenvectors().col(1)[0] * u +
solver.eigenvectors().col(1)[1] * v;
return true;
}
};
4. 曲率特征提取和应用
4.1 曲率特征计算器 (curvature_feature.h)
#ifndef CURVATURE_FEATURE_H
#define CURVATURE_FEATURE_H
#include "point_cloud.h"
#include "pca_curvature.h"
#include "normal_variation_curvature.h"
#include "curvature_tensor.h"
class CurvatureFeatureExtractor {
private:
int k_neighbors;
float search_radius;
bool compute_principal_curvatures;
public:
CurvatureFeatureExtractor(int k = 30, float radius = 0.1f,
bool compute_principal = true) :
k_neighbors(k), search_radius(radius),
compute_principal_curvatures(compute_principal) {}
// 计算所有曲率特征
bool computeFeatures(PointCloud& cloud) {
if (cloud.size() < k_neighbors) {
std::cerr << "点云点数不足" << std::endl;
return false;
}
// 建立KD树
KdTree kdtree(cloud);
for (size_t i = 0; i < cloud.size(); i++) {
std::vector<int> neighbor_indices;
std::vector<float> distances;
kdtree.knnSearch(cloud.getPoints()[i], k_neighbors,
neighbor_indices, distances);
if (neighbor_indices.size() < 3) {
continue;
}
// 1. 计算PCA曲率
float pca_curvature = PCACurvature::computePCACurvature(
cloud, neighbor_indices, i);
cloud.getPoints()[i].curvature = pca_curvature;
// 2. 计算法向量变化曲率
float normal_variation = NormalVariationCurvature::compute(
cloud, neighbor_indices, i);
// 3. 如果需要，计算主曲率
if (compute_principal_curvatures) {
float k1, k2, gaussian_k, mean_k;
if (PCACurvature::computePrincipalCurvatures(
cloud, neighbor_indices, i, k1, k2, gaussian_k, mean_k)) {
cloud.getPoints()[i].principal_k1 = k1;
cloud.getPoints()[i].principal_k2 = k2;
cloud.getPoints()[i].gaussian_k = gaussian_k;
cloud.getPoints()[i].mean_k = mean_k;
}
}
// 4. 计算曲率张量
Eigen::Matrix3f curvature_tensor;
float pc1, pc2;
Eigen::Vector3f dir1, dir2;
if (CurvatureTensor::compute(cloud, neighbor_indices, i,
curvature_tensor, pc1, pc2, dir1, dir2)) {
// 保存张量信息
}
}
return true;
}
// 计算曲率直方图
std::vector<float> computeCurvatureHistogram(const PointCloud& cloud,
int bins = 20) {
std::vector<float> histogram(bins, 0.0f);
if (cloud.size() == 0) return histogram;
// 找到曲率范围
float min_curvature = std::numeric_limits<float>::max();
float max_curvature = std::numeric_limits<float>::lowest();
for (const auto& p : cloud.getPoints()) {
min_curvature = std::min(min_curvature, p.curvature);
max_curvature = std::max(max_curvature, p.curvature);
}
if (max_curvature <= min_curvature) {
return histogram;
}
float bin_width = (max_curvature - min_curvature) / bins;
// 统计直方图
for (const auto& p : cloud.getPoints()) {
int bin_idx = static_cast<int>((p.curvature - min_curvature) / bin_width);
bin_idx = std::max(0, std::min(bins - 1, bin_idx));
histogram[bin_idx] += 1.0f;
}
// 归一化
float total = cloud.size();
for (auto& val : histogram) {
val /= total;
}
return histogram;
}
// 基于曲率的点云分割
std::vector<int> segmentByCurvature(const PointCloud& cloud,
float curvature_threshold,
int min_cluster_size = 10) {
std::vector<int> labels(cloud.size(), -1);
int current_label = 0;
// 建立KD树
KdTree kdtree(cloud);
for (size_t i = 0; i < cloud.size(); i++) {
if (labels[i] != -1) continue;
if (cloud.getPoints()[i].curvature < curvature_threshold) {
// 低曲率区域，开始区域增长
std::queue<int> queue;
queue.push(i);
labels[i] = current_label;
int cluster_size = 0;
while (!queue.empty()) {
int current_idx = queue.front();
queue.pop();
cluster_size++;
// 搜索邻域
std::vector<int> neighbor_indices;
std::vector<float> distances;
kdtree.knnSearch(cloud.getPoints()[current_idx], k_neighbors,
neighbor_indices, distances);
for (int neighbor_idx : neighbor_indices) {
if (labels[neighbor_idx] == -1 &&
cloud.getPoints()[neighbor_idx].curvature < curvature_threshold) {
labels[neighbor_idx] = current_label;
queue.push(neighbor_idx);
}
}
}
if (cluster_size >= min_cluster_size) {
current_label++;
} else {
// 标记为噪声
labels[i] = -1;
}
}
}
return labels;
}
// 计算曲率梯度
std::vector<float> computeCurvatureGradient(const PointCloud& cloud) {
std::vector<float> gradient(cloud.size(), 0.0f);
KdTree kdtree(cloud);
for (size_t i = 0; i < cloud.size(); i++) {
std::vector<int> neighbor_indices;
std::vector<float> distances;
kdtree.knnSearch(cloud.getPoints()[i], 6, // 使用6个最近邻
neighbor_indices, distances);
float max_gradient = 0.0f;
float center_curvature = cloud.getPoints()[i].curvature;
for (int j = 0; j < neighbor_indices.size(); j++) {
int neighbor_idx = neighbor_indices[j];
if (neighbor_idx == i) continue;
float neighbor_curvature = cloud.getPoints()[neighbor_idx].curvature;
float distance = distances[j];
if (distance > 1e-6f) {
float grad = std::abs(center_curvature - neighbor_curvature) / distance;
max_gradient = std::max(max_gradient, grad);
}
}
gradient[i] = max_gradient;
}
return gradient;
}
};
5. 应用示例
5.1 主程序示例 (main.cpp)
#include <iostream>
#include <fstream>
#include <vector>
#include <string>
#include "point_cloud.h"
#include "curvature_feature.h"
// 从PLY文件读取点云
bool readPLY(const std::string& filename, PointCloud& cloud) {
std::ifstream file(filename);
if (!file.is_open()) {
std::cerr << "无法打开文件: " << filename << std::endl;
return false;
}
std::string line;
int vertex_count = 0;
bool in_header = true;
// 读取PLY头
while (in_header && std::getline(file, line)) {
if (line.find("element vertex") != std::string::npos) {
std::sscanf(line.c_str(), "element vertex %d", &vertex_count);
} else if (line == "end_header") {
in_header = false;
}
}
if (vertex_count == 0) {
std::cerr << "未找到顶点信息" << std::endl;
return false;
}
// 读取顶点数据
for (int i = 0; i < vertex_count; i++) {
float x, y, z;
if (!(file >> x >> y >> z)) {
std::cerr << "读取顶点数据失败" << std::endl;
return false;
}
cloud.addPoint(x, y, z);
// 跳过可能的颜色、法向量等信息
std::getline(file, line);
}
file.close();
std::cout << "读取了 " << cloud.size() << " 个点" << std::endl;
return true;
}
// 保存点云和曲率到文件
bool saveWithCurvature(const std::string& filename, const PointCloud& cloud) {
std::ofstream file(filename);
if (!file.is_open()) {
std::cerr << "无法创建文件: " << filename << std::endl;
return false;
}
// 写入PLY头
file << "ply\n";
file << "format ascii 1.0\n";
file << "element vertex " << cloud.size() << "\n";
file << "property float x\n";
file << "property float y\n";
file << "property float z\n";
file << "property float nx\n";
file << "property float ny\n";
file << "property float nz\n";
file << "property float curvature\n";
file << "property float principal_k1\n";
file << "property float principal_k2\n";
file << "property float gaussian_k\n";
file << "property float mean_k\n";
file << "end_header\n";
// 写入数据
for (const auto& p : cloud.getPoints()) {
file << p.x << " " << p.y << " " << p.z << " "
<< p.nx << " " << p.ny << " " << p.nz << " "
<< p.curvature << " "
<< p.principal_k1 << " " << p.principal_k2 << " "
<< p.gaussian_k << " " << p.mean_k << "\n";
}
file.close();
std::cout << "已保存到: " << filename << std::endl;
return true;
}
// 生成测试点云（球体）
void generateSphere(PointCloud& cloud, float radius = 1.0f, int points = 1000) {
for (int i = 0; i < points; i++) {
// 球面坐标
float u = static_cast<float>(rand()) / RAND_MAX;
float v = static_cast<float>(rand()) / RAND_MAX;
float theta = 2.0f * M_PI * u;
float phi = std::acos(2.0f * v - 1.0f);
float x = radius * std::sin(phi) * std::cos(theta);
float y = radius * std::sin(phi) * std::sin(theta);
float z = radius * std::cos(phi);
cloud.addPoint(x, y, z);
}
}
// 生成测试点云（平面+球体）
void generateMixed(PointCloud& cloud) {
int total_points = 2000;
// 平面部分
for (int i = 0; i < total_points / 2; i++) {
float x = static_cast<float>(rand()) / RAND_MAX * 4.0f - 2.0f;
float y = static_cast<float>(rand()) / RAND_MAX * 4.0f - 2.0f;
float z = 0.0f;
cloud.addPoint(x, y, z);
}
// 球体部分
float radius = 1.0f;
for (int i = 0; i < total_points / 2; i++) {
float u = static_cast<float>(rand()) / RAND_MAX;
float v = static_cast<float>(rand()) / RAND_MAX;
float theta = 2.0f * M_PI * u;
float phi = std::acos(2.0f * v - 1.0f);
float x = radius * std::sin(phi) * std::cos(theta) + 3.0f;
float y = radius * std::sin(phi) * std::sin(theta);
float z = radius * std::cos(phi);
cloud.addPoint(x, y, z);
}
}
int main() {
PointCloud cloud;
// 生成测试点云
std::cout << "生成测试点云..." << std::endl;
generateMixed(cloud);
// 计算曲率特征
std::cout << "计算曲率特征..." << std::endl;
CurvatureFeatureExtractor extractor(20, 0.2f, true);
if (!extractor.computeFeatures(cloud)) {
std::cerr << "曲率计算失败" << std::endl;
return 1;
}
// 计算曲率直方图
std::vector<float> histogram = extractor.computeCurvatureHistogram(cloud, 20);
std::cout << "曲率直方图:" << std::endl;
for (size_t i = 0; i < histogram.size(); i++) {
std::cout << "Bin " << i << ": " << histogram[i] << std::endl;
}
// 基于曲率分割
float curvature_threshold = 0.1f;
std::vector<int> labels = extractor.segmentByCurvature(cloud, curvature_threshold, 10);
int num_clusters = 0;
for (int label : labels) {
if (label > num_clusters) num_clusters = label;
}
std::cout << "找到 " << num_clusters + 1 << " 个簇" << std::endl;
// 计算曲率梯度
std::vector<float> gradient = extractor.computeCurvatureGradient(cloud);
// 统计结果
float min_curvature = std::numeric_limits<float>::max();
float max_curvature = std::numeric_limits<float>::lowest();
float avg_curvature = 0.0f;
for (const auto& p : cloud.getPoints()) {
min_curvature = std::min(min_curvature, p.curvature);
max_curvature = std::max(max_curvature, p.curvature);
avg_curvature += p.curvature;
}
avg_curvature /= cloud.size();
std::cout << "\n曲率统计:" << std::endl;
std::cout << "最小曲率: " << min_curvature << std::endl;
std::cout << "最大曲率: " << max_curvature << std::endl;
std::cout << "平均曲率: " << avg_curvature << std::endl;
// 保存结果
saveWithCurvature("output_with_curvature.ply", cloud);
return 0;
}
6. CMake配置文件
cmake_minimum_required(VERSION 3.10)
project(PointCloudCurvature)
set(CMAKE_CXX_STANDARD 11)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
# 查找Eigen3
find_package(Eigen3 REQUIRED)
# 包含目录
include_directories(${EIGEN3_INCLUDE_DIR})
include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include)
# 源文件
set(SOURCES
src/main.cpp
src/point_cloud.cpp
src/pca_curvature.cpp
src/kd_tree.cpp
src/curvature_feature.cpp
src/normal_variation_curvature.cpp
src/curvature_tensor.cpp
)
# 头文件
set(HEADERS
include/point_cloud.h
include/math_utils.h
include/pca_curvature.h
include/kd_tree.h
include/curvature_feature.h
include/normal_variation_curvature.h
include/curvature_tensor.h
)
# 可执行文件
add_executable(point_cloud_curvature ${SOURCES} ${HEADERS})
# 链接Eigen
target_link_libraries(point_cloud_curvature Eigen3::Eigen)
# 启用优化
if(CMAKE_BUILD_TYPE STREQUAL "Release")
add_compile_options(-O3 -march=native)
else()
add_compile_options(-O0 -g)
endif()
7. 算法比较和选择
7.1 不同方法的比较
class CurvatureComparison {
public:
// 比较不同算法的计算时间和精度
static void compareMethods(const PointCloud& cloud,
int k_neighbors = 20,
int iterations = 10) {
PointCloud cloud_copy = cloud;
KdTree kdtree(cloud);
// 准备测试点
std::vector<int> test_indices;
for (int i = 0; i < std::min(100, (int)cloud.size()); i++) {
test_indices.push_back(i);
}
// 测试PCA方法
auto start = std::chrono::high_resolution_clock::now();
for (int iter = 0; iter < iterations; iter++) {
for (int idx : test_indices) {
std::vector<int> neighbor_indices;
std::vector<float> distances;
kdtree.knnSearch(cloud.getPoints()[idx], k_neighbors,
neighbor_indices, distances);
float curvature = PCACurvature::computePCACurvature(
cloud, neighbor_indices, idx);
}
}
auto end = std::chrono::high_resolution_clock::now();
auto pca_time = std::chrono::duration_cast<std::chrono::milliseconds>(end - start);
// 测试法向量变化方法
start = std::chrono::high_resolution_clock::now();
for (int iter = 0; iter < iterations; iter++) {
for (int idx : test_indices) {
std::vector<int> neighbor_indices;
std::vector<float> distances;
kdtree.knnSearch(cloud.getPoints()[idx], k_neighbors,
neighbor_indices, distances);
float curvature = NormalVariationCurvature::compute(
cloud, neighbor_indices, idx);
}
}
end = std::chrono::high_resolution_clock::now();
auto normal_time = std::chrono::duration_cast<std::chrono::milliseconds>(end - start);
std::cout << "算法性能比较:" << std::endl;
std::cout << "PCA方法: " << pca_time.count() << " ms" << std::endl;
std::cout << "法向量变化方法: " << normal_time.count() << " ms" << std::endl;
}
// 计算不同邻域大小的影响
static void analyzeNeighborhoodEffect(PointCloud& cloud) {
std::vector<int> k_values = {5, 10, 20, 30, 50, 100};
std::vector<float> avg_curvatures;
for (int k : k_values) {
CurvatureFeatureExtractor extractor(k, 0.1f, false);
PointCloud test_cloud = cloud;
extractor.computeFeatures(test_cloud);
float total_curvature = 0.0f;
for (const auto& p : test_cloud.getPoints()) {
total_curvature += p.curvature;
}
avg_curvatures.push_back(total_curvature / test_cloud.size());
}
std::cout << "邻域大小对平均曲率的影响:" << std::endl;
for (size_t i = 0; i < k_values.size(); i++) {
std::cout << "K=" << k_values[i] << ": " << avg_curvatures[i] << std::endl;
}
}
};
参考代码 点云数据计算曲率的算法实现 www.youewnfan.com/contentcnv/71129.html
8. 实用工具函数
8.1 曲率滤波和增强
class CurvatureProcessor {
public:
// 高斯滤波平滑曲率
static void smoothCurvature(PointCloud& cloud, int k_neighbors = 20,
float sigma = 0.5f) {
KdTree kdtree(cloud);
for (size_t i = 0; i < cloud.size(); i++) {
std::vector<int> neighbor_indices;
std::vector<float> distances;
kdtree.knnSearch(cloud.getPoints()[i], k_neighbors,
neighbor_indices, distances);
float sum_weight = 0.0f;
float sum_curvature = 0.0f;
for (size_t j = 0; j < neighbor_indices.size(); j++) {
float dist = distances[j];
float weight = std::exp(-dist * dist / (2.0f * sigma * sigma));
int idx = neighbor_indices[j];
sum_curvature += weight * cloud.getPoints()[idx].curvature;
sum_weight += weight;
}
if (sum_weight > 1e-6f) {
cloud.getPoints()[i].curvature = sum_curvature / sum_weight;
}
}
}
// 基于曲率的点云下采样
static PointCloud downsampleByCurvature(const PointCloud& cloud,
float curvature_threshold = 0.1f,
float min_distance = 0.05f) {
PointCloud result;
std::vector<bool> keep_point(cloud.size(), false);
// 优先保留高曲率点
for (size_t i = 0; i < cloud.size(); i++) {
if (cloud.getPoints()[i].curvature > curvature_threshold) {
keep_point[i] = true;
}
}
// 对低曲率区域进行下采样
KdTree kdtree(cloud);
for (size_t i = 0; i < cloud.size(); i++) {
if (keep_point[i]) {
result.addPoint(cloud.getPoints()[i]);
continue;
}
// 检查是否已经有附近的高曲率点
std::vector<int> neighbor_indices;
std::vector<float> distances;
kdtree.radiusSearch(cloud.getPoints()[i], min_distance,
neighbor_indices, distances);
bool has_high_curvature = false;
for (int idx : neighbor_indices) {
if (keep_point[idx]) {
has_high_curvature = true;
break;
}
}
if (!has_high_curvature) {
result.addPoint(cloud.getPoints()[i]);
}
}
return result;
}
// 提取高曲率特征点
static std::vector<int> extractFeaturePoints(const PointCloud& cloud,
float curvature_threshold = 0.3f,
int min_neighbors = 5) {
std::vector<int> feature_indices;
KdTree kdtree(cloud);
for (size_t i = 0; i < cloud.size(); i++) {
if (cloud.getPoints()[i].curvature < curvature_threshold) {
continue;
}
// 检查是否为局部最大值
std::vector<int> neighbor_indices;
std::vector<float> distances;
kdtree.knnSearch(cloud.getPoints()[i], min_neighbors + 1,
neighbor_indices, distances);
bool is_local_max = true;
float center_curvature = cloud.getPoints()[i].curvature;
for (size_t j = 1; j < neighbor_indices.size(); j++) { // 从1开始，跳过自身
int idx = neighbor_indices[j];
if (cloud.getPoints()[idx].curvature > center_curvature) {
is_local_max = false;
break;
}
}
if (is_local_max) {
feature_indices.push_back(i);
}
}
return feature_indices;
}
};
9. 注意事项和优化建议
9.1 参数选择指南
class ParameterGuide {
public:
// 根据点云密度自动选择K值
static int autoSelectK(const PointCloud& cloud) {
if (cloud.size() < 100) return 10;
// 计算点云密度
Eigen::Vector3f min_pt, max_pt;
cloud.computeBoundingBox(min_pt, max_pt);
Eigen::Vector3f diag = max_pt - min_pt;
float volume = diag[0] * diag[1] * diag[2];
float density = cloud.size() / volume;
// 根据密度选择K
if (density < 100) return 10; // 稀疏点云
else if (density < 1000) return 20; // 中等密度
else if (density < 10000) return 30; // 高密度
else return 50; // 超高密度
}
// 根据噪声水平选择平滑参数
static float autoSelectSigma(const PointCloud& cloud) {
if (cloud.size() < 10) return 0.1f;
// 计算平均最近邻距离
KdTree kdtree(cloud);
float total_distance = 0.0f;
int count = 0;
for (size_t i = 0; i < std::min(100, (int)cloud.size()); i++) {
std::vector<int> neighbor_indices;
std::vector<float> distances;
kdtree.knnSearch(cloud.getPoints()[i], 2, // 自身+最近邻
neighbor_indices, distances);
if (distances.size() > 1) {
total_distance += distances[1]; // 最近邻距离
count++;
}
}
float avg_distance = total_distance / count;
return avg_distance * 2.0f; // sigma设为平均距离的2倍
}
};
10. 编译和使用
10.1 编译命令
# 创建构建目录
mkdir build
cd build
# 使用CMake配置
cmake ..
# 编译
make -j4
# 运行程序
./point_cloud_curvature
10.2 输入数据格式
程序支持以下格式：
- PLY格式（ASCII）
- XYZ格式（每行x y z）
- 程序内生成测试数据
10.3 输出结果
程序会输出：
- 每个点的曲率
- 主曲率（k1, k2）
- 高斯曲率和平均曲率
- 法向量
- 曲率直方图
- 基于曲率的分割结果
