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

本文主要是介绍二次曲面拟合计算点云法曲率、主曲率以及平均曲率（手写版），希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

一、相关原理及介绍

二、计算流程

三、实现代码

四、最终结果

参考文献：[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();					//初始化矩阵元素全为0Inverse_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_curvatureGausscurvature = (L * N - M * M) / (E * G - F * F);//Mean_curvatureMeancurvature = (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));}}