使用克拉默法则进行三点定圆（三维）

本文由CSDN点云侠原创，爬虫网站请自重。

1.三维圆

  已知不共线的三个点，设其坐标为$(x_1,y_1,z_1)$、 $(x_2,y_2,z_2)$、$(x_3,y_3,z_3)$，空间圆的一般方程为：
$$A(x^2+y^2+z^2)+Bx+Cy+Dz+E=0\text{\hspace{1em}(1)}$$
将式(1)变形可得圆的标准方程为：
$$(x+\frac{B}{2A}{)}^2+(y+\frac{C}{2A}{)}^2+(z+\frac{D}{2A}{)}^2=\frac{B^2+C^2+D^2-4AE}{4A^2}\text{\hspace{1em}(2)}$$
将三个已知点代入式(1)，可得关于$A,B,C,D,E$的齐次线性方程组：

[ x 2 + y 2 + z 2 x y z 1 x 1 2 + y 1 2 + z 1 2 x 1 y 1 z 1 1 x 2 2 + y 2 2 + z 2 2 x 2 y 2 z 2 1 x 3 2 + y 3 2 + z 3 2 x 3 y 3 z 3 1 ] ⋅ [ A B C D E ] = [ 0 0 0 0 0 ] (3) \left[ \begin{matrix} x^2+y^2+z^2 & x & y&z&1\\ x_1^2+y_1^2+z_1^2 & x_1 & y_1&z_1&1 \\ x_2^2+y_2^2+z_2^2 & x_2 & y_2&z_2&1 \\ x_3^2+y_3^2+z_3^2 & x_3 & y_3&z_3&1 \\ \end{matrix} \right]\cdot \left[ \begin{matrix} A\\ B \\ C \\ D \\ E \\ \end{matrix} \right]= \left[ \begin{matrix} 0\\ 0 \\ 0 \\ 0 \\ 0 \\ \end{matrix} \right]\tag{3} ​x2+y2+z2x12​+y12​+z12​x22​+y22​+z22​x32​+y32​+z32​​xx1​x2​x3​​yy1​y2​y3​​zz1​z2​z3​​1111​ ​⋅ ​ABCDE​ ​= ​00000​ ​(3)

式(3)中的系数矩阵并非方阵。于是利用三个已知点来确定平面$\pi$，其方程为：
$$mx+ny+pz+q=0\text{\hspace{1em}(4)}$$
将三个已知点代入式(4)，可得关于$m,n,p,q$的齐次线性方程组：

[ x 2 + y 2 x y 1 x 1 2 + y 1 2 x 1 y 1 1 x 2 2 + y 2 2 x 2 y 2 1 x 3 2 + y 3 2 x 3 y 3 1 ] ⋅ [ m n p q ] = [ 0 0 0 0 ] (5) \left[ \begin{matrix} x^2+y^2 & x & y&1\\ x_1^2+y_1^2 & x_1 & y_1&1 \\ x_2^2+y_2^2 & x_2 & y_2&1 \\ x_3^2+y_3^2 & x_3 & y_3&1 \\ \end{matrix} \right]\cdot \left[ \begin{matrix} m\\ n \\ p \\ q \\ \end{matrix} \right]= \left[ \begin{matrix} 0\\ 0 \\ 0 \\ 0 \\ \end{matrix} \right]\tag{5} ​x2+y2x12​+y12​x22​+y22​x32​+y32​​xx1​x2​x3​​yy1​y2​y3​​1111​ ​⋅ ​mnpq​ ​= ​0000​ ​(5)

在三点不共线的前提下，该齐次线性方程组有非零解，其等价于系数矩阵不满秩，即有：

∣ x 2 + y 2 x y 1 x 1 2 + y 1 2 x 1 y 1 1 x 2 2 + y 2 2 x 2 y 2 1 x 3 2 + y 3 2 x 3 y 3 1 ∣ = 0 (6) \left| \begin{matrix} x^2+y^2 & x & y&1\\ x_1^2+y_1^2 & x_1 & y_1&1 \\ x_2^2+y_2^2 & x_2 & y_2&1 \\ x_3^2+y_3^2 & x_3 & y_3&1 \\ \end{matrix} \right|=0\tag{6} ​x2+y2x12​+y12​x22​+y22​x32​+y32​​xx1​x2​x3​​yy1​y2​y3​​1111​ ​=0(6)
将式(6)展开，并与式(4)对比可得四个系数：
m = + ∣ y 1 z 1 1 y 2 z 2 1 y 3 z 3 1 ∣ (7) m=+\left| \begin{matrix} y_1 & z_1&1 \\ y_2 & z_2&1 \\ y_3 & z_3&1 \\ \end{matrix} \right|\tag{7} m=+ ​y1​y2​y3​​z1​z2​z3​​111​ ​(7)

n = − ∣ x 1 z 1 1 x 2 z 2 1 x 3 z 3 1 ∣ (8) n=-\left| \begin{matrix} x_1& z_1&1 \\ x_2 & z_2&1 \\ x_3& z_3&1 \\ \end{matrix} \right|\tag{8} n=− ​x1​x2​x3​​z1​z2​z3​​111​ ​(8)

p = + ∣ x 1 y 1 1 x 2 y 2 1 x 3 y 3 1 ∣ (9) p=+\left| \begin{matrix} x_1& y_1 &1 \\ x_2 & y_2 &1 \\ x_3 & y_3 &1 \\ \end{matrix} \right|\tag{9} p=+ ​x1​x2​x3​​y1​y2​y3​​111​ ​(9)

q = − ∣ x 1 y 1 z 1 x 2 y 2 z 2 x 3 y 3 z 3 ∣ (10) q=-\left| \begin{matrix} x_1 & y_1 & z_1 \\ x_2 & y_2 & z_2 \\ x_3 & y_3 & z_3 \\ \end{matrix} \right|\tag{10} q=− ​x1​x2​x3​​y1​y2​y3​​z1​z2​z3​​ ​(10)
由式(2)可得圆心坐标$(x_c,y_c,z_c)$：
$$\{\begin{array}{l}{x}_c=-\frac{B}{2A}\\ y_c=-\frac{C}{2A}\\ z_c=-\frac{D}{2A}\end{array}\text{\hspace{1em}(11)}$$
由于圆心 $(x_c,y_c,z_c)$也在平面$\pi$上，于是将式((11)代入式(4)可得
$$2qA-mB-nC-pD=0\text{\hspace{1em}(12)}$$

将式(12)与式(3)结合，可得
[ x 2 + y 2 + z 2 x y z 1 x 1 2 + y 1 2 + z 1 2 x 1 y 1 z 1 1 x 2 2 + y 2 2 + z 2 2 x 2 y 2 z 2 1 x 3 2 + y 3 2 + z 3 2 x 3 y 3 z 3 1 2 q − m − n − p 1 ] ⋅ [ A B C D E ] = [ 0 0 0 0 0 ] (13) \left[ \begin{matrix} x^2+y^2+z^2 & x & y&z&1\\ x_1^2+y_1^2+z_1^2 & x_1 & y_1&z_1&1 \\ x_2^2+y_2^2+z_2^2 & x_2 & y_2&z_2&1 \\ x_3^2+y_3^2+z_3^2 & x_3 & y_3&z_3&1 \\ 2q & -m & -n&-p&1 \\ \end{matrix} \right]\cdot \left[ \begin{matrix} A\\ B \\ C \\ D \\ E \\ \end{matrix} \right]= \left[ \begin{matrix} 0\\ 0 \\ 0 \\ 0 \\ 0 \\ \end{matrix} \right]\tag{13} ​x2+y2+z2x12​+y12​+z12​x22​+y22​+z22​x32​+y32​+z32​2q​xx1​x2​x3​−m​yy1​y2​y3​−n​zz1​z2​z3​−p​11111​ ​⋅ ​ABCDE​ ​= ​00000​ ​(13)
在三点不共线的前提下，该齐次线性方程组有非零解，其等价于系数矩阵不满秩，即有：

A = + ∣ x 1 y 1 z 1 1 x 2 y 2 z 2 1 x 3 y 3 z 3 1 − m − n − p 0 ∣ (14) A=+\left| \begin{matrix} x_1 & y_1&z_1&1 \\ x_2 & y_2&z_2&1 \\ x_3 & y_3&z_3&1 \\ -m & -n&-p&0 \\ \end{matrix} \right|\tag{14} A=+ ​x1​x2​x3​−m​y1​y2​y3​−n​z1​z2​z3​−p​1110​ ​(14)

B = − ∣ x 1 2 + y 1 2 + z 1 2 y 1 z 1 1 x 2 2 + y 2 2 + z 2 2 y 2 z 2 1 x 3 2 + y 3 2 + z 3 2 y 3 z 3 1 2 q − n − p 0 ∣ (15) B=-\left| \begin{matrix} x_1^2+y_1^2+z_1^2& y_1&z_1&1 \\ x_2^2+y_2^2+z_2^2 & y_2&z_2&1 \\ x_3^2+y_3^2+z_3^2 & y_3&z_3&1 \\ 2q & -n&-p&0 \\ \end{matrix} \right|\tag{15} B=− ​x12​+y12​+z12​x22​+y22​+z22​x32​+y32​+z32​2q​y1​y2​y3​−n​z1​z2​z3​−p​1110​ ​(15)

C = + ∣ x 1 2 + y 1 2 + x 1 2 x 1 z 1 1 x 2 2 + y 2 2 + x 2 2 x 2 z 2 1 x 3 2 + y 3 2 + x 3 2 x 3 z 3 1 2 q − m − p 0 ∣ (16) C=+\left| \begin{matrix} x_1^2+y_1^2+x_1^2& x_1&z_1&1 \\ x_2^2+y_2^2+x_2^2 & x_2&z_2&1 \\ x_3^2+y_3^2+x_3^2 & x_3&z_3&1 \\ 2q & -m&-p&0 \\ \end{matrix} \right|\tag{16} C=+ ​x12​+y12​+x12​x22​+y22​+x22​x32​+y32​+x32​2q​x1​x2​x3​−m​z1​z2​z3​−p​1110​ ​(16)

D = − ∣ x 1 2 + y 1 2 + x 1 2 x 1 y 1 1 x 2 2 + y 2 2 + x 2 2 x 2 y 2 1 x 3 2 + y 3 2 + x 3 2 x 3 y 3 1 2 q − m − n 0 ∣ (17) D=-\left| \begin{matrix} x_1^2+y_1^2+x_1^2& x_1&y_1&1 \\ x_2^2+y_2^2+x_2^2 & x_2&y_2&1 \\ x_3^2+y_3^2+x_3^2 & x_3&y_3&1 \\ 2q & -m&-n&0 \\ \end{matrix} \right|\tag{17} D=− ​x12​+y12​+x12​x22​+y22​+x22​x32​+y32​+x32​2q​x1​x2​x3​−m​y1​y2​y3​−n​1110​ ​(17)

E = + ∣ x 1 2 + y 1 2 + x 1 2 x 1 y 1 z 1 x 2 2 + y 2 2 + x 2 2 x 2 y 2 z 2 x 3 2 + y 3 2 + x 3 2 x 3 y 3 z 2 2 q − m − n − p ∣ (18) E=+\left| \begin{matrix} x_1^2+y_1^2+x_1^2& x_1&y_1&z_1 \\ x_2^2+y_2^2+x_2^2 & x_2&y_2&z_2 \\ x_3^2+y_3^2+x_3^2 & x_3&y_3&z_2 \\ 2q & -m&-n&-p \\ \end{matrix} \right|\tag{18} E=+ ​x12​+y12​+x12​x22​+y22​+x22​x32​+y32​+x32​2q​x1​x2​x3​−m​y1​y2​y3​−n​z1​z2​z2​−p​ ​(18)

可得圆心坐标$(x_c,y_c,z_c)$和半径$r$，即
$$\{\begin{array}{l}{x}_c=-\frac{B}{2A}\\ y_c=-\frac{C}{2A}\\ z_c=-\frac{D}{2A}\\ r=\sqrt{\frac{B^2+C^2+D^2-4AE}{4A^2}}\end{array}\text{\hspace{1em}(19)}$$

2.python代码

import numpy as np


def three_points_fit_3dcircle(points):
    p1 = points[0]
    p2 = points[1]
    p3 = points[2]
    # 共线检查
    temp01 = p1 - p2
    temp02 = p3 - p2
    temp03 = np.cross(temp01, temp02)
    temp = (temp03 @ temp03) / (temp01 @ temp01) / (temp02 @ temp02)
    if temp < 10 ** -6:
        print('\t三点共线, 无法确定圆')
        return None
    temp1 = np.vstack((p1, p2, p3))
    temp2 = np.ones(3).reshape(3, 1)
    mat1 = np.hstack((temp1, temp2))  # size = 3x4

    m = +np.linalg.det(mat1[:, 1:])
    n = -np.linalg.det(np.delete(mat1, 1, axis=1))
    p = +np.linalg.det(np.delete(mat1, 2, axis=1))
    q = -np.linalg.det(temp1)

    temp3 = np.array([p1 @ p1, p2 @ p2, p3 @ p3]).reshape(3, 1)
    temp4 = np.hstack((temp3, mat1))
    temp5 = np.array([2 * q, -m, -n, -p, 0])
    mat2 = np.vstack((temp4, temp5))  # size = 4x5

    A = +np.linalg.det(mat2[:, 1:])
    B = -np.linalg.det(np.delete(mat2, 1, axis=1))
    C = +np.linalg.det(np.delete(mat2, 2, axis=1))
    D = -np.linalg.det(np.delete(mat2, 3, axis=1))
    E = +np.linalg.det(mat2[:, :-1])

    pc = -np.array([B, C, D]) / 2 / A
    r = np.sqrt(B * B + C * C + D * D - 4 * A * E) / 2 / abs(A)

    return pc, r

3.计算结果

圆心坐标:[-0.77585587 -0.26649462 -0.01626231]
圆半径:0.9999999780711272