离散点云拟合三维平面参数推导（基于最小二乘）

embedded/2024/9/22 23:16:22/

1、背景介绍

实际中，很多人工构造物是由平面结构构造而成，如下图所示，为一典型的由多个平面组成的人工构筑物。因此，根据离散点拟合成平面，获取拟合平面方程，是点云数据处理中非常常见的数据处理操作。

2、平面拟合参数推导

基于若离散点，估算平面方程过程如下：

3、程序设计

基于上述理论，使用C++、PCL编写的程序，下载链接如下：https://download.csdn.net/download/qq_32867925/89381863。

3.1 已知平面方程拟合测试

本次首先给定平面方程为x+2y+3z+4=0 （x+4y+5z+6=0）的平面，在平面上随机生成50个点，利用这50个点拟合平面方程。部分结果与代码如下：

int main()
{//a*x+b*y+c*z+d=0  平面方程形式double a, b, c, d;a = 1;b = 4;c = 5;d = 6;//随机生成50个点// 初始化随机数生成器srand(static_cast<unsigned int>(time(nullptr)));vector<pcl::PointXYZ> pts;for (int i = 0; i < 50; i++){pcl::PointXYZ pt;pt.x= static_cast<double>(rand()) / RAND_MAX;pt.y = static_cast<double>(rand()) / RAND_MAX;pt.z = (-d - a * pt.x - b * pt.y) / c;pts.push_back(pt);}NormalEigenvalueCurvature planeparam = EstimateNormalEigenvalue(pts);cout << "在平面方程为" << a << "x+" << b << "y+" << c << "z+" << d << "=0" << "平面上产生的点,经过拟合后得到的平面参数如下：" << endl;cout << "拟合参数 a b c d 分别如下：" << endl << "a=" << planeparam.normal_x << "  b=" << planeparam.normal_y << "  c=" << planeparam.normal_z << "  d=" << planeparam.d << endl;system("pause");return 0;
}

根据拟合可知，拟合得到的平面参数，与实现给定平面方程，几乎一致，只是参数a、b、c进行了单位归一化。证明了本方法的正确性。

3.2 法向量估算

根据推导过程可知，平面方程ax+by+cz+d=0中参数a、b、c为平面法向量。使用kdtree搜索点云近邻点，利用近邻点拟合得到局部平面，将局部平面的法向量进行可视化，可以进一步验证估算法向量是否正确。部分代码与结果如下：

将估算的参数a、b、c作为法向量，并进行可视化代码示意：

vector<NormalEigenvalueCurvature> ptsNormal = PtsEstimateNormalEigenvalue(pts, 15);//估算每个点平面方程//增加点云法向量pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);normals->width = pts.size();normals->height = 1;normals->resize(normals->width * normals->height);for (int i = 0; i < ptsNormal.size(); i++){double normal_x = ptsNormal[i].normal_x;double normal_y = ptsNormal[i].normal_y;double normal_z = ptsNormal[i].normal_z;double length = sqrt(normal_x * normal_x + normal_y * normal_y + normal_z * normal_z);normal_x = normal_x / length;normal_y = normal_y / length;normal_z = normal_z / length;normals->points[i].normal_x = normal_x;normals->points[i].normal_y = normal_y;//将法向量调整都朝向if (normal_z >= 0){normals->points[i].normal_z = normal_z;}else{normals->points[i].normal_z = -normal_z;}}