PCL从理解到应用【08】点云特征 | 法线估计

前言

在PCL中，有多种方法和函数可以用来提取点云特征，本文介绍几何特征。

其中，几何特征主要包括法线估计和主曲率估计。

这些特征能够描述点云表面的几何形状，常用于进一步的点云处理和分析，如配准、分割和物体识别。

平面的法向量估计，示例效果：

局部区域点云，法向量估计示例效果：

1、法线估计 (Normal Estimation)

法线是描述点云表面局部几何形状的基本特征之一。

PCL提供了多种方法来估计法线，以下是两种常用的方法：

pcl::NormalEstimation
pcl::IntegralImageNormalEstimation

pcl::NormalEstimation 是最常用的法线估计类。

它使用给定的点云和邻域搜索方法来计算每个点的法线。以下是基本的使用步骤：

创建对象: 实例化 pcl::NormalEstimation 对象。
设置输入点云: 使用 setInputCloud() 方法设置输入点云。
设置搜索方法: 使用 setSearchMethod() 设置邻域搜索方法，如 pcl::search::KdTree。
设置半径或K值: 设置用于搜索邻域的半径或最近邻数量（K值），使用 setRadiusSearch() 或 setKSearch()。
计算法线: 使用 compute() 方法计算法线。

示例代码：

pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud(cloud);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
ne.setSearchMethod(tree);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
ne.setRadiusSearch(0.03);
ne.compute(*cloud_normals);

pcl::IntegralImageNormalEstimation 是另一种法线估计方法

特别适用于组织良好的点云（如深度图像）
它使用积分图像来快速计算法线，是一种更高效的方法

示例代码：

pcl::IntegralImageNormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
ne.setInputCloud(cloud);
ne.setNormalEstimationMethod(ne.AVERAGE_3D_GRADIENT);
ne.setMaxDepthChangeFactor(0.02f);
ne.setNormalSmoothingSize(10.0f);
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
ne.compute(*cloud_normals);

通常情况下，为了计算点云的法向量，需要先拟合平面或者确定点云局部的几何特征。

法向量描述了平面在三维空间中的方向，因此拟合平面是计算法向量的一个关键步骤。

具体来说，计算法向量的常见步骤包括：

定义邻域：选择一个点的邻域，即确定该点附近的一组点。这些邻域点可以通过固定半径搜索或K最近邻搜索来确定。
拟合平面：在定义的邻域中，使用平面拟合算法来确定邻域点的最佳平面。这通常涉及最小二乘法来拟合平面。
平面拟合的结果通常以平面方程的形式表示：ax+by+cz+d=0。
计算法向量：平面拟合的结果提供了平面法向量的方向。平面方程中的 (a,b,c)就是平面的法向量。
法向量归一化：将法向量归一化为单位向量，即长度为1。

2、主曲率估计 (Principal Curvature Estimation)

主曲率描述了点云表面的曲率信息，可以帮助理解表面的几何形状特征。

`pcl::PrincipalCurvaturesEstimation`

该类通过计算每个点的主曲率方向和值来描述点云的几何结构。主要使用步骤如下：

创建对象: 实例化 pcl::PrincipalCurvaturesEstimation 对象。
设置输入点云和法线: 使用 setInputCloud() 和 setInputNormals() 方法设置输入点云和法线。
设置搜索方法: 设置邻域搜索方法，如 pcl::search::KdTree。
计算主曲率: 使用 compute() 方法计算主曲率。

示例代码：

pcl::PrincipalCurvaturesEstimation<pcl::PointXYZ, pcl::Normal, pcl::PrincipalCurvatures> pc;
pc.setInputCloud(cloud);
pc.setInputNormals(cloud_normals);
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
pc.setSearchMethod(tree);
pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr cloud_curvatures(new pcl::PointCloud<pcl::PrincipalCurvatures>);
pc.setRadiusSearch(0.05);
pc.compute(*cloud_curvatures);

3、实践案例——平面法线估计

估计一个主法向量，通常是在处理一个平面或近似平面的点云时进行的。

主法向量可以通过对点云的法线进行聚合，或者使用主成分分析（PCA）来估计整个点云的方向。

示例效果：

示例代码：

#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/search/kdtree.h>
#include <Eigen/Dense>int main(int argc, char** argv)
{// 创建点云对象并生成平面点云数据pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());// 定义平面的参数，例如 z = 1.0float z_value = 1.0f;float noise_level = 0.02f; // 噪声水平// 生成位于平面上的点并添加噪声for (float x = -1.0; x <= 1.0; x += 0.05){for (float y = -1.0; y <= 1.0; y += 0.05){pcl::PointXYZ point;point.x = x + noise_level * static_cast<float>(rand()) / RAND_MAX - noise_level / 2.0f;point.y = y + noise_level * static_cast<float>(rand()) / RAND_MAX - noise_level / 2.0f;point.z = z_value + noise_level * static_cast<float>(rand()) / RAND_MAX - noise_level / 2.0f;cloud->points.push_back(point);}}cloud->width = static_cast<uint32_t>(cloud->points.size());cloud->height = 1;cloud->is_dense = true;// 计算点云的质心Eigen::Vector4f centroid;pcl::compute3DCentroid(*cloud, centroid);// 构建点云的协方差矩阵Eigen::Matrix3f covariance_matrix;pcl::computeCovarianceMatrixNormalized(*cloud, centroid, covariance_matrix);// 计算协方差矩阵的特征值和特征向量Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance_matrix, Eigen::ComputeEigenvectors);Eigen::Matrix3f eigen_vectors = eigen_solver.eigenvectors();Eigen::Vector3f eigen_values = eigen_solver.eigenvalues();// 主法向量是协方差矩阵的最小特征值对应的特征向量Eigen::Vector3f main_normal_vector = eigen_vectors.col(0);// 输出主法向量std::cout << "主法向量: " << main_normal_vector.transpose() << std::endl;// 可视化pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));viewer->setBackgroundColor(0, 0, 0);// 添加点云到可视化器pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud, 0, 255, 0);viewer->addPointCloud<pcl::PointXYZ>(cloud, single_color, "sample cloud");viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "sample cloud");// 可视化主法向量pcl::PointXYZ centroid_point(centroid[0], centroid[1], centroid[2]);pcl::PointXYZ normal_endpoint(centroid[0] + main_normal_vector[0],centroid[1] + main_normal_vector[1],centroid[2] + main_normal_vector[2]);viewer->addArrow(normal_endpoint, centroid_point, 2.0, 0.0, 0.0, false, "main_normal");// 运行可视化器viewer->initCameraParameters();while (!viewer->wasStopped()){viewer->spinOnce(100);}return 0;
}

代码说明：

生成平面点云数据并添加噪声：
- 与之前的例子类似，生成一个位于平面上的点云，并添加随机噪声。
计算质心：
- 使用 pcl::compute3DCentroid 计算点云的质心，这将作为主法向量的起点。
计算协方差矩阵：
- 使用 pcl::computeCovarianceMatrixNormalized 计算点云的协方差矩阵，这是PCA的核心步骤。
计算特征值和特征向量：
- 使用 Eigen 库的 Eigen::SelfAdjointEigenSolver 计算协方差矩阵的特征值和特征向量。
- 主法向量对应于最小特征值的特征向量。
可视化：
- 使用PCL可视化工具展示点云，并使用箭头显示主法向量。

4、实践案例——局部区域法线估计

使用PCL生成随机点云数据，然后进行法线估计，并使用PCL提供的可视化工具进行显示。

示例说明：

生成随机点云数据:
- 使用 rand() 函数生成随机点云数据，其中每个点的 x, y, z 坐标在 [0, 1024) 范围内。
法线估计:
- 使用 pcl::NormalEstimation 进行法线估计，设置输入点云和邻域搜索方法。
- 设置 KSearch 为100，表示使用每个点的100个最近邻来计算法线。
可视化:
- 使用 pcl::visualization::PCLVisualizer 创建一个3D可视化窗口。
- 使用 addPointCloud 方法将点云添加到可视化器中，并设定点云的颜色。
- 使用 addPointCloudNormals 方法将法线添加到可视化器中，其中 10 表示显示每10个点的法线，0.05 表示法线的缩放比例。
- viewer->spinOnce(100); 控制可视化器的刷新频率。

示例效果：

示例代码：

#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/search/kdtree.h>
#include <pcl/common/common.h>int main(int argc, char** argv)
{// 创建点云对象并生成随机点云数据pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());// 随机生成1000个点for (size_t i = 0; i < 1000; ++i){pcl::PointXYZ point;point.x = 1024 * rand() / (RAND_MAX + 1.0f);point.y = 1024 * rand() / (RAND_MAX + 1.0f);point.z = 1024 * rand() / (RAND_MAX + 1.0f);cloud->points.push_back(point);}cloud->width = static_cast<uint32_t>(cloud->points.size());cloud->height = 1;cloud->is_dense = true;// 创建法线估计对象pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;ne.setInputCloud(cloud);// 创建一个搜索树对象，并将其传递给法线估计对象pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());ne.setSearchMethod(tree);// 创建一个保存法线的点云对象pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);// 设置用于邻域搜索的K值ne.setKSearch(100);// 计算法线ne.compute(*cloud_normals);// 可视化pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));viewer->setBackgroundColor(0, 0, 0);// 添加点云到可视化器pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud, 0, 255, 0);viewer->addPointCloud<pcl::PointXYZ>(cloud, single_color, "sample cloud");viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "sample cloud");// 可视化法线，显示每个点的法线viewer->addPointCloudNormals<pcl::PointXYZ, pcl::Normal>(cloud, cloud_normals, 10, 0.1, "normals");// 运行可视化器viewer->initCameraParameters();while (!viewer->wasStopped()){viewer->spinOnce(100);}return 0;
}

5、实践案例——点云主曲率

示例效果：

示例代码：

#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <Eigen/Dense>
#include <ctime>
#include <cstdlib>int main(int argc, char** argv)
{// 初始化随机数生成器std::srand(std::time(nullptr));// 创建点云对象并生成曲面点云数据pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());// 生成双曲抛物面 z = x^2 - y^2，并添加噪声float noise_level = 0.02f; // 噪声水平for (float x = -1.0; x <= 1.0; x += 0.05){for (float y = -1.0; y <= 1.0; y += 0.05){pcl::PointXYZ point;point.x = x + noise_level * static_cast<float>(rand()) / RAND_MAX - noise_level / 2.0f;point.y = y + noise_level * static_cast<float>(rand()) / RAND_MAX - noise_level / 2.0f;point.z = (x * x - y * y) + noise_level * static_cast<float>(rand()) / RAND_MAX - noise_level / 2.0f;cloud->points.push_back(point);}}cloud->width = static_cast<uint32_t>(cloud->points.size());cloud->height = 1;cloud->is_dense = true;// 计算点云的质心Eigen::Vector4f centroid;pcl::compute3DCentroid(*cloud, centroid);// 构建点云的协方差矩阵Eigen::Matrix3f covariance_matrix;pcl::computeCovarianceMatrixNormalized(*cloud, centroid, covariance_matrix);// 计算协方差矩阵的特征值和特征向量Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigen_solver(covariance_matrix, Eigen::ComputeEigenvectors);Eigen::Matrix3f eigen_vectors = eigen_solver.eigenvectors();Eigen::Vector3f eigen_values = eigen_solver.eigenvalues();// 主法向量是协方差矩阵的最小特征值对应的特征向量Eigen::Vector3f main_normal_vector = eigen_vectors.col(0);// 输出主法向量和主曲率std::cout << "主法向量: " << main_normal_vector.transpose() << std::endl;std::cout << "主曲率: " << eigen_values(0) << std::endl;// 可视化pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));viewer->setBackgroundColor(0, 0, 0);// 添加点云到可视化器pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> single_color(cloud, 0, 255, 0);viewer->addPointCloud<pcl::PointXYZ>(cloud, single_color, "sample cloud");viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "sample cloud");// 可视化主法向量pcl::PointXYZ centroid_point(centroid[0], centroid[1], centroid[2]);pcl::PointXYZ normal_endpoint(centroid[0] + main_normal_vector[0],centroid[1] + main_normal_vector[1],centroid[2] + main_normal_vector[2]);viewer->addArrow(normal_endpoint, centroid_point, 1.0, 0.0, 0.0, false, "main_normal");// 运行可视化器viewer->initCameraParameters();while (!viewer->wasStopped()){viewer->spinOnce(100);}return 0;
}

代码分析：

生成曲面点云数据：
- 生成一个双曲抛物面（z = x^2 - y^2）的点云数据，并添加一定的噪声。
法线估计：
- 使用 pcl::NormalEstimation 计算每个点的法线。
主曲率估计：
- 使用 pcl::PrincipalCurvaturesEstimation 计算每个点的主曲率方向和大小。
- 设置 setRadiusSearch 用于定义邻域的搜索半径。