三维空间中散点平面拟合方法

1、三点求平面方程、平面法向量和点到平面的距离
已知三点p1（x1,y1,z1），p2(x2,y2,z2)，p3(x3,y3,z3)，

要求确定的平面方程,关键在于求出平面的一个法向量

为此做向量p1p2（x2-x1,y2-y1,z2-z1), p1p3(x3-x1,y3-y1,z3-z1),平面法线和这两个向量垂直，因此法向量n：
在这里插入图片描述
平面方程：a(x-x1)+b(y-y1)+ c(z-z1)=0；

d=-ax1-by1-c*z1。
平面平面方程为 ax+by+cz+d=0。

//已知3点坐标，求平面ax+by+cz+d=0;
 
void get_panel(Point p1,Point p2,Point p3,double &a,double &b,double &c,double &d)
 
{
    
    
 
    a = (p2.y - p1.y)*(p3.z - p1.z) - (p2.z - p1.z)*(p3.y - p1.y);
 
    b = (p2.z - p1.z)*(p3.x - p1.x) - (p2.x - p1.x)*(p3.z - p1.z);
 
    c = (p2.x - p1.x)*(p3.y - p1.y) - (p2.y - p1.y)*(p3.x - p1.x);
 
    d = 0 - (a * p1.x + b*p1.y + c*p1.z);
 
}
 
 
// 已知三点坐标，求法向量
 
Vec3 get_Normal(Point p1,Point p2,Point p3)
 
{
    
    
 
    a = (p2.y - p1.y)*(p3.z - p1.z) - (p2.z - p1.z)*(p3.y - p1.y);
 
    b = (p2.z - p1.z)*(p3.x - p1.x) - (p2.x - p1.x)*(p3.z - p1.z);
 
    c = (p2.x - p1.x)*(p3.y - p1.y) - (p2.y - p1.y)*(p3.x - p1.x);
 
    return Vec3(a, b, c);
 
}
 
 
//点到平面距离
 
double dis_pt2panel(Point pt,double a,double b,double c,double d)
{
    
    
 
    return f_abs(a * pt.x + b*pt.y + c*pt.z + d) / sqrt(a * a + b * b + c * c);
 
}

设空间中一组带噪声的位于某个平面的散点 $x_{i},y_{i},z_{i}),i=1,2,...,N，$ 对于平面方程的一般表达式 $A x + B y + C z + D = 0$ ，对于不过原点的平面有 $A x + B y + C z + 1 = 0$ ，于是问题就变成了求系数A，B，C使得散点尽可能的落在该平面上。下面笔者将从两种角度来推导最小二乘下的最优解。

求误差函数的极小值点的方法
设A，B，C为平面的最优参数，则对于每个点 $x_{i},y_{i},z_{i}),i=1,2,...,N$ 带人该平面表达式中存在误差 $e_{i}=Ax_{i}+By_{i}+Cz_{i}+1$ ，我们将所有的误差的平方进行求和则有：
设A，B，C为平面的最优参数，则对于每个点带人该平面表达式中存在误差，我们将所有的误差的平方进行求和则有：
在这里插入图片描述
可见误差的平方和为A,B,C的函数，我们通过观察可以很容易的发现该函数是4维空间中的一个下凸超平面，且具有唯一的极小值，同时也是全局最小值。那么E对A,B,C的一阶偏导都为0，即：

展开整理可得：
在这里插入图片描述
我们令： $\bar{x}=\frac{1}{N}\sum_{i=1}^{N}x_{i}，\bar{y}=\frac{1}{N}\sum_{i=1}^{N}y_{i}，\bar{z}=\frac{1}{N}\sum_{i=1}^{N}z_{i}，\bar{x^{2}}=\frac{1}{N}\sum_{i=1}^{N}x_{i}^{2},\bar{y^{2}}=\frac{1}{N}\sum_{i=1}^{N}y_{i}^{2},\bar{z^{2}}=\frac{1}{N}\sum_{i=1}^{N}z_{i}^{2}，\bar{xy}=\frac{1}{N}\sum_{i=1}^{N}x_{i}y_{i}，\bar{xz}=\frac{1}{N}\sum_{i=1}^{N}x_{i}z_{i}，\bar{yz}=\frac{1}{N}\sum_{i=1}^{N}y_{i}z_{i}$ 则（3）可以被写成如下形式：
在这里插入图片描述
可以看到（4）就是一个3元一次方程组，求解这个方程组就可以得到A,B,C的值。

几何的方法
下面来介绍如何使用几何的方法来进行推导，我们令
$X=\begin{bmatrix} x_{1}\\ x_{2}\\ ...\\ x_{N} \end{bmatrix}，{X}'=\begin{bmatrix} -1-By_{1}-Cz_{1}\\ -1-By_{2}-Cz_{2}\\ ...\\ -1-By_{N}-Cz_{N} \end{bmatrix}$

现在的问题是找到合适的参数A使得AX最接近{X}‘，它们的误差向量为AX-{X}’，从几何的角度来看当误差向量AX-{X}'与X正交时有最优系数，即
在这里插入图片描述
将(5)展开可得：

可以看到（6）式即为（3）中的第一项。
我们分别令
$Y=\begin{bmatrix} y_{1}\\ y_{2}\\ ...\\ y_{N} \end{bmatrix}，{Y}'=\begin{bmatrix} -1-Ax_{1}-Cz_{1}\\ -1-Ax_{2}-Cz_{2}\\ ...\\ -1-Ax_{N}-Cz_{N} \end{bmatrix}，Z=\begin{bmatrix} z_{1}\\ z_{2}\\ ...\\ z_{N} \end{bmatrix}，{Z}'=\begin{bmatrix} -1-Ax_{1}-By_{1}\\ -1-Ax_{2}-By_{2}\\ ...\\ -1-Ax_{N}-By_{N} \end{bmatrix}，同样地我们有：Y^{T}(BY-{Y}')=0，Z^{T}(CZ-{Z}')=0$ ，展开后发现它们分别是式（3）中的第2和第3项。

我们可以看到利用求误差函数的极小值方法与几何方法最后的最优最小二乘解的表达式是一样的。利用该方法我们很容易求得空间中三维点云构成的物体在局部的法向量，从而进行相关的计算，例如，计算该点的声波散射分布情况，或者是计算该点光波散射情况。

三维空间中散点平面拟合方法

猜你喜欢