同前一篇文章相同，算法主要根据这篇文章，修改了其中一些笔误并微调以适合C#代码。
首先是将圆柱参数化并简化该模型。
圆柱的轴线为一空间直线，由一点\(P\)和一空间向量\(V\)定义。
如果经过坐标变换，新坐标系零点为\(P\)且坐标z轴为\(V\)，那么该轴线的未知量就由\(x_0,y_0,z_0,a,b,c\)减少为4个。
参数化
圆柱的轴线表示为经过点\(P\left(x_0,y_0,z_0\right)\)向量为\(V\left(a,b,c\right)\)的直线，半径为R
求初始值
我个人觉得这种求法偏繁琐，不过的确相当接近最后拟合值，迭代次数相当少。
\(a,b,c\)的初始值
对于圆柱面上任一点\(P_i\left(x_i,y_i,z_i\right)\)有\[A^2+B^2+C^2=R^2\]其中\[A=c\left(y_i-y_0\right)-b\left(z_i-z_0\right)\]\[B=a\left(z_i-z_0\right)-c\left(x_i-x_0\right)\]\[C=b\left(x_i-x_0\right)-a\left(y_i-y_0\right)\]
将上面的等式代入展开\[\left[c\left(y_i-y_0\right)-b\left(z_i-z_0\right)\right]^2+\left[a\left(z_i-z_0\right)-c\left(x_i-x_0\right)\right]^2+\left[b\left(x_i-x_0\right)-a\left(y_i-y_0\right)\right]^2=R^2\]这个等式很简单（不得不吐槽，当时看到这里我都疯了，现在手打这些公式更疯了），它等价于\[Ax_i^2+By_i^2+Cz_i^2+Dx_iy_i+Ex_iz_i+Fy_iz_i+Gx_i+Hy_i+Iz_i+J=0\]其中\begin{array}{lclp{12}}
A&=&\left(b^2+c^2\right)\\
B&=&\left(a^2+c^2\right)\\
C&=&\left(a^2+b^2\right)\\
D&=&-2ab\\
E&=&-2ac\\
F&=&-2bc\\
G&=&-2\left(b^2+c^2\right)x_0+2aby_0+2acz_0\\
H&=&2abx_0-2\left(a^2+c^2\right)y_0+2bcz_0\\
I&=&2acx_0+2bcy_0-2\left(a^2+b^2\right)z_0\\
J&=&\left(b^2+c^2\right)x_0^2+\left(a^2+c^2\right)y_0^2+\left(a^2+b^2\right)z_0^2-2bcy_0z_0-2acz_0x_0-2abx_0y_0-R^2
\end{array}
两边同除以\(A\)并将\(x\)二次项移到等式右边得
\[\frac{B}{A}y_i^2+\frac{C}{A}z_i^2+\frac{D}{A}x_iy_i+\frac{E}{A}x_iz_i+\frac{F}{A}y_iz_i+\frac{G}{A}x_i+\frac{G}{A}y_i+\frac{I}{A}z_i+\frac{J}{A}=-x^2\]
这是一个线性方程组
\[\begin{bmatrix}y_1^2 &z_1^2 & x_1y_1 & x_1z_1 & y_1z_1 & x_1 & y_1 & z_1 & 1 \\y_2^2 & z_2^2 & x_2y_2 & x_2z_2 & y_2z_2 & x_2 & y_2 & z_2 & 1 \\\vdots &\vdots &\vdots &\vdots &\vdots &\vdots &\vdots &\vdots &\vdots \\y_n^2 &z_n^2 & x_ny_n & x_nz_n & y_nz_n & x_n & y_n & z_n & 1 \end{bmatrix}\begin{bmatrix}B/A\\C/A\\D/A\\E/A\\F/A\\G/A\\H/A\\I/A\\J/A\end{bmatrix}=\begin{bmatrix}-x_1^2\\-x_2^2\\\vdots\\-x_n^2\end{bmatrix}\]
求解：
令\(B’=B/A,C’=C/A,D’=D/A,E’=E/A,F’=F/A,G’=G/A,H’=H/A,I’=I/A,J’=J/A\)
如果\(|D’|\)，\(|E’|\)和\(|F’|\)近似等于0（意味着abc中有两个近似于0，向量与某坐标轴平行），如果\(B’\)近似于1向量为\(\left(0,0,1\right)\)，如果\(C’\)近似于1向量为\(\left(0,1,0\right)\)
否则，令\[k=\frac{2}{1+B’+C’}\]则\[A=k,B=kB’,C=kC’,D=kD’,E=kE’,F=kF’,G=kG’,H=kH’,I=kI’,J=kJ’\]如果\(A,B\)近似于1，则\begin{array}{lcl}c’&=&\left(1-C\right)^{1/2}\\a’&=&E/-2c’\\b’&=&F/-2c’\end{array}
如果A近似1，B不近似1则\begin{array}{lcl}b’&=&\left(1-B\right)^{1/2}\\a’&=&D/-2b’\\c’&=&F/-2b’\end{array}
如果A不近似于1\begin{array}{lcl}a’&=&\left(1-A\right)^{1/2}\\b’&=&D/-2a’\\c’&=&E/-2a’\end{array}
将向量\(\left(a’,b’,c’\right)\)正规化得到单位向量\[\left(a,b,c\right)\]
注：近似于这个概念很模糊，由于我使用在机械测量中，通常设备采集原始点的精度为0.001左右，而点云本身的标准差在0.001~1，所以我在程序中将近似于定义为\(\Delta<0.001\)
上文两次提到近似于，第一次是\(DEF\)，用来判断向量是否平行于某一轴。第二次是\(A,B\)，实际上此时已经排除了向量平行于轴线的可能性，这里是用来判断向量更接近于哪个轴，从而避免近似0 的小数开平方造成的精度误差，所以在算法中我将这部分改为了比大小。
另外就是\(ABC\)三个值有可能略微大于1，用原文算法有可能出现负数开方造成无解。
\(x_0,y_0,z_0\)的初始值
根据\(x_0,y_0,z_0\)与\(G,H,I\)的关系，以及等式\(ax_0+by_0+cz_0=0\)（注：原文中使用原点到轴线的垂足作为起始点，因为个人需要，我将之替换为数据第一点投影到轴线的垂足\(ax_0+by_0+cz_0=ax_1+by_1+cz_1\)得到线性方程组
\[\begin{bmatrix}-2\left(b^2+c^2\right)&2ab&2ac\\2ab&-2\left(a^2+c^2\right)&2bc\\2ac&2bc&-2\left(a^2+b^2\right)\\a&b&c\end{bmatrix}\begin{bmatrix}x_0\\y_0\\z_0\end{bmatrix}=\begin{bmatrix}G\\H\\I\\ax_1+by_1+cz_1\end{bmatrix}\]
注：在实际使用中，这个初始值算法有一些问题，当圆柱的轴线非常接近于\((0,0,1)\)时（很常见，通常为了仪器测量方便都会先找正），该方程组变为了\[\begin{bmatrix}-2&0&0\\0&-2&0\\0&0&0\\0&0&1\end{bmatrix}\begin{bmatrix}x_0\\y_0\\z_0\end{bmatrix}=\begin{bmatrix}G\\H\\I\\z_1\end{bmatrix}\]这个一看就知道\(x_0=-\frac{G}{2},y_0=-\frac{H}{2},z_0=z_1\)，但是由于第三行都是0的存在，使得矩阵的任何操作都有问题。
\(r\)的初始值
原文使用了前面\(J\)的等式。\[R^2=(b^2+c^2)x_0^2+(a^2+c^2)y_0^2+(a^2+b^2)z_0^2-2bcy_0z_0-2acx_0z_0-2abx_0y_0-J\]
注：依然是在我的实际使用中，发现J的解在特定情况下会是极大的正数（可能是由于圆柱的轴线长度小于直径的原因），于是又出现了负数开方无解的情况，例如这组数据，于是我用了所有测量点半径平均值来代替。
迭代算法
1.坐标变换
在\((x_0,y_0,z_0)\)处以\((a,b,c)\)为Z轴建立一个坐标系U并将所有实测点的一个副本变换到该坐标系下。
\[\begin{bmatrix}x_i\\y_i\\z_i\end{bmatrix}=\left(\begin{bmatrix}x_i\\y_i\\z_i\end{bmatrix}-\begin{bmatrix}x_0\\y_0\\z_0\end{bmatrix}\right)U\]
其中\[U=\begin{bmatrix}1&0&0\\0&c_1&s_1\\0&-s1&c1\end{bmatrix}\begin{bmatrix}c_2&0&s2\\0&1&0\\-s2&0&c2\end{bmatrix}\]
当\((a,b,c)\)为\((1,0,0)\)时\(s_1=0,c_1=1,s_2=-1,c_2=0\)否则\[\begin{array}{lcl}c_1&=&\frac{c}{\sqrt{b^2+c^2}}\\s_1&=&\frac{-b}{\sqrt{b^2+c^2}}\\c_2&=&\frac{cc_1-b_s1}{\sqrt{a^2+(cc_1-bs_1)^2}}\\s_2&=&\frac{-a}{\sqrt{a^2+(cc_1-bs_1)^2}}\end{array}\]
2.构造Jacobian矩阵
参考直线距离公式，并且将\(a=b=0,c=1,z=0\)代入得到\[\begin{bmatrix}-x_1/r_1&-y_1/r_1&-x_1z_1/r_1&-y_1z_1/r_1&-1\\-x_2/r_2&-y_2/r_2&-x_2z_2/r_2&-y_2z_2/r_2&-1\\\vdots&\vdots&\vdots&\vdots&\vdots\\-x_m/r_m&-y_m/r_m&-x_mz_m/r_m&-y_mz_m/r_m&-1\end{bmatrix}\begin{bmatrix}P_{x0}\\P_{y0}\\P_a\\P_b\\P_r\end{bmatrix}=-1\begin{bmatrix}r_1-r\\r_2-r\\\vdots\\r_m-r\end{bmatrix}\]
3.更新参数
\[\begin{bmatrix}x_0\\y_0\\z_0\end{bmatrix}=\begin{bmatrix}x_0\\y_0\\z_0\end{bmatrix}+\begin{bmatrix}P_{x_0}\\P_{y_0}\\-P_{x_0}P_a-P_{y_0}P_b\end{bmatrix}U^T\]
\[\begin{bmatrix}a\\b\\c\end{bmatrix}=\begin{bmatrix}P_a\\P_b\\1\end{bmatrix}U^T\]
\[r=r+P_r\]
4.确定迭代结束条件
重复1~3，直到满足终止条件。
终止条件3选1：
- 新参数与旧参数对应相减，平方和小于退出值
- 求所有点的偏差二乘和，新值与旧值相减，差小于退出值
- 迭代次数超过退出值