最小二乘法拟合圆柱

注: 本文借鉴了 博客1

圆柱体可以由一条空间直线(柱芯)和圆柱半径(柱径)表示。
其中，柱芯由一条空间直线表示
$$\frac{x-x_0}{a}=\frac{y-y_0}{b}=\frac{z-z_0}{c}$$
柱芯上任意一点为($x_0,y_0,z_0$)，柱芯上的方向向量为(a, b, c)。
柱径为r

方法阐述

圆柱面上的任意点为($x_i,y_i,z_i)$
$$A^{2}x+B^{2}y+C^{2}z=r^2,其中\{\begin{array}{rl}A& =c(y_i-y_0)-b(z_i-z_0)\\ B& =a(z_i-z_0)-c(x_i-x_0)\\ C& =b(x_i-x_0)-a(y_i-y_0)\end{array}$$
至于为什么$A^{2}x+B^{2}y+C^{2}z=r^2$，本人还不是非常理解
以上写成矩阵为：
$$\left[\begin{array}{c}{x}_i-x_0\\ y_i-y_0\\ z_i-z_0\end{array}\right]\left[\begin{array}{ccc}0& c& -b\\ -c& 0& a\\ b& -a& 0\end{array}\right]=\left[\begin{array}{c}A\\ B\\ C\end{array}\right]$$
将以上等式中的$A,B,C$代入，得：
$$[c(y_i-y_0)-b(z_i-z_0){]}^2+[a(z_i-z_0)-c(x_i-x_0){]}^2+[b(x_i-x_0)-a(y_i-y_0){]}^2=r^2$$
将等式化简：
$${\theta }_0{x}^2+{\theta }_1{y}^2+{\theta }_2{z}^2+{\theta }_{3}xy+{\theta }_{4}xz+{\theta }_{5}yz+{\theta }_{6}x+{\theta }_{7}y+{\theta }_{8}z+{\theta }_9=0$$
其中：
$$\{\begin{array}{rl}{\theta }_0& =b^2+c^2\\ {\theta }_1& =a^2+c^2\\ {\theta }_2& =a^2+b^2\\ {\theta }_3& =-2ab\\ {\theta }_4& =-2ac\\ {\theta }_5& =-2bc\\ {\theta }_6& =-2(b^2+c^2)x_0+2ab{y}_0+2ac{z}_0\\ {\theta }_7& =2ab{x}_0-2(a^2+c^2)x_0+2bc{z}_0\\ {\theta }_8& =2ac{x}_0+2bc{y}_0-2(a^2+b^2)z_0\\ {\theta }_9& =(b^2+c^2)x_0^2+(a^2+c^2)y_0^2+(a^2+b^2)z_0^2-2bc{y}_0{z}_0-2ac{z}_0{x}_0-2ab{x}_0{y}_0-r^2\end{array}$$
等式两边同时除${\theta }_0$，并移项得：
$$\frac{{\theta }_1}{{\theta }_0}{y}^2+\frac{{\theta }_2}{{\theta }_0}{z}^2+\frac{{\theta }_3}{{\theta }_0}xy+\frac{{\theta }_4}{{\theta }_0}xz+\frac{{\theta }_6}{{\theta }_0}yz+\frac{{\theta }_7}{{\theta }_0}x+\frac{{\theta }_7}{{\theta }_0}y+\frac{{\theta }_8}{{\theta }_0}z+\frac{{\theta }_9}{{\theta }_0}=-x^2$$
书写成矩阵为：
$$\left[\begin{array}{ccccccccc}{y}_i^2& z_i^2& x_i{y}_i& x_i{z}_i& y_i{z}_i& x_i& y_i& z_i& 1\end{array}\right]\left[\begin{array}{c}{\theta }_1/{\theta }_0\\ {\theta }_2/{\theta }_0\\ {\theta }_3/{\theta }_0\\ {\theta }_4/{\theta }_0\\ {\theta }_5/{\theta }_0\\ {\theta }_6/{\theta }_0\\ {\theta }_7/{\theta }_0\\ {\theta }_8/{\theta }_0\\ {\theta }_9/{\theta }_0\end{array}\right]=\left[\begin{array}{c}-x_i^2\end{array}\right]$$
最小二乘拟合：
$$\left[\begin{array}{ccccccccc}{y}_1^2& z_1^2& x_1{y}_1& x_1{z}_1& y_1{z}_1& x_1& y_1& z_1& 1\\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮& ⋮\\ y_n^2& z_n^2& x_n{y}_n& x_n{z}_n& y_n{z}_n& x_n& y_n& z_n& 1\end{array}\right]\left[\begin{array}{c}{\theta }_1/{\theta }_0\\ {\theta }_2/{\theta }_0\\ {\theta }_3/{\theta }_0\\ {\theta }_4/{\theta }_0\\ {\theta }_5/{\theta }_0\\ {\theta }_6/{\theta }_0\\ {\theta }_7/{\theta }_0\\ {\theta }_8/{\theta }_0\\ {\theta }_9/{\theta }_0\end{array}\right]=\left[\begin{array}{c}-x_1^2\\ ⋮\\ -x_n^2\end{array}\right]$$

用eigen库写了一下矩阵的通解，命名不是很规范（已知AB矩阵，求解P）

// 矩阵 P * A = B
Eigen::MatrixXd PA_B(Eigen::MatrixXd A, Eigen::MatrixXd B)
{
	Eigen::MatrixXd At = A.transpose();
	return B * At * (A * At).inverse();
}

// 矩阵: A * P = B
Eigen::MatrixXd AP_B(Eigen::MatrixXd A, Eigen::MatrixXd B)
{
	Eigen::MatrixXd At = A.transpose();
	return (At * A).inverse() * At * B;
}

对$\theta$组进行求解

求解方向向量

因为${\theta }_3=-2ab,{\theta }_4=-2ac,{\theta }_5=-2bc$，所以只有当abc中至少有两者为0时，${\theta }_3,{\theta }_4,{\theta }_5$会全部为0。

1. 当$\frac{{\theta }_3}{{\theta }_0},\frac{{\theta }_4}{{\theta }_0},\frac{{\theta }_5}{{\theta }_0}$都为0时

• 若$\frac{{\theta }_1}{{\theta }_0}$=1（接近于1），即$\frac{a^2+c^2}{b^2+c^2}$ =1，$a^2=b^2$，又因为abc中一定有两者为0，所以此时的方向向量$(a,b,c)$为$(0,0,1)$
• 若$\frac{{\theta }_2}{{\theta }_0}$=1（接近于1），即$\frac{a^2+b^2}{b^2+c^2}$ =1，$a^2=c^2$，又因为abc中一定有两者为0，所以此时的方向向量$(a,b,c)$为$(0,1,0)$

2. 当$\frac{{\theta }_3}{{\theta }_0},\frac{{\theta }_4}{{\theta }_0},\frac{{\theta }_5}{{\theta }_0}$不都为0时，令：
   $$\begin{gathered} K=\frac{2}{1+\frac{{\theta }_1}{{\theta }_0}+\frac{{\theta }_2}{{\theta }_0}} \\ =\frac{2}{1+\frac{a^2+c^2}{b^2+c^2}+\frac{a^2+b^2}{b^2+c^2}}=\frac{2}{1+\frac{a^2+c^2}{b^2+c^2}+\frac{a^2+b^2}{b^2+c^2}} \\ =\frac{2}{\frac{b^2+c^2}{b^2+c^2}+\frac{a^2+c^2}{b^2+c^2}+\frac{a^2+b^2}{b^2+c^2}}=\frac{2}{\frac{b^2+c^2}{b^2+c^2}+\frac{a^2+c^2}{b^2+c^2}+\frac{a^2+b^2}{b^2+c^2}} \\ =\frac{2}{\frac{2(a^2+b^2+c^2)}{b^2+c^2}}=\frac{b^2+c^2}{a^2+b^2+c^2} \end{gathered}$$
   将以上$\theta$组乘K
   $$\left[\begin{array}{c}{\theta }_1/{\theta }_0\\ {\theta }_2/{\theta }_0\\ {\theta }_3/{\theta }_0\\ {\theta }_4/{\theta }_0\\ {\theta }_5/{\theta }_0\\ {\theta }_6/{\theta }_0\\ {\theta }_7/{\theta }_0\\ {\theta }_8/{\theta }_0\\ {\theta }_9/{\theta }_0\end{array}\right]=\left[\begin{array}{c}{\theta }_1/(b^2+c^2)\\ {\theta }_2/(b^2+c^2)\\ {\theta }_3/(b^2+c^2)\\ {\theta }_4/(b^2+c^2)\\ {\theta }_5/(b^2+c^2)\\ {\theta }_6/(b^2+c^2)\\ {\theta }_7/(b^2+c^2)\\ {\theta }_8/(b^2+c^2)\\ {\theta }_9/(b^2+c^2)\end{array}\right]\ast K(\frac{b^2+c^2}{a^2+b^2+c^2})=\left[\begin{array}{c}{\theta }_1/(a^2+b^2+c^2)\\ {\theta }_2/(a^2+b^2+c^2)\\ {\theta }_3/(a^2+b^2+c^2)\\ {\theta }_4/(a^2+b^2+c^2)\\ {\theta }_5/(a^2+b^2+c^2)\\ {\theta }_6/(a^2+b^2+c^2)\\ {\theta }_7/(a^2+b^2+c^2)\\ {\theta }_8/(a^2+b^2+c^2)\\ {\theta }_9/(a^2+b^2+c^2)\end{array}\right]$$
   注：$(a,b,c)$为方向向量，对该向量进行归一化后$a^2+b^2+c^2=1$，$\theta$组即为：
   $${\left[\begin{array}{ccccccccc}{\theta }_1& {\theta }_2& {\theta }_3& {\theta }_4& {\theta }_5& {\theta }_6& {\theta }_7& {\theta }_8& {\theta }_9\end{array}\right]}^T$$
   令$A=K,B=K{\theta }_1,C=K{\theta }_2,D=K{\theta }_3,E=K{\theta }_4,F=K{\theta }_5$
   其中
   $$\begin{gathered} A=\frac{b^2+c^2}{a^2+b^2+c^2},B=\frac{a^2+c^2}{a^2+b^2+c^2},C=\frac{a^2+b^2}{a^2+b^2+c^2} \\ D=\frac{-2ab}{a^2+b^2+c^2},E=\frac{-2ac}{a^2+b^2+c^2},F=\frac{-2bc}{a^2+b^2+c^2} \end{gathered}$$
   选择$A,B,C$中数值最大的数字（其实选出的数值只需要不为0即可）

• 若$A$不为0时，用$A$来计算方向向量$(a,b,c)$
  $$a=(1-A{)}^{\frac{1}{2}},b=\frac{D}{-2a},c=\frac{E}{-2a}$$
• 若$B$不为0时，用$B$来计算方向向量$(a,b,c)$
  $$b=(1-B{)}^{\frac{1}{2}},a=\frac{D}{-2b},c=\frac{F}{-2b}$$
• 若$C$不为0时，用$C$来计算方向向量$(a,b,c)$
  $$c=(1-C{)}^{\frac{1}{2}},a=\frac{E}{-2c},b=\frac{F}{-2c}$$
  对方向向量$(a,b,c)$进行归一化，即可。

求解轴上一点

太多了下次再写！