最小二乘法平面拟合

最小二乘法平面拟合
在介绍平面拟合之前，我先给大家介绍一下有关平面的相关知识（相关介绍来自QVPak 3D,日本三丰）
Definition of the Plane Feature
A plane feature is reported as the projection of the centroid of the points used to fit the plane, which is projected onto the plane feature, a measurement of the direction measured as an angle, a measurement of the flatness of the plane and a measurement of the parallelism of the plane.
If measured in a Cartesian coordinate system, the coordinates of the plane's centroid are reported as follows:
X: The distance from the origin to the centroid, as measured along the x-axis.
Y: The distance from the origin to the centroid, as measured along the y-axis.
Z: The distance from the origin to the centroid, as measured along the z-axis.
If measured in a Cylindrical coordinate system, the coordinates of the plane's centroid are reported as follows:
R: The distance from the z-axis of the coordinate system to the centroid, as measured within a plane which contains the centroid and is orthogonal to the z-axis of the coordinate system.
A: The direction, measured as an angle, between a reference radius vector and a radius vector that contains the centroid and is projected onto the xy-plane. The reference radius vector may be considered to be the x-axis.
Z: The height from the origin to the centroid in the cylindrical coordinate system, as measured along the z-axis.
The other attributes of the plane feature are:
Angle: The angle between the projection of the plane’s normal vector onto the xy-plane and the x-axis of the current coordinate system.
X-angle: The angle between the plane’s normal vector and the x-axis of the current coordinate system. (X-Angle = arc cosine k). The x-angle is a positive number between 0 and 180 degrees.
Y-angle: The angle between the plane’s normal vector and the y-axis of the current coordinate system. (Y-Angle = arc cosine l). The y-angle is a positive number between 0 and 180 degrees.
Z-angle: The angle between the plane’s normal vector and the z-axis of the current coordinate system. (Z-Angle = arc cosine m). The z-angle is a positive number between 0 and 180 degrees.
Flatness: Flatness is a condition for which an element of a surface is in a plane.
Flatness is reported as the width of the zone formed by two closest parallel planes that fully contain the point set used to fit the plane feature. A value of zero indicates perfect flatness.
Flatness (minimum): The distance from the fitted plane to the measured point farthest below the fitted plain in the point set. Above and below are determined by the direction of the plane vector. See Explanation of Max/Min distance in different cases.
Flatness (maximum): The distance from the fitted plane to the measured point farthest above the fitted plain in the point set. Above and below are determined by the direction of the plane vector. See Explanation of Max/Min distance in different cases.
Parallelism: The condition of a feature, projected to a certain plane, being equidistant at all elements from a datum (reference). Quantitatively, parallelism is defined as the absolute distant difference between the farthest and closest points from the datum.
Parallelism is evaluated relative to a reference line or xy-plane. When evaluating a set of points with a reference line, parallelism uses the projections of the points and reference line onto the xy-plane in the current coordinate system, or z/ref plane feature, and is specified as a zone tolerance. The z/ref plane feature is a plane including the reference line and parallel to (or including) the z-axis. When evaluating a set of points with a xy-plane, parallelism is calculated in three-dimensional space.
Parallelism (minimum): The distance from the referenced line or plane to the point in the point set with the least value (least positive value if all evaluated points are positive, or most negative value if evaluated
points include negative values). See Explanation of Max/Min distance in different cases.
Parallelism (maximum): The distance from the reference line or plane to the point in the point set with the greatest value (most positive value if evaluated points include positive values, or least negative value if all evaluated points are negative). See Explanation of Max/Min distance in different cases.
平面相关知识：
算法如下：
VB源代码：
Option Explicit
Public Const PI = 3.1415926535897
Public Type tagPoint
x As Double
y As Double
z As Double
End Type
Public Type tagLine2D
k As Double 'Slope ,K is the "K" of "y=kx+b"
b As Double 'intercept,B is the "B" of "y=kx+b"
Angle As Double 'arctg(k) '0 to 180 deg
Straightness As Double
RSQ As Double
End Type
Public Type tagLine3D
'3D line's formula is showing as following.
'1)|Ax +By +Z+D =0
' |A1x+B1y+z+D1=0
'2)(x-x0)/m=(y-y0)/n=(z-z0)/p----(x-x0)/m=(y-y0)/n=z/1
'3)x=mt+x0,y=nt+y0,z=pt+z0
'Only point's coordinate is (a+b*Z, c+d*Z,Z),so the line's vector is {b,d,1}
x As Double
y As Double
z As Double
x0 As Double ' Added on Jul.1st,2009
y0 As Double ' Added on Jul.1st,2009
z0 As Double
m As Double '(x-x0)/m=(y-y0)/n=(z-z0)/p ,same as :z=a+bx ,z=c+dy
n As Double
p As Double 'vector s={m,n,p}
Angle As Double
xAn As Double
yAn As Double
zAn As Double
Straightness As Double
MinSt As Double
MaxSt As Double
End Type
Public Type tagCircle
x As Double
y As Double
z As Double
r As Double
d As Double
Circular As Double
End Type
Public Type tagVector
a As Double
b As Double
c As Double
End Type
Type tagPlane
x As Double 'The distance from the origin to the centroid, as measured along the x-axis.
y As Double 'The distance from the origin to the centroid, as measured along the y-axis
z As Double 'The distance from the origin to the centroid, as measured along the z-axis
'Z + A*x + B*y + C =0 z's coefficient is just 1
ax As Double 'coefficient of X
by As Double 'coefficient of Y
cz As Double
d As Doubl
e 'Constant C
Angle As Double
xAn As Double
YAn as Double
zAn As Double
Flat As Double
MinFlat As Double
End Type
'*************************************************************
' 函数名：PlaneSet
' 功能：求拟合平面（相关参数）
' 参数： dataRaw - tagPoint型自定义点类型(x,y,z)，数组
' PlaneSet – tagPlane 其值返回为平面圆相关参数' 返回值：Long型，失败为0，成功为-1
'*************************************************************
Public Function PlaneSet(dataRaw() As tagPoint, Plane As tagPlane) As Long 'z+Ax+BY+C=0
PlaneSet = 0
Dim lb As Long, ub As Long, MaxF As Double, MinF As Double, tmp As Double
lb = LBound(dataRaw): ub = UBound(dataRaw)
If ub - lb + 1 < 4 Then Exit Function
Dim i As Long, n As Long
n = ub - lb + 1
Dim x As Double, y As Double, z As Double
Dim XY As Double, XZ As Double, YZ As Double
Dim X2 As Double, Y2 As Double
Dim a As Double, b As Double, c As Double, d As Double
Dim a1 As Double, b1 As Double, z1 As Double
Dim a2 As Double, b2 As Double, z2 As Double
Dim n1 As tagVector '{.ax,by,1} s1
Dim n2 As tagVector '{0,0,N} XY plane s2
Dim n3 As tagVector 'line projected plane
Dim SLine As tagVector
Dim xLine As tagVector '{1,0,0}
Dim yLine As tagVector '{1,0,0}
Dim zLine As tagVector '{1,0,0}
Dim VectorPlane As tagVector
xLine.a = 1: xLine.b = 0: xLine.c = 0
yLine.a = 0: yLine.b = 1: yLine.c = 0
zLine.a = 0: zLine.b = 0: zLine.c = 1
For i = lb To ub
With dataRaw(i)
x = x + .x
y = y + .y
z = z + .z
XY = XY + .x * .y
XZ = XZ + .x * .z
YZ = YZ + .y * .z
X2 = X2 + .x ^ 2
Y2 = Y2 + .y ^ 2
End With
Next i
z1 = n * XZ - x * z 'e=z-Ax-By-C z=Ax+By+D
a1 = n * X2 - x ^ 2
b1 = n * XY - x * y
z2 = n * YZ - y * z
a2 = n * XY - x * y
b2 = n * Y2 - y ^ 2
a = (z1 * b2 - z2 * b1) / (a1 * b2 - a2 * b1)
b = (a1 * z2 - a2 * z1) / (a1 * b2 - a2 * b1)
c = 1
d = (z - a * x - b * y) / n
With Plane
.x = x / (ub - lb + 1)
.y = y / (ub - lb + 1)
.z = z / (ub - lb + 1)
'sum(Mi *Ri)/sum(Mi) ,Mi is mass . here Mi is seted to be 1 and .z is just the average of z
.Ax = -a
.By = -b
.Cz = 1
.d = -d 'z=Ax+By+D-----Ax+By+Z+D=0
VectorPlane.a = .Ax: VectorPlane.b = .By: VectorPlane.c = 1
.xAn = Intersect(VectorPlane, xLine)
.yAn = Intersect(VectorPlane, yLine)
.zAn = Intersect(VectorPlane, zLine)
n1.a = .Ax: n1.b = .By: n1.c = 1
SLine.a = .Ax: SLine.b = .By: SLine.c = 0
.Angle = Intersect(xLine, SLine) '(xLine.A * SLine.A + xLine.A * SLine.B + xLine.C * SLine.C)
If SLine.b < 0 Then .Angle = 360 - .Angle
For i = lb To ub
PointToPlane dataRaw(i), Plane, tmp, 0
If i = lb Then
MaxF = tmp: MinF = tmp
Else
If MaxF < tmp Then MaxF = tmp
If MinF > tmp Then MinF = tmp
End If
Next i
.MaxFlat = MaxF
.MinFlat = MinF
.Flat = MaxF - MinF
End With
PlaneSet = -1
End Function
' 函数名：VectorMulti
' 功能：求两向量的积,结果存放于参数rtV3中
' 参数： v1 - tagVector
' v2 - tagVector
' rtV3 -tagVector
'*************************************************************
Public Function VectorMulti(v1 As tagVector, v2 As tagVector, rtv3 As tagVector) As Long
'rtV3=v1*v2=|i j k |
'|v1.A v1.B v1.C|
'|v2.A v2.B v2.C|
'rtv3.A=(v1.B*v2.c-v2.B*v1.C) 'i
'rtV3.B=-(v1.A*v2.C-v2.A*v1.C) 'j
'rtV3.C=(v1.A*v2.B-v2.A*V1.B) 'k
rtv3.a = (v1.b * v2.c - v2.b * v1.c)
rtv3.b = -(v1.a * v2.c - v2.a * v1.c)
rtv3.c = (v1.a * v2.b - v2.a * v1.b)
End Function
'************************************************************
' 函数名：VectorN
' 功能：求两向量的数量积,结果存放于参数rtV3中
' 参数： v1 - tagVector
' v2 - tagVector
' rtV3 -tagVector
'*************************************************************
Public Function VectorN(v1 As tagVector, v2 As tagVector, rtv3 As tagVector) As Long
rtv3.a = v1.a * v2.a
rtv3.b = v1.b * v2.b
rtv3.c = v1.c * v2.c
End Function
'*************************************************************
' 函数名：Intersect
' 功能：求两向量之间的夹角
' 参数： v1 - tagVector
' v2 - tagVector
' LinePlane -long 0:表示两直线之间的夹角,其它值:表示如线与平面之间,平面与平面之间的夹角(0~90)
' 返回值：Double型,单位:度.
'*************************************************************
Public Function Intersect(v1 As tagVector, v2 As tagVector, Optional LinePlane As Long = 0) As Double
'LinePlane 0 :line -line ,1:line --Plane
Dim tmp As Double, tmpSqr1 As Double, tmpSqr2 As Double
tmp = (v1.a * v2.a + v1.b * v2.b + v1.c * v2.c)
'MsgBox tmp
tmpSqr1 = Sqr(v1.a ^ 2 + v1.b ^ 2 + v1.c ^ 2)
tmpSqr2 = Sqr(v2.a ^ 2 + v2.b ^ 2 + v2.c ^ 2)
If tmpSqr1 <> 0 Then
If tmpSqr2 <> 0 Then
tmp = tmp / tmpSqr1 / tmpSqr2
Else
tmp = tmp / tmpSqr1
End If
Else
If tmpSqr2 <> 0 Then
tmp = tmp / tmpSqr2
Else
tmp = 0
End If
End If
If LinePlane <> 0 Then
tmp = Abs(tmp)
End If
If -tmp * tmp + 1 <> 0 Then
tmp = Atn(-tmp / Sqr(-tmp * tmp + 1)) + 2 * Atn(1)
tmp = tmp / PI * 180
Else
tmp = 90
End If
Intersect = tmp
End Function
'*************************************************************
' 函数名：PointToPlane
' 功能：求点到平面的距离
' 参数： dataRaw - tagPoint型自定义点类型(x,y,z)
' Plane - tagPlane Double
' RtnDistance -Double 其值为点到直线的距离. ' AbsDist -Long ０：点到平面距离（有正有负），其它值：点到平面距离（绝对值）
' 返回值：Long型，失败为0，成功为-1
'*************************************************************
Public Function PointToPlane(dataRaw As tagPoint, Plane As tagPlane, rtnDistance As Double, Optional AbsDist As Long = 0) As Long Dim i As Long, lb As Long, ub As Long, tmp As Double
With Plane
tmp = (.ax *dataRaw.x + .by *dataRaw.y + .cz * dataRaw.z + .d)/ Sqr(.ax ^ 2 + .by ^ 2 + .cz ^ 2)
If AbsDist <> 0 Then
tmp = Abs(tmp)
End If
End With
rtnDistance = tmp
End Function
'*************************************************************
' 函数名：PointToLine
' 功能：求点到直线的距离
' 参数： dataRaw - tagPoint型自定义点类型(x,y,z)
' nLine3D - tagLine3D Double
' RtnDistance -Double 其值为点到直线的距离. ' 返回值：Long型，失败为0，成功为-1
'*************************************************************
Public Function PointToLine(dataRaw As tagPoint, nLine3D As tagLine3D, rtnDistance As Double) As Long
Dim tmp As Double, d As Double, t As Double
Dim Dataraw0 As tagPoint
With nLine3D
'直线{m,n,1}为平面:ax+by+z+d=0的法线,所以平面法线向量{a,b,1}={m,n,1}
'点(dataRaw)过平面:ax+by+z+d=0, 求出d
d = -.m * dataRaw.x - .n * dataRaw.y - .p * dataRaw.z '.p=1
'直线与平面相交,将(x-xo)/m=(y-y0)/n=z=t ' x=m*t+x0 ; y=n*t+yo z=t 代入平面, 求得t
t = -(d + .m * .x0 + .n * .y0) / (.m ^ 2 + .n ^ 2 + 1)
Dataraw0.x = .m * t + .x0
Dataraw0.y = .n * t + .y0
Dataraw0.z = .p * t + .z0
rtnDistance = Sqr((Dataraw0.x - dataRaw.x) ^ 2 + _
(Dataraw0.y - dataRaw.y) ^ 2 + _
(Dataraw0.z -
dataRaw.z) ^ 2)
End With End Function。