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$可求得： u = u 2 = 1 1 / u 2 = 1 / 1 + v 2 u 2 + w 2 u 2 = 1 / ( 1 + ( 2 v s i n θ 2 u s i n θ ) 2 + ( 2 w s i n θ 2 u s i n θ ) 2 v = u 2 v s i n θ 2 u s i n θ w = u 2 w s i n θ 2 u s i n θ s i n θ = 2 u s i n θ / 2 u c o s θ = u 2 + ( v 2 + w 2 ) ( 1 − c o s θ ) − u 2 v 2 + w 2 = H o m M a t 3 D [ 0 ] − u 2 1 − u 2 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} u=u