C#ATIA

↑タイトル詐欺 主にFusion360API 偶にCATIA V5 VBA(絶賛ネタ切れ中)

複数の3D点から平面を作成したい1

こちらに挑戦しようと思っています。
Creating best matching plane using several(more than 3) points script - Autodesk Community
明らかに最小二乗法を利用した、平面フィッティングな問題です。

これをpythonで行おうと検索すると、numpyとscipy(両方とも
外部モジュール)のサンプルコードばかり。
Fusion360API的にはバニラpythonで処理したいのに・・・。

pythonで探すのは諦めたところ、こちらを見つけました。
[数学] 最小二乗平面をプログラムで求める - Qiita

生憎、計算内容的には分かっておらず、無心でC#
pythonに直してみました。


def SumSamplingData(points: list[core.Point3D]):
    # xの合計値
    x = 0

    # x^2の合計値
    x2 = 0

    # x * yの合計値
    xy = 0

    # x * zの合計値
    xz = 0

    # yの合計値
    y = 0

    # y^2の合計値
    y2 = 0

    # y * zの合計値
    yz = 0

    # zの合計値
    z = 0

    # 計測したデータから、各種必要なsumを得る
    for v in points:
        # 最小二乗平面との誤差は高さの差を計算するので、(今回の式の都合上)Yの値をZに入れて計算する
        vx = v.x
        vy = v.z
        vz = v.y

        x += vx
        x2 += (vx * vx)
        xy += (vx * vy)
        xz += (vx * vz)

        y += vy
        y2 += (vy * vy)
        yz += (vy * vz)

        z += vz

    # matA[0, 0]要素は要素数と同じ(\sum{1}のため)
    l = 1 * len(points)

    # 求めた和を行列の要素として2次元配列を生成
    matA = [
        [l,  x,  y],
        [x, x2, xy],
        [y, xy, y2],
    ]
    b = [
        z, xz, yz
    ]

    # 求めた値を使ってLU分解→結果を求める
    return LUDecomposition(matA, b)


def LUDecomposition(aMatrix, b):
    # 行列数(Vector3データの解析なので3x3行列)
    N = len(aMatrix)

    # L行列(零行列に初期化)
    lMatrix = [[0 for i in range(N)] for j in range(N)]

    # U行列(対角要素を1に初期化)
    uMatrix = [[0 for i in range(N)] for j in range(N)]
    for i in range(N):
        for j in range(N):
            if i == j:
                uMatrix[i][j] = 1 

    # 計算用のバッファ
    buffer = [[0 for i in range(N)] for j in range(N)]

    # LU分解開始
    for i in range(N):
        n = N - i - 1

        l0 = lMatrix[i][i] = aMatrix[0][0]

        # l1成分をコピー
        l1 = [0 for _ in range(n)] 
        for j in range(n):
            lMatrix[j + i + 1][i] = l1[j] =  aMatrix[j + 1][0]

        # u1^T成分をコピー
        u1 = [0 for _ in range(n)] 
        for j in range(n):
            uMatrix[i][j + i + 1] = u1[j] = aMatrix[0][j + 1] / l0

        # luを求める
        for j in range(n):
            for k in range(n):
                buffer[j][k] = l1[j] * u1[k]

        # A1を求める
        A1 = [[0 for i in range(n)] for j in range(n)]
        for j in range(n):
            for k in range(n):
                A1[j][k] = aMatrix[j + 1][k + 1] - buffer[j][k]

        # A1を新しいaMatrixとして利用する
        aMatrix = A1

    # 求めたLU行列を使って連立方程式を解く
    y = [0 for i in range(N)]
    for i in range(N):
        sum = 0
        for k in range(i):
            sum += lMatrix[i][k] * y[k]
        y[i] = (b[i] - sum) / lMatrix[i][i]

    x = [0 for i in range(N)]
    for i in reversed(range(N)):
        sum = 0
        for j in range(i+1, N):
            sum += uMatrix[i][j] * x[j]
        x[i] = y[i] - sum

    return x

正直な所、正しく直せているのか分かりませんし、無心で直したため、
pythonらしさが無いコードになっている事は、本人も分かっています。

適当なサンプルデータ(点)を実行した結果がこちらです。

画像だけだとわかりにくいのですが、早い話が全然間違ってます。
(微妙な間違いだと気が付かなかったかも知れませんが、
圧倒的に間違っているので、気が付きました)

何が間違っているのかな?
CATIA V5だと複数の3D点から平面作るコマンドがあるのですけどね。