最小二乗法による球の推定

3次元空間に点が6点くらいあり、その点群に最もフィットする球面を最小二乗法もしくは他の方法で求めたいと考えております。
①下記サイトをC言語からPythonにしてもよいと思いますし、（すいません、C言語に不慣れ、、）
https://qiita.com/yujikaneko/items/955b4474772802b055bc
②下記のコードも見様見真似で組んでみましたが、これがよいのかどうか判断が難しいです。
https://www.slideshare.net/j_rocket_boy/fitting-88311197

①でPythonに直せるのがもっともよいのかもしれませんが、②のコードのご見解を伺わせていただけると幸いです。
よろしくお願いいたします。

import itertools
import numpy as np
import pandas as pd
import math
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import warnings
warnings.filterwarnings('ignore')

#球面フィッティング
def SS_fit(data) : 
    #x,y,z要素取り出し
    x = data[:,[0]]
    y = data[:,[1]]
    z = data[:,[2]]

    #データの長さを格納（n = Σ1)    
    n = len(x)
    
    #それぞれの要素の二乗を求める
    x2 = np.power(x,2)
    y2 = np.power(y,2)
    z2 = np.power(z,2)

    #違う要素との積を求める
    xy = x*y
    xz = x*z
    yz = y*z

    #右辺の数値用
    E = -x*(x2+y2+z2)
    F = -y*(x2+y2+z2)
    G = -z*(x2+y2+z2)
    H =   -(x2+y2+z2)

    #要素の総和に変換
    x = np.sum(x)
    y = np.sum(y)
    z = np.sum(z)

    x2 = np.sum(x2)
    y2 = np.sum(y2)
    z2 = np.sum(z2)

    xy = np.sum(xy)
    xz = np.sum(xz)
    yz = np.sum(yz)    

    E = np.sum(E)
    F = np.sum(F)
    G = np.sum(G)
    H = np.sum(H)

    #左辺の4×4行列を作る
    K = np.array([  [x2,xy,xz,x],
                    [xy,y2,yz,y],
                    [xz,yz,z2,z],
                    [x,y,z,n]])
    
    #右辺の4×1行列を作る
    L = np.array([E,F,G,H])

    #A,B,C,Dの行列を計算
    P = np.dot(np.linalg.inv(K),L)

    A = P[0]
    B = P[1]
    C = P[2]
    D = P[3]

    #中心座標と半径に変換
    x0 = (-1/2)* A
    y0 = (-1/2)* B
    z0 = (-1/2)* C
    r  = pow(pow(x0,2)+pow(y0,2)+pow(z0,2)-D,1/2)
    
    return np.array([x0,y0,z0,r])

# データの読み込み
datapd = pd.read_csv("data.csv", encoding='SHIFT-JIS', index_col=0)
data = np.array(datapd)
data = data.astype(float)
MagX = np.ravel( np.c_[data[:,0]] )
MagY = np.ravel( np.c_[data[:,1]] )
MagZ = np.ravel( np.c_[data[:,2]] )

#球面フィッティング
S = SS_fit(data)

#球面フィッティング後の4変数（中心xyz座標、半径）を表示
print("X: {0}, Y: {1}, Z: {2}, radius: {3}".format(S[0], S[1], S[2], S[3]))

# 3Dプロット
fig = plt.figure()
#ax = fig.gca(projection='3d')
ax = fig.add_subplot(111,projection="3d")
fig=plt.figure(figsize=(5,5))
# 点のプロット
ax.scatter(MagX, MagY, MagZ)
# draw sphere
u,v=np.mgrid[0:2*np.pi:50j, 0:np.pi:25j]
x=S[3] * np.cos(u)*np.sin(v) + S[0]
y=S[3] * np.sin(u)*np.sin(v) + S[1]
z=S[3] * np.cos(v) + S[2]

XMin = np.min(MagX)
XMax = np.max(MagX)
YMin = np.min(MagY)
YMax = np.max(MagY)
ZMin = np.min(MagZ)
ZMax = np.max(MagZ)
ax.set_xlim(XMin - 0.05 * (XMax - XMin), XMax + 0.05 * (XMax - XMin))
ax.set_ylim(YMin - 0.05 * (YMax - YMin), YMax + 0.05 * (YMax - YMin))
ax.set_zlim(ZMin - 0.05 * (ZMax - ZMin), ZMax + 0.05 * (ZMax - ZMin))

ax.set_xlabel("X-axis")
ax.set_ylabel("Y-axis")
ax.set_zlabel("Z-axis")

ax.plot_surface(x,y,z, alpha=0.2)

plt.savefig('SS_fitting.png', bbox_inches="tight")
plt.show()

行動規範の内容に同意します

回答1件

ベストアンサー

Least Squares Sphere Fitのコードがそのまま使えると思います。

Python
1import math
2import numpy as np
3
4#   fit a sphere to X,Y, and Z data points
5#   returns the radius and center points of
6#   the best fit sphere
7def sphereFit(spX,spY,spZ):
8    #   Assemble the A matrix
9    spX = np.array(spX)
10    spY = np.array(spY)
11    spZ = np.array(spZ)
12    A = np.zeros((len(spX),4))
13    A[:,0] = spX*2
14    A[:,1] = spY*2
15    A[:,2] = spZ*2
16    A[:,3] = 1
17
18    #   Assemble the f matrix
19    f = np.zeros((len(spX),1))
20    f[:,0] = (spX*spX) + (spY*spY) + (spZ*spZ)
21    C, residules, rank, singval = np.linalg.lstsq(A,f,rcond=None)
22
23    #   solve for the radius
24    t = (C[0]*C[0])+(C[1]*C[1])+(C[2]*C[2])+C[3]
25    radius = math.sqrt(t)
26
27    return radius, C[0], C[1], C[2]
28
29ret = sphereFit( [1,0,-1,0], [0,1,0,-1], [0,0,0,0])
30print(ret) # (0.9999999999999999, array([-3.4863056e-32]), array([-1.11022302e-16]), array([0.]))