二次曲面拟合计算点云法曲率、主曲率以及平均曲率（手写版）

点云处理

已于 2023-12-11 22:14:16 修改

阅读量1.6k

点赞数 22

分类专栏：激光点云数据处理文章标签：算法机器学习人工智能

于 2023-11-26 10:44:47 首次发布

本文链接：https://blog.csdn.net/qq_48694523/article/details/134624204

版权

激光点云数据处理专栏收录该内容

28 篇文章 12 订阅 ¥19.90 ¥99.00

订阅专栏

超级会员免费看

参考文献：[1]周煜,张万兵,杜发荣等.散乱点云数据的曲率精简算法[J].北京理工大学学报,2010,30(07):785-789.DOI:10.15918/j.tbit1001-0645.2010.07.021.

[2]蔡志敏,王晏民,黄明.基于KD树散乱点云数据的Guass平均曲率精简算法[J].测绘通报,2013(S1):44-46.

一、相关原理及介绍

从微分几何的角度来看，曲面曲率较高的区域，应该有较多的采样点表示，相反则应该由较少的采样点表示，曲率反映了曲面的基本特性。点云曲率特征是指在三维空间中，对于点云数据集中的每个点，通过计算其周围的几何形状来确定该点的曲率特征，曲率特征是点云数据集中非常重要的特征之一，它可以应用于点云中的分类、分割、配准、特征提取等应用中。

曲率特征可以分为平均曲率、高斯曲率以及主曲率。平均曲率作为微分几何中一个“外在的”弯曲测量标准，对一个曲面嵌入周围空间（比如二维曲面嵌入三维欧式空间）的曲率进行了区部描述。高斯曲率作为一个描述曲面的凹凸性质的量，当这个量变化程度较大的时候表面曲面内部变化也比较大，这就表明曲面的光滑程度越低。点云中任意一点都存在某曲面 $f=r(x,y,z)$ 逼近该点的邻域点云，一点处的曲率可用该点及其邻域点拟合的局部曲面曲率来表征。通过最小二乘拟合，可以用二次曲面来表征局部区域，计算每点处的主曲率 $k{_{1}}$ 、 $k_{2}$ ，平均曲率 $H$ 及高斯曲率 $K$ 。

二、计算流程

设点云中某一点 $P_{i}$ 的 $K$ 邻域点集为 $Q$ ，通过邻域点建立二次曲面参数方程：

$r(u,v)=\sum_{i,j=0}^{2}q_{i,j}u^{i}v^{j}$ (1)

令：

$a=[a_{00} a_{01} a_{02} a_{10} a_{11} a_{12} a_{20} a_{21} a_{22}]$ $b=[b_{00} b_{01} b_{02} b_{10} b_{11} b_{12} b_{20} b_{21} b_{22}]$

$c=[c_{00} c_{01} c_{02} c_{10} c_{11} c_{12} c_{20} c_{21} c_{22}]$

$w=[{u^{0}}{v^{0}}\, \, {u^{0}}{v^{1}} \, \, {u^{0}}{v^{2}} \, \, {u^{1}}{v^{0}} \, \, {u^{1}}{v^{1}} \, \, {u^{1}}{v^{2}} \, \, {u^{2}}{v^{0}} \, \, {u^{2}}{v^{1}}\, \, {u^{2}}{v^{2}}]$ ， $Q=[a,b,c]$ (2)

式（1）可改写成： $r(u,v)=[x(u,v) \, \, y(u,v)\, \, z(u,v)]=[w^{T}a \, \,w^{T}b \, \,w^{T}c]$ (3)

引入两个矩阵：

$B=\begin{bmatrix} x_{0} & y_{0} &z_{0} \\ x_{1}&y_{1} &z_{1} \\ ... &... &... \\ x_{k}&y_{k} & z_{k} \end{bmatrix}$ ， $W=\begin{bmatrix} W_{0}^{T} \\ W_{1}^{T} \\ ... \\ W_{k}^{T} \end{bmatrix}$ (4)

测点 $P_{i}$ 的坐标为 $P(x_{0}\, \, y_{0}\, \,z_{0})$ ，选择邻域点数大于8，则逼近曲面和数据点的误差函数为：

$\Omega =WQ-B$ （5）

运用最小二乘法，推到出系数矩阵 $Q$ 为：

$Q=(W^{T}W)^{-1}W^{T}B$ （6）

从而求出曲面 $r(u,v)$ 的偏导数 $r_{u}\; r_{v}\; r_{uu}\; r_{uv}\; r_{vv}$ ，曲面的单位法矢 $n$ 为：

$n=\frac{r_{u} \times r_{v}}{|r_{u} \times r_{v}|}$ (7)

曲面的第一、第二基本量为：

$E= r_{u}^{2},\; F=r_{u} r_{v},\; G=r^{2}$

$L= nr_{uu},\; M=nr_{uv},\; N=nr_{vv}$ (8)

则，

平均曲率为：

$H=\frac{EN-2FM+GL}{2(EG-F^{2})}$

高斯曲率为：

$K=\frac{LN-M^{2}}{EG-F^{2}}$

主曲率为：

$k_{1},k_{2}=H\pm \sqrt{H^{2}-K}$

三、实现代码

#include <iostream>
#include <pcl/io/pcd_io.h> 
#include <pcl/point_types.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/features/boundary.h>
#include <pcl/filters/normal_space.h>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <vector>
#include <math.h>
#include <pcl/common/eigen.h>
#include <pcl/point_cloud.h>
#include <pcl/features/principal_curvatures.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>

using namespace std;
using namespace Eigen;

//曲面拟合计算点云曲率
void surfacefitting(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr& curvacloud, int k_neborradius, vector<double>& curvature_thereshold)
{

	//Mean_curvature.resize(cloud->size(), 1);//初始化矩阵 cloud->size * 1；
	//Gauss_curvature.resize(cloud->size(), 1);
	Matrix<double, 6, 6>Coefficient_matrix; //求解方程组系数矩阵
	Matrix<double, 6, 6>Inverse_Coefficient_matrix;		//系数矩阵的逆矩阵
	Matrix<double, 6, 1>Value_matrix;					//方程组右值矩阵
	pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
	kdtree.setInputCloud(cloud);
	vector<int> pointIdRadiusSearch;             //点号
	vector<float> pointRadiusSquaredDistance;	 //距离

	for (int i = 0; i < cloud->points.size(); i++)
	{
		kdtree.nearestKSearch(cloud->points[i], k_neborradius, pointIdRadiusSearch, pointRadiusSquaredDistance);
		double Target_x = 0, Target_y = 0, Target_z = 0;//数据点的坐标值
		Coefficient_matrix.setZero();					//初始化矩阵元素全为0
		Inverse_Coefficient_matrix.setZero();
		Value_matrix.setZero();
		double a = 0, b = 0, c = 0, d = 0, e = 0, f = 0;//二次曲面方程系数
		double u = 0, v = 0;							//二次曲面参数方程参数
		double E = 0, G = 0, F = 0, L = 0, M = 0, N = 0;//曲面第一、第二基本量

		Target_x = cloud->points[i].x;
		Target_y = cloud->points[i].y;
		Target_z = cloud->points[i].z;

		double Meancurvature = 0, Gausscurvature = 0;	//平均曲率、高斯曲率

		//遍历数据点i的邻域点
		for (int j = 0; j < pointIdRadiusSearch.size(); j++)
		{
			Coefficient_matrix(0, 0) += (cloud->points[pointIdRadiusSearch[j]].x * cloud->points[pointIdRadiusSearch[j]].x * cloud->points[pointIdRadiusSearch[j]].x * cloud->points[pointIdRadiusSearch[j]].x);
			Coefficient_matrix(0, 1) += (cloud->points[pointIdRadiusSearch[j]].x * cloud->points[pointIdRadiusSearch[j]].x * cloud->points[pointIdRadiusSearch[j]].x * cloud->points[pointIdRadiusSearch[j]].y);
			Coefficient_matrix(0, 2) += (cloud->points[pointIdRadiusSearch[j]].x * cloud->points[pointIdRadiusSearch[j]].x * cloud->points[pointIdRadiusSearch[j]].y * cloud->points[pointIdRadiusSearch[j]].y);
			Coefficient_matrix(0, 3) += (cloud->points[pointIdRadiusSearch[j]].x * cloud->points[pointIdRadiusSearch[j]].x * cloud->points[pointIdRadiusSearch[j]].x);
			Coefficient_matrix(0, 4) += (cloud->points[pointIdRadiusSearch[j]].x * cloud->points[pointIdRadiusSearch[j]].x * cloud->points[pointIdRadiusSearch[j]].y);
			Coefficient_matrix(0, 5) += (cloud->points[pointIdRadiusSearch[j]].x * cloud->points[pointIdRadiusSearch[j]].x);
			Coefficient_matrix(1, 1) += (cloud->points[pointIdRadiusSearch[j]].x * cloud->points[pointIdRadiusSearch[j]].x * cloud->points[pointIdRadiusSearch[j]].y * cloud->points[pointIdRadiusSearch[j]].y);
			Coefficient_matrix(1, 2) += (cloud->points[pointIdRadiusSearch[j]].x * cloud->points[pointIdRadiusSearch[j]].y * cloud->points[pointIdRadiusSearch[j]].y * cloud->points[pointIdRadiusSearch[j]].y);
			Coefficient_matrix(1, 3) += (cloud->points[pointIdRadiusSearch[j]].x * cloud->points[pointIdRadiusSearch[j]].x * cloud->points[pointIdRadiusSearch[j]].y);
			Coefficient_matrix(1, 4) += (cloud->points[pointIdRadiusSearch[j]].x * cloud->points[pointIdRadiusSearch[j]].y * cloud->points[pointIdRadiusSearch[j]].y);
			Coefficient_matrix(1, 5) += (cloud->points[pointIdRadiusSearch[j]].x * cloud->points[pointIdRadiusSearch[j]].y);
			Coefficient_matrix(2, 2) += (cloud->points[pointIdRadiusSearch[j]].y * cloud->points[pointIdRadiusSearch[j]].y * cloud->points[pointIdRadiusSearch[j]].y * cloud->points[pointIdRadiusSearch[j]].y);
			Coefficient_matrix(2, 3) += (cloud->points[pointIdRadiusSearch[j]].x * cloud->points[pointIdRadiusSearch[j]].y * cloud->points[pointIdRadiusSearch[j]].y);
			Coefficient_matrix(2, 4) += (cloud->points[pointIdRadiusSearch[j]].y * cloud->points[pointIdRadiusSearch[j]].y * cloud->points[pointIdRadiusSearch[j]].y);
			Coefficient_matrix(2, 5) += (cloud->points[pointIdRadiusSearch[j]].y * cloud->points[pointIdRadiusSearch[j]].y);
			Coefficient_matrix(3, 3) += (cloud->points[pointIdRadiusSearch[j]].x * cloud->points[pointIdRadiusSearch[j]].x);
			Coefficient_matrix(3, 4) += (cloud->points[pointIdRadiusSearch[j]].x * cloud->points[pointIdRadiusSearch[j]].y);
			Coefficient_matrix(3, 5) += (cloud->points[pointIdRadiusSearch[j]].x);
			Coefficient_matrix(4, 4) += (cloud->points[pointIdRadiusSearch[j]].y * cloud->points[pointIdRadiusSearch[j]].y);
			Coefficient_matrix(4, 5) += (cloud->points[pointIdRadiusSearch[j]].y);
			Coefficient_matrix(5, 5) += 1;
			Value_matrix(0, 0) += (cloud->points[pointIdRadiusSearch[j]].z * cloud->points[pointIdRadiusSearch[j]].x * cloud->points[pointIdRadiusSearch[j]].x);
			Value_matrix(1, 0) += (cloud->points[pointIdRadiusSearch[j]].z * cloud->points[pointIdRadiusSearch[j]].y * cloud->points[pointIdRadiusSearch[j]].x);
			Value_matrix(2, 0) += (cloud->points[pointIdRadiusSearch[j]].z * cloud->points[pointIdRadiusSearch[j]].y * cloud->points[pointIdRadiusSearch[j]].y);
			Value_matrix(3, 0) += (cloud->points[pointIdRadiusSearch[j]].z * cloud->points[pointIdRadiusSearch[j]].x);
			Value_matrix(4, 0) += (cloud->points[pointIdRadiusSearch[j]].z * cloud->points[pointIdRadiusSearch[j]].y);
			Value_matrix(5, 0) += (cloud->points[pointIdRadiusSearch[j]].z);
		}
		Coefficient_matrix(0, 0) += Target_x * Target_x * Target_x * Target_x;
		Coefficient_matrix(0, 1) += Target_x * Target_x * Target_x * Target_y;
		Coefficient_matrix(0, 2) += Target_x * Target_x * Target_y * Target_y;
		Coefficient_matrix(0, 3) += Target_x * Target_x * Target_x;
		Coefficient_matrix(0, 4) += Target_x * Target_x * Target_y;
		Coefficient_matrix(0, 5) += Target_x * Target_x;
		Coefficient_matrix(1, 1) += Target_x * Target_x * Target_y * Target_y;
		Coefficient_matrix(1, 2) += Target_x * Target_y * Target_y * Target_y;
		Coefficient_matrix(1, 3) += Target_x * Target_x * Target_y;
		Coefficient_matrix(1, 4) += Target_x * Target_y * Target_y;
		Coefficient_matrix(1, 5) += Target_x * Target_y;
		Coefficient_matrix(2, 2) += Target_y * Target_y * Target_y * Target_y;
		Coefficient_matrix(2, 3) += Target_x * Target_y * Target_y;
		Coefficient_matrix(2, 4) += Target_y * Target_y * Target_y;
		Coefficient_matrix(2, 5) += Target_y * Target_y;
		Coefficient_matrix(3, 3) += Target_x * Target_x;
		Coefficient_matrix(3, 4) += Target_x * Target_y;
		Coefficient_matrix(3, 5) += Target_x;
		Coefficient_matrix(4, 4) += Target_y * Target_y;
		Coefficient_matrix(4, 5) += Target_y;
		Coefficient_matrix(5, 5) += 1;
		Value_matrix(0, 0) += Target_x * Target_x * Target_z;
		Value_matrix(1, 0) += Target_z * Target_x * Target_y;
		Value_matrix(2, 0) += Target_z * Target_y * Target_y;
		Value_matrix(3, 0) += Target_z * Target_x;
		Value_matrix(4, 0) += Target_z * Target_y;
		Value_matrix(5, 0) += Target_z;
		Coefficient_matrix(1, 0) = Coefficient_matrix(0, 1);
		Coefficient_matrix(2, 0) = Coefficient_matrix(0, 2);
		Coefficient_matrix(2, 1) = Coefficient_matrix(1, 2);
		Coefficient_matrix(3, 0) = Coefficient_matrix(0, 3);
		Coefficient_matrix(3, 1) = Coefficient_matrix(1, 3);
		Coefficient_matrix(3, 2) = Coefficient_matrix(2, 3);
		Coefficient_matrix(4, 0) = Coefficient_matrix(0, 4);
		Coefficient_matrix(4, 1) = Coefficient_matrix(1, 4);
		Coefficient_matrix(4, 2) = Coefficient_matrix(2, 4);
		Coefficient_matrix(4, 3) = Coefficient_matrix(3, 4);
		Coefficient_matrix(5, 0) = Coefficient_matrix(0, 5);
		Coefficient_matrix(5, 1) = Coefficient_matrix(1, 5);
		Coefficient_matrix(5, 2) = Coefficient_matrix(2, 5);
		Coefficient_matrix(5, 3) = Coefficient_matrix(3, 5);
		Coefficient_matrix(5, 4) = Coefficient_matrix(4, 5);
		Inverse_Coefficient_matrix = Coefficient_matrix.inverse();

		for (int l = 0; l < 6; ++l)
		{
			a += Inverse_Coefficient_matrix(0, l) * Value_matrix(l, 0);
		}
		for (int l = 0; l < 6; ++l)
		{
			b += Inverse_Coefficient_matrix(1, l) * Value_matrix(l, 0);
		}
		for (int l = 0; l < 6; ++l)
		{
			c += Inverse_Coefficient_matrix(2, l) * Value_matrix(l, 0);
		}
		for (int l = 0; l < 6; ++l)
		{
			d += Inverse_Coefficient_matrix(3, l) * Value_matrix(l, 0);
		}
		for (int l = 0; l < 6; ++l)
		{
			e += Inverse_Coefficient_matrix(4, l) * Value_matrix(l, 0);
		}
		for (int l = 0; l < 6; ++l)
		{
			f += Inverse_Coefficient_matrix(5, l) * Value_matrix(l, 0);
		}
		//根据所求曲面方程的系数计算曲面第一第二基本量
		u = 2 * a * cloud->points[i].x + b * cloud->points[i].y + d;
		v = 2 * c * cloud->points[i].y + b * cloud->points[i].x + e;
		E = 1 + u * u;
		F = u * v;
		G = 1 + v * v;
		double u_v = sqrt(1 + u * u + v * v);
		L = (2 * a) / u_v;
		M = b / u_v;
		N = (2 * c) / u_v;
		//Gauss_curvature
		Gausscurvature = (L * N - M * M) / (E * G - F * F);
		//Mean_curvature
		Meancurvature = (E * N - 2 * F * M + G * L) / (2 * E * G - 2 * F * F);
		curvature_thereshold.push_back(abs(Meancurvature));
		cout << "The " << i << " point, Gauss_curvature = " << Gausscurvature << "Mean_curvature = " << Meancurvature << endl;
	}
}

int main(int argc, char** argv)
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloudout(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::io::loadPCDFile<pcl::PointXYZ>("bunny.pcd", *cloud);
	vector<double> Meancurvature;
	surfacefitting(cloud, cloudout, 10, Meancurvature);
	vector<double> curvatureTem = Meancurvature;
	sort(curvatureTem.begin(), curvatureTem.end());
	int radio = Meancurvature.size() * 0.8;  //控制提取比列
	cout << radio;
	for (int i = 0; i < cloud->size(); i++)
	{
		if (Meancurvature[i] > curvatureTem[radio])
		{
			pcl::PointXYZ pt;
			pt.x = cloud->points[i].x;
			pt.y = cloud->points[i].y;
			pt.z = cloud->points[i].z;
			cloudout->push_back(pt);
		}
	}

	pcl::io::savePCDFile("extract.pcd", *cloudout);
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("Point Cloud visual"));
	int showBefore = 0;
	int showAfter = 1;
	viewer->createViewPort(0, 0, 0.5, 1, showBefore);
	viewer->createViewPort(0.5, 0, 1, 1, showAfter);
	viewer->setBackgroundColor(0, 0, 0, showBefore);
	viewer->setBackgroundColor(0.05, 0, 0, showAfter);
	//原始点云
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h(cloud, 0, 255, 0);
	//输出点云
	pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> after_sac(cloudout, 0, 0, 255);

	viewer->setBackgroundColor(0, 0, 0);
	viewer->addPointCloud(cloud, src_h, "source cloud", showBefore);
	viewer->addPointCloud(cloudout, after_sac, "target cloud1", showAfter);
	while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(10000));
	}

}

四、最终结果

点云处理

关注

22
点赞
踩
34

收藏

觉得还不错? 一键收藏
打赏
3
评论
二次曲面拟合计算点云法曲率、主曲率以及平均曲率（手写版）

采用KD树结构对点云数据建立k领域。在散乱点参数化的基础上,对k领域内的点进行二次曲面拟合,求出拟合平面的平均曲率、主曲率以及高斯曲率，为点云特征提取分类提供后续支撑，手写版
复制链接

扫一扫