[转载]最小二乘法-3:空间直线

tmtony

摘录: 原作者 zenhileo

在看源代码之前，请多看看有关直线的相关参数及其算法。
方法一：的直线的拟合，可以认为是X--Z， Y--Z的两个（一般直线y=kx+b）的组合
方法二：我是借助人家的算法。

注：方法一与方法二算出结果有一点点差，我不知道是什么原因。

以下为VBA相关算法

Option Explicit

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
'A line feature is reported as the projection of the centroid of the data points used to fit the line, which is projected onto the line feature, the direction of measurement measured as an angle, and a measurement of the straightness of the line. If a projection plane (xy, yz, zx) is specified, the data points are projected onto the projection plane and then fitted to a line
'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
'Angle: The angle between the projection of the line onto the xy-plane and the x-axis of the current coordinate system.
'X-angle: The angle between the line 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 line 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 line 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.
'Straightness: Straightness is a condition for which an element of a surface or an axis is in a straight line.
'Straightness, in a 3-D definition, is reported as the width of the zone formed by a cylinder with diameter (t) with the constructed line feature as its axis. The zone's width is determined by finding the point that is the maximum distance from the constructed line. A value of zero indicates perfect straightness.
'Straightness, as used in a 2-D definition, is the distance between two closest parallel lines that fully contain the point set used to fit the line feature.
'Straightness (minimum): The distance from the fitted line to the nearest measured point in the point set. See Explanation of Max/Min distance in different cases.
'Straightness (maximum): The distance from the fitted line to the farthest measured point in the point set. See Explanation of Max/Min distance in different cases.
'Angularity: Angularity is a condition for where the line feature must be inclined at a specific angle with respect to a datum plane or datum axis.
'QVPAK reports the angularity of a line as the width of a zone formed when projecting the measured points onto the xy-plane or a z/ref plane. The z/ref plane feature is a plane including the reference line and parallel to (or including) the z-axis. A specified reference feature is also projected onto the plane, and a line feature is constructed inclined at a specified angle relative to the to the projection of the reference feature. The zone's width is determined by finding the distance between two lines which are parallel to the to the constructed line feature. The positions of these two parallel lines are determined by the finding the two projected points which have minimum and maximum distance from the constructed line. The value returned is the distance between the parallel lines. This value indicates the number of measurement units (mm or inches) by which the zone deviates from a straight line at the specified angle with respect to the reference feature.
'Line Angularity
'Parallelism: Parallelism is a special case of angularity, where the specified angle relative to the reference feature is zero (0) degrees.
'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.
'Perpendicularity: Perpendicularity is a special case of angularity, where the specified angle relative to the reference feature is ninety (90) degrees.

End Type

tmtony

Public Type tagCircle
   x As Double
   y As Double
   z As Double
   r As Double
   d As Double
   Circular As Double '
   'Circularity: Circularity is defined as the condition of a surface of revolution (for example, cylinders or cones) for which all points of the surface intersected by any plane perpendicular to the rotation axis are equidistant from the center.
   'QVPAK reports circularity as the width of the zone defined by the two closest concentric circles that bound the data points.

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 Double   'Constant C
    Angle As Double  'The angle(deg) between  the projection of the (fitted) plane's normal vector onto the XY-plane and the X-axis of the current coordinate system
        'The fitted plane's normal vector is n1={A,B,1} ,is also line s1
        'The XY plane's normal vector is  n2={0,0,N}, is also Line s2
        'Line s1 is projected on to plane xy-Plane,so Plane n3 = s1*s2 (the multiplication of two vetors)
        'the intersect line n4=s1*n3
        'the Angle(0,360) s3 =n4* Sxyplane  s3(0,180)
        'to dicide the line location in which. we should know value of B3 (A3,B3,C3)
        'if b3>0 then s2(0,180), b3<0 then (180,360)
        
    xAn As Double    'The angle(deg) between the (fitted) plane's normal vector and the X axis of the current coordinate system.(xAn=arc cosine K). The xAn is a positive number between 0 to 180
    yAn As Double    'The angle(deg) between the (fitted) plane's normal vector and the Y axis of the current coordinate system.(xAn=arc cosine K). The yAn is a positive number between 0 to 180
    zAn As Double    'The angle(deg) between the (fitted) plane's normal vector and the Z axis of the current coordinate system.(xAn=arc cosine K). The zAn is a positive number between 0 to 180
    Flat As Double   'Flatness is ka condition for which an element of a surface is in a plane
    'Flatness is reproted as the width of the zone formed by two closest parallel planes that fully contain the point int the point set used to fit the plane feature. A value of zero indicates perfect flatness.
    MinFlat As Double   '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
    MaxFlat As Double   'Flatness(maximum), the distance from the fitted plane to the measured point farthest above---  the fitted plain in the point set
End Type

tmtony

'*************************************************************
'  函数名：Line3DSet0
'  功能：  求拟合空间直线（相关参数）
'  参数：  dataRaw  - tagPoint型  自定义点类型(x,y,z)，数组
'          nLine  - tagLine3D  　其值返回为直线相关参数
'  返回值：Long型，失败为0，成功为-1
'*************************************************************
Function Line3DSet0(dataRaw() As tagPoint, nLine As tagLine3D) As Long
'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
    '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}
    Dim SumX, SumY, SumZ, SumXZ, SumYZ, SumZ2
    Dim TolN As Long, i As Long, j As Long, lb As Long, ub As Long
    Dim d, d1, d2
    Dim VecLine As tagVector
    Dim xLine As tagVector '{1,0,0}
    Dim yLine As tagVector '{0,1,0}
    Dim zLine As tagVector '{0,0,1}
    Dim maxS As Double, minS As Double, tmpD As Double
    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
    'Dim AvgX As Double, AvgY As Double
    lb = LBound(dataRaw): ub = UBound(dataRaw)
    If ub < 2 Then
       Line3DSet0 = 0
       Exit Function
    End If
    TolN = ub - lb + 1

    For i = lb To ub
       With dataRaw(i)
        SumX = SumX + .x
        SumY = SumY + .y
        SumZ = SumZ + .z
        SumXZ = SumXZ + .x * .z
        SumYZ = SumYZ + .y * .z
        SumZ2 = SumZ2 + .z ^ 2
       End With
    Next i
    d = TolN * SumZ2 - SumZ ^ 2
    d1 = SumX * SumZ2 - SumZ * SumXZ
    d2 = TolN * SumXZ - SumX * SumZ
    With nLine
        .p = 1
        .x0 = d1 / d
        .m = d2 / d
        
        d1 = SumY * SumZ2 - SumZ * SumYZ
        d2 = TolN * SumYZ - SumY * SumZ
        .y0 = d1 / d
        .n = d2 / d
        
        .z0 = 0
        .x = SumX / TolN
        .y = SumY / TolN
        .z = SumZ / TolN

       VecLine.a = .m: VecLine.b = .n: VecLine.c = 0
      .Angle = Intersect(VecLine, xLine)    '(xLine.A * SLine.A + xLine.A * SLine.B + xLine.C * SLine.C)
      If VecLine.b < 0 Then .Angle = 360 - .Angle
       VecLine.a = .m: VecLine.b = .n: VecLine.c = .p
       .xAn = Intersect(VecLine, xLine)
       .yAn = Intersect(VecLine, yLine)
       .zAn = Intersect(VecLine, zLine)
       For i = lb To ub
          Call PointToLine(dataRaw(i), nLine, tmpD)
          If i = lb Then
             maxS = tmpD
             minS = tmpD
          Else
             If tmpD > maxS Then maxS = tmpD
             If tmpD < minS Then minS = tmpD
          End If
          'Cells(i + 1, 15) = tmpD
       Next i
       .MaxSt = maxS
       .MinSt = minS
       .Straightness = 2 * maxS
    End With
    Line3DSet0 = -1
End Function

'*************************************************************
'  函数名：Line3DSet
'  功能：  求拟合空间直线（相关参数）
'  参数：  dataRaw  - tagPoint型  自定义点类型(x,y,z)，数组
'          nLine  - tagLine3D  　其值返回为直线相关参数
'  返回值：Long型，失败为0，成功为-1
'*************************************************************
Public Function Line3DSet(dataRaw() As tagPoint, nLine As tagLine3D) As Long
'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
    '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}

    Dim TolN As Long, i As Long, j As Long, lb As Long, ub As Long
    Dim VecLine As tagVector
    Dim xLine As tagVector '{1,0,0}
    Dim yLine As tagVector '{0,1,0}
    Dim zLine As tagVector '{0,0,1}
    Dim maxS As Double, minS As Double, tmpD As Double
    Dim a(1, 1) As Double, b(1, 1) As Double, c(1, 1) As Double
    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
    Dim SumXZ, SumX, SumYZ, SumY, SumZ2, SumZ
   
    lb = LBound(dataRaw): ub = UBound(dataRaw)
    TolN = ub - lb + 1
     If ub < 2 Then
       Line3DSet = 0
       Exit Function
    End If
    For i = lb To ub
       With dataRaw(i)
          SumXZ = SumXZ + .x * .z
          SumYZ = SumYZ + .y * .z
          SumX = SumX + .x
          SumY = SumY + .y
          SumZ = SumZ + .z
          SumZ2 = SumZ2 + .z ^ 2
         
       End With
    Next i
    a(0, 0) = SumXZ: a(0, 1) = SumX: a(1, 0) = SumYZ: a(1, 1) = SumY
    c(0, 0) = SumZ2: c(0, 1) = SumZ: c(1, 0) = SumZ: c(1, 1) = TolN
   
    Call sMInverse(b, c) ' to inverse matrix
   
    Call sMMult(c, a, b)  'to multiple two matrix
    '{m,x0}
    '{n,y0}
   
    With nLine
      .m = c(0, 0)
      .n = c(1, 0)
      .x0 = c(0, 1)
      .y0 = c(1, 1)
      .z0 = 0
      .p = 1
      .x = SumX / TolN
      .y = SumY / TolN
      .z = SumZ / TolN
       VecLine.a = .m: VecLine.b = .n: VecLine.c = 0
      .Angle = Intersect(VecLine, xLine)    '(xLine.A * SLine.A + xLine.A * SLine.B + xLine.C * SLine.C)
      If VecLine.b < 0 Then .Angle = 360 - .Angle
       VecLine.a = .m: VecLine.b = .n: VecLine.c = .p
       .xAn = Intersect(VecLine, xLine)
       .yAn = Intersect(VecLine, yLine)
       .zAn = Intersect(VecLine, zLine)
      
       For i = lb To ub
          Call PointToLine(dataRaw(i), nLine, tmpD)
          If i = lb Then
             maxS = tmpD
             minS = tmpD
          Else
             If tmpD > maxS Then maxS = tmpD
             If tmpD < minS Then minS = tmpD
          End If
         ' Cells(i + 1, 15) = tmpD
       Next i
       .MaxSt = maxS
       .MinSt = minS
       .Straightness = 2 * maxS
       Line3DSet = -1
       Debug.Print "Totol Number", TolN
       Debug.Print ".x: ", .x
       Debug.Print ".y: ", .y
       Debug.Print ".z: ", .z
       Debug.Print ".x0: ", .x0
       Debug.Print ".y0: ", .y0
       Debug.Print ".m: ", .m
       Debug.Print ".n: ", .n
       Debug.Print ".p: ", .p
       Debug.Print ".Angle: ", .Angle
       Debug.Print ".xAn: ", .xAn
       Debug.Print ".yAn: ", .yAn
       Debug.Print ".zAn: ", .zAn
       Debug.Print "Straightness", .Straightness, .MaxSt, .MinSt
    End With
End Function

tmtony

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 '( .z0=0)
        rtnDistance = Sqr((Dataraw0.x - dataRaw.x) ^ 2 + _
                         (Dataraw0.y - dataRaw.y) ^ 2 + _
                         (Dataraw0.z - dataRaw.z) ^ 2)
        
    End With
End Function

'************************************************************
'  函数名：sMMult
'  功能：  求两矩阵的乘积,结果存放于参数a中
'  参数：  a  - double   数组,存放矩阵数据的乘积
'          b  - double   数组,存放矩阵数据
'          c  - double   数组,存放矩阵数据
'*************************************************************

Public Sub sMMult(a() As Double, b() As Double, c() As Double)
   a(0, 0) = b(0, 0) * c(0, 0) + b(0, 1) * c(1, 0)
   a(0, 1) = b(0, 0) * c(0, 1) + b(0, 1) * c(1, 1)
   a(1, 0) = b(1, 0) * c(0, 0) + b(1, 1) * c(1, 0)
   a(1, 1) = b(1, 0) * c(0, 1) + b(1, 1) * c(1, 1)
End Sub
'************************************************************
'  函数名：sMMult
'  功能：  求矩阵的逆矩阵,结果存放于参数a中
'  参数：  a  - double   数组,存放矩阵数据
'          b  - double   数组,存放矩阵数据
'*************************************************************
Public Sub sMInverse(a() As Double, b() As Double)
   a(0, 0) = b(1, 1) / (b(0, 0) * b(1, 1) - b(0, 1) * b(1, 0))
   a(0, 1) = b(0, 1) / (b(0, 1) * b(1, 0) - b(0, 0) * b(1, 1))
   a(1, 0) = b(1, 0) / (b(0, 1) * b(1, 0) - b(0, 0) * b(1, 1))
   a(1, 1) = b(0, 0) / (b(0, 0) * b(1, 1) - b(0, 1) * b(1, 0))
End Sub
'************************************************************
'  函数名：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

tanhong

学习收藏了.

yanwei82123300

站长这是用在哪里的？是代数吗？

tmtony

我没用过, 是网友问, 就帮他解决

wufeng980114

有点晕，哈哈。