【B++】几何图形拟合公式大赏

使用最小二乘法拟合下列各几何图形，假设点云共有n个点，第i个点的坐标为$(x_i,y_i,z_i)$，下同。
python里用最小二乘法解方程Ax=b的语句是x=np.linalg.pinv(A).dot(b)。

点

到两线段最近的点/两线段的近似交点

假设线段p的端点坐标为P₀和P₁，线段q的端点坐标为Q₀和Q₁，则p上的点可表示为$P=m{P}_0+(1-m)P_1$，q上的点可表示为$Q=n{Q}_0+(1-n)Q_1$。
尽量使P=Q，则有$m{P}_0+(1-m)P_1=n{Q}_0+(1-n)Q_1$，移项得$m(P_0-P_1)+n(Q_1-Q_0)=Q_1-P_1$。
于是可得方程组$\left[\begin{array}{cc}{x}_{P0}-x_{P1}& x_{Q0}-x_{Q1}\\ y_{P0}-y_{P1}& z_{Q0}-z_{Q1}\\ x_{P0}-x_{P1}& x_{Q0}-x_{Q1}\end{array}\right]\left[\begin{array}{c}m\\ n\end{array}\right]=\left[\begin{array}{c}{x}_{Q1}-x_{P1}\\ y_{Q1}-y_{P1}\\ z_{Q1}-z_{P1}\end{array}\right]$，由此可解得m和n的近似值。
所以，到两线段最近的点为$\frac{1}{2}[m{P}_0+(1-m)P_1+n{Q}_0+(1-n)Q_1]$。当两线段靠得很近时，可将$m{P}_0+(1-m)P_1$或$n{Q}_0+(1-n)Q_1$作为两线段的近似交点。

二维图形

平面

由点云拟合平面$Ax+By+Cz=1$：
$\left[\begin{array}{ccc}{x}_1& y_1& z_1\\ x_2& y_2& z_2\\ & \cdots & \\ x_n& y_n& z_n\end{array}\right]\left[\begin{array}{c}A\\ B\\ C\end{array}\right]=\left[\begin{array}{c}1\\ 1\\ \cdots \\ 1\end{array}\right]\stackrel{Px=y}{\to }\left[\begin{array}{c}A\\ B\\ C\end{array}\right]=(P^{T}P{)}^{-1}{P}^{T}y$，由此可解出A、B、C的近似值，其它方程组近似解解法相同。

圆（三个互不重合的圆/直线的公共内/外切圆）

先考虑公共外切圆。假设⊙A=(x₁,y₁,r₁)，⊙B=(x₂,y₂,r₂)，⊙C=(x₃,y₃,r₃)，它们的公共外切圆为⊙P=(x,y,z)。
原问题可表示为：
$\{\begin{array}{c}(x-x_1{)}^2+(y-y_1{)}^2=(r+r_1{)}^2\\ (x-x_2{)}^2+(y-y_2{)}^2=(r+r_2{)}^2\\ (x-x_3{)}^2+(y-y_3{)}^2=(r+r_3{)}^2\end{array}$
用（1）式减（2）式，（2）式减（3）式得：
$\{\begin{array}{c}-2x(x_1-x_2)+x_1^2-x_2^2-2y(y_1-y_2)+y_1^2-y_2^2=2r(r_1-r_2)+r_1^2-r_2^2\\ -2x(x_2-x_3)+x_2^2-x_3^2-2y(y_2-y_3)+y_2^2-y_3^2=2r(r_2-r_3)+r_2^2-r_3^2\\ (x-x_3{)}^2+(y-y_3{)}^2=(r+r_3{)}^2\end{array}$
换元并化简前两式得：
$\{\begin{array}{c}{a}_1=2(x_1-x_2),b_1=2(y_1-y_2),c_1=2(r_1-r_2),d_1=x_1^2-x_2^2+y_1^2-y_2^2-r_1^2+r_2^2\\ a_2=2(x_2-x_3),b_2=2(y_2-y_3),c_2=2(r_2-r_3),d_2=x_2^2-x_3^2+y_2^2-y_3^2-r_2^2+r_3^2\\ A_1=\frac{b_1{c}_2-b_2{c}_1}{a_1{b}_2-a_2{b}_1},B_1=\frac{b_2{d}_1-b_1{d}_2}{a_1{b}_2-a_2{b}_1},A_2=\frac{a_2{c}_1-a_1{c}_2}{a_1{b}_2-a_2{b}_1},B_2=\frac{a_1{d}_2-a_2{d}_1}{a_1{b}_2-a_2{b}_1}\\ x=A_{1}r+B_1\\ y=A_{2}r+B_2\\ (x-x_3{)}^2+(y-y_3{)}^2=(r+r_3{)}^2\end{array}$
最后一式可看做以r为变量的一元二次方程，直接套用求根公式可得：
$\{\begin{array}{c}{a}_1=2(x_1-x_2),b_1=2(y_1-y_2),c_1=2(r_1-r_2),d_1=x_1^2-x_2^2+y_1^2-y_2^2-r_1^2+r_2^2\\ a_2=2(x_2-x_3),b_2=2(y_2-y_3),c_2=2(r_2-r_3),d_2=x_2^2-x_3^2+y_2^2-y_3^2-r_2^2+r_3^2\\ d=a_1{b}_2-a_2{b}_1\\ A_1=\frac{1}{d}(b_1{c}_2-b_2{c}_1),B_1=\frac{1}{d}(b_2{d}_1-b_1{d}_2),A_2=\frac{1}{d}(a_2{c}_1-a_1{c}_2),B_2=\frac{1}{d}(a_1{d}_2-a_2{d}_1)\\ C_1=B_1-x_3,C_2=B_2-y_3\\ A=A_1^2+A_2^2-1,B=2(A_1{C}_1+A_2{C}_2-r_3),C=C_1^2+C_2^2-r_3^2\\ r=\frac{-B-\sqrt{B^2-4AC}}{2A},x=A_{1}r+B_1,y=A_{2}r+B_2\end{array}$
若与某个圆内切，则将相应的圆的半径变号。若为过某定点，则相应的半径变为0。
一元二次方程有两个解，上述解为主解。若副解$r=\frac{-B+\sqrt{B^2-4AC}}{2A}$为正，则主副解同时成立，圆有两个解。此时，该圆与三个已知圆不全为外切或内切，且三个圆的圆心近似共线。

直线

由点云拟合直线$x=x_0+At,y=y_0+Bt,z=z_0+Ct$：
将其变形为$\frac{C}{A}(x-x_0)=z-z_0,\frac{C}{B}(y-y_0)=z-z_0$，两式相加得$\frac{C}{A}(x-x_0)+\frac{C}{B}(y-y_0)=2(z-z_0)$，可得方程组
$\left[\begin{array}{cc}{x}_1-x_0& y_1-y_0\\ x_2-x_0& y_2-y_0\\ \cdots & \\ x_n-x_0& y_n-y_0\end{array}\right]\left[\begin{array}{c}\frac{C}{A}\\ \frac{C}{B}\end{array}\right]=2\left[\begin{array}{c}{z}_1-z_0\\ z_2-z_0\\ \cdots \\ z_n-z_0\end{array}\right]$，由此可解得$\frac{C}{A}$和$\frac{C}{B}$的近似值，设定C可得A和B。

三维图形

球

由点云拟合球$(x-a{)}^2+(y-b{)}^2+(z-c{)}^2=R^2$：
先将其变形为$2ax+2by+2cz+R^2-a^2-b^2-c^2=x^2+y^2+z^2$，可得方程组
$\left[\begin{array}{cccc}2x_1& 2y_1& 2z_1& 1\\ 2x_2& 2y_2& 2z_2& 1\\ \cdots & & & \\ 2x_n& 2y_n& 2z_n& 1\end{array}\right]\left[\begin{array}{c}a\\ b\\ c\\ R^2-a^2-b^2-c^2\end{array}\right]=\left[\begin{array}{c}{x}_1^2+y_1^2+z_1^2\\ x_2^2+y_2^2+z_2^2\\ \cdots \\ x_n^2+y_n^2+z_n^2\end{array}\right]$，解就行了。