目录
参考文献:[1]周煜,张万兵,杜发荣等.散乱点云数据的曲率精简算法[J].北京理工大学学报,2010,30(07):785-789.DOI:10.15918/j.tbit1001-0645.2010.07.021.
[2]蔡志敏,王晏民,黄明.基于KD树散乱点云数据的Guass平均曲率精简算法[J].测绘通报,2013(S1):44-46.
一、相关原理及介绍
从微分几何的角度来看,曲面曲率较高的区域,应该有较多的采样点表示,相反则应该由较少的采样点表示,曲率反映了曲面的基本特性。点云曲率特征是指在三维空间中,对于点云数据集中的每个点,通过计算其周围的几何形状来确定该点的曲率特征,曲率特征是点云数据集中非常重要的特征之一,它可以应用于点云中的分类、分割、配准、特征提取等应用中。
曲率特征可以分为平均曲率、高斯曲率以及主曲率。平均曲率作为微分几何中一个“外在的”弯曲测量标准,对一个曲面嵌入周围空间(比如二维曲面嵌入三维欧式空间)的曲率进行了区部描述。高斯曲率作为一个描述曲面的凹凸性质的量,当这个量变化程度较大的时候表面曲面内部变化也比较大,这就表明曲面的光滑程度越低。点云中任意一点都存在某曲面 逼近该点的邻域点云,一点处的曲率可用该点及其邻域点拟合的局部曲面曲率来表征。通过最小二乘拟合,可以用二次曲面来表征局部区域,计算每点处的主曲率 、,平均曲率 及高斯曲率 。
二、计算流程
设点云中某一点的邻域点集为,通过邻域点建立二次曲面参数方程:
(1)
令:
, (2)
式(1)可改写成: (3)
引入两个矩阵:
, (4)
测点的坐标为,选择邻域点数大于8,则逼近曲面和数据点的误差函数为:
(5)
运用最小二乘法,推到出系数矩阵为:
(6)
从而求出曲面的偏导数,曲面的单位法矢为:
(7)
曲面的第一、第二基本量为:
(8)
则,
平均曲率为:
高斯曲率为:
主曲率为:
三、实现代码
#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));
}
}