Halcon|通过旋转前后3维坐标点求旋转轴及旋转角度

绕空间任意轴的旋转矩阵

绕空间任意轴旋转的旋转矩阵为：
点击查看图片来源及公式推导
其中，(u,v,w)为旋转轴的单位矢量，(a,b,c)为旋转轴上任意一点，$\theta$为旋转角度。

Halcon求解旋转矩阵

在Halcon中，已知n个空间点旋转前的坐标：X1，Y1，Z1，以及旋转后的坐标：X2，Y2，Z2，可使用算子 vector_to_hom_mat3d 求解旋转矩阵。X1，Y1，Z1，X2，Y2，Z2均为含n个元素的向量。
例如：

vector_to_hom_mat3d ('rigid', X1 Y1, Z1, X2, Y2, Z2, HomMat3D)

HomMat3D即为求得的旋转矩阵。
vector_to_hom_mat3d算子中，第一个参数选择‘rigid’表示刚体变换（即仅有旋转+平移运动），此时已知旋转前后坐标点的个数至少要等于3，即n>=3；选择‘similarity’表示相似变换（即旋转+平移+缩放），此时需n>=3；选择‘affine’表示仿射变换，此时需n>=4；选择‘projective’表示投影变换，此时需n>=5。当坐标点量大于所需最少点数时，将采用最小二乘法进行拟合。

求旋转轴矢量及旋转角度

在Halcon中，二维矩阵的下标从0开始，并且按照先行后列的方式进行，即$$\begin{gathered} HomMat3D[0]=u^2+(v^2+w^2)(1-cos\theta ) \\ HomMat3D[1]=uv(1-cos\theta )-wsin\theta \end{gathered}$$依此类推。
则参考旋转矩阵的形式，可知：$$\begin{gathered} 2usin\theta =HomMat3D[9]-HomMat3D[6] \\ 2vsin\theta =HomMat3D[2]-HomMat3D[8] \\ 2wsin\theta =HomMat3D[4]-HomMat3D[1] \end{gathered}$$
则利用$u^2+v^2+w^2=1$可求得：$$\begin{gathered} u=\sqrt{u^2}=\frac{1}{\sqrt{1/u^2}}=1/\sqrt{1+\frac{v^2}{u^2}+\frac{w^2}{u^2}}=1/\sqrt{(1+(\frac{2vsin\theta }{2usin\theta }{)}^2+(\frac{2wsin\theta }{2usin\theta }{)}^2} \\ v=u\frac{2vsin\theta }{2usin\theta } \\ w=u\frac{2wsin\theta }{2usin\theta } \\ sin\theta =2usin\theta /2u \\ cos\theta =\frac{u^2+(v^2+w^2)(1-cos\theta )-u^2}{v^2+w^2}=\frac{HomMat3D[0]-u^2}{1-u^2} \end{gathered}$$
至此，旋转轴矢量(u,v,w)以及旋转角度$\theta$均已求出。

求旋转轴上一点

设(a,b,c)为旋转轴上任意一点，则根据旋转矩阵第四列的形式可得：
$$A\ast x=y$$
其中，$$A=\left(\begin{array}{ccc}(1-u^2)(1-cos\theta )& wsin\theta -uv(1-cos\theta )& -vsin\theta -uw(1-cos\theta )\\ -wsin\theta -uv(1-cos\theta )& (1-v^2)(1-cos\theta )& u\ast sin\theta -vw\ast (1-cos\theta )\\ vsin\theta -uw(1-cos\theta )& -u\ast sin\theta -vw(1-cos\theta )& (1-w^2)(1-cos\theta )\end{array}\right)$$
$$x=\left(\begin{array}{c}a\\ b\\ c\end{array}\right)$$
$$y=\left(\begin{array}{c}HomMat3D[3]\\ HomMat3D[7]\\ HomMat3D[11]\end{array}\right)$$
如果直接求解该方程，会发现A近似于奇异，无法求解。原因如下：
我们知道，坐标轴上任一点(a,b,c)可写成如下形式：
$$(a,b,c)=(a_0,b_0,c_0)+k(u,v,w)$$
其中，k为任意实数，$(a_0,b_0,c_0)$为坐标轴上一基点。可知，该方程的解是由一个特解和一个通解组成的，且通解$(u,v,w)$我们已经求得。由线性代数知识可知，$rank(A)=2$，即A奇异不可逆。
为求通解，可令$c$为任意值$c_0$，之后使用 $A\ast x=y$ 的三个方程中任意两个方程求得$b_0$、$c_0$，即可得到通解$(a_0,b_0,c_0)$。
需要注意：由于此时A的每一个元素都较小，利用$A\ast x=y$ 任意两个方程的求解的$b_0$、$c_0$值，也可能由于矩阵近于奇异造成较大的误差。此时需要其他方法测量得到旋转轴上的一个点。

小结

至此，我们已经求得了旋转轴的单位矢量(u,v,w)，旋转轴上任意一点(a,b,c)和旋转角度$\theta$。
在机器人学中，控制转动角度$\theta$，利用旋转轴的单位矢量(u,v,w)和旋转轴上任意一点(a,b,c)，可直接求解得旋转矩阵，之后利用旋转矩阵，可求解得到数据点旋转后的坐标。
在计算机视觉中，将标定板绕着旋转轴旋转任意角度，利用标定板求得旋转前后的坐标，可使用以上方法可以对旋转轴进行标定。