Just for Fun

同前一篇文章相同，算法主要根据这篇文章，修改了其中一些笔误并微调以适合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：

• 新参数与旧参数对应相减，平方和小于退出值
• 求所有点的偏差二乘和，新值与旧值相减，差小于退出值
• 迭代次数超过退出值

包含直线、平面与圆柱算法的C#实例与说明

本系列的主要目的是将拟合算法用C#实现，相关的操作用到了Math.NET数值运算库，所以将一些基本概念，算法和对应的C#代码放到前面。

对于线性模型，例如直线和平面，算法大约分三步：

1. 参数化：将模型用参数化方式表达出来。
2. 构造Design（非线性中的Jacobian）矩阵：根据参数公式求所有参数的偏导数作为列(Column)，将实测值作为行(Row)。将偏差值作为等式右值。
3. 求解。

对于非线性模型，例如圆柱，Gauss-Newton算法类似：

1. 参数化：
2. 求（或猜）初始解。
3. 找到Jacobian矩阵及右值
4. 求解
5. 通过4计算逼近值，重复3和4
6. 迭代结束

线性模型的解法是基础，详细的分析请看这篇文章，这里只解释代码部分。在Math.Net中

对于矩阵\(A=\begin{bmatrix}A_{11} & A_{12}\\A_{21} & A_{22}\end{bmatrix}\)
A.Transpose() 代表A的转置矩阵\(A^T=\begin{bmatrix}A_{11} & A_{21}\\A_{12} & A_{22}\end{bmatrix}\)

这在坐标运算中是非常重要的，因为坐标运算中坐标系都表示为正交矩阵（抱歉，没找到证明），当计算一个点（用二维举例）\(P=\left(x,y\right)\)在新坐标系A中的坐标\(P^{‘}=\left(x^{‘},y^{‘}\right)\)时。\[P^{‘}=PA\]

而已知坐标系A中的坐标点\(P^{‘}\)求原坐标值可用\[P=P^{‘}A^T\]

一些类似的代码为：

A.Inverse()代表A的逆矩阵\(A^{-1}\)

A.Multiply(B)代表矩阵乘法\(A*B\)或者矩阵乘以向量\(A*b\)

对于常用的算法：

线性模型

已知Jacobian矩阵\(J\)及等式右值（偏差值向量）\(y\)，另\[Ax=y\]求解向量x

1. 正规方程（normal equations）
   \[A^TAx=A^Ty\]
   逆矩阵解法
   \[x=\left(A^TA\right)^{-1}A^Ty\]
   实现代码 

   x = A.Transpose().Multiply(A).Inverse().Multiply(A.Transpose().Multiply(y));

   更快和更稳定的方法将是对左侧的矩阵进行乔姆斯基分解（Cholesky decomposition），实现代码

   x = A.Transpose().Multiply(A).Cholesky().Solve(A.Transpose().Multiply(y));

   这两个在一些限定条件下是等价的，而这些限定条件在点数远大于参数的情况下是满足的。

2. QR分解
   推论见原文，代码更简单： 

   x=A.QR().Solve(y);

3. 奇异值分解（Singular Value Decomposition, SVD）
   A是方阵的情况下（即点数等于参数）代码为 

   x = A.Svd(true).Solve(y);

   A不是方阵的情况下代码为

// compute the SVD 
Svd svd = A.Svd(true);
// get matrix of left singular vectors with first n columns of U 
Matrix U1 = svd.U().SubMatrix(0, m,0, n); 
// get matrix of singular values 
Matrix S = new DiagonalMatrix(n, n, svd.S().ToArray()); 
// get matrix of right singular vectors 
Matrix V = svd.VT().Transpose(); 
x = V.Multiply(S.Inverse()).Multiply(U1.Transpose().Multiply(y));

非线性模型

这部分对于代码并没有什么区别，只是需要迭代（循环）及判断终止条件。