如何将旋转矩阵转换为四元数

编程问答 2022-08-12

问题描述

我可以将旋转矩阵转换为四元数吗？ 我知道如何将四元数转换为旋转矩阵，但是我找不到相反的方法。 我可以向您展示代码，如何将四元数转换为波纹管。

示例（C ++）：Quaterniond quat; MatrixXd t; t = quat.matrix(); 我想知道将旋转矩阵转换为四元数的方法。

解决方法

将方向余弦矩阵D转换为四元数q的数值稳定算法如下：

T = D(1,1) + D(2,2) + D(3,3)
M = max( D(1,1),D(2,2),D(3,3),T )
qmax = (1/2) * sqrt( 1 – T + 2*M )
if( M == D(1,1) )
        qx = qmax
        qy =  ( D(1,2) + D(2,1) ) / ( 4*qmax )
        qz =  ( D(1,3) + D(3,1) ) / ( 4*qmax )
        qw = ±( D(3,2) - D(2,3) ) / ( 4*qmax )
elseif( M == D(2,2) )
        qx =  ( D(1,1) ) / ( 4*qmax )
        qy = qmax
        qz =  ( D(2,2) ) / ( 4*qmax )
        qw = ±( D(1,3) - D(3,1) ) / ( 4*qmax )
elseif( M == D(3,3) )
        qx =  ( D(1,1) ) / ( 4*qmax )
        qy =  ( D(2,2) ) / ( 4*qmax )
        qz = qmax
        qw = ±( D(1,1) ) / ( 4*qmax )
else
        qx = ±( D(3,3) ) / ( 4*qmax )
        qy = ±( D(1,1) ) / ( 4*qmax )
        qz = ±( D(2,1) - D(1,2) ) / ( 4*qmax )
        qw = qmax
endif

请注意，四元数中固有有符号歧义。上面的算法任意选择最大元素qmax的符号为正，但是将这个符号选择为负同样有效（即，基本上翻转结果的所有符号）。用户可以根据应用程序来确定哪个更合适。

根据您使用的四元数约定进行±选择：

为汉密尔顿左链公约或JPL右链公约选择+

选择-用于汉密尔顿右链公约或JPL左链公约

汉密尔顿约定表示四元数元素i，j，k以右手方式进行乘法运算（如叉积）：

 i * j = k,j * k = i,k * i = j

JPL约定表示四元数元素i，j，k以左手方式进行乘法运算（叉积为负）：

 i * j = -k,j * k = -i,k * i = -j

右链表示矢量的四元数旋转操作在右侧具有未修改的四元数：

 D * v1 = v2 = q^-1 * v1 * q

左链表示矢量的四元数旋转操作在左侧具有未修改的四元数：

 D * v1 = v2 = q * v1 * q^-1

为完整起见，这是另一个方向的算法，将四元数转换为方向余弦矩阵：

 D = (qw^2 - dot(qv,qv))*I3 + 2*qv*qv^T ± 2*qw*Skew(qv)

其中^ T表示转置（对于该术语中的外部乘积），

qv = [qx]
     [qy]
     [qz]

I3 = [1 0 0]
     [0 1 0]
     [0 0 1]

Skew(qv) = [  0 -qz  qy]
           [ qz   0 -qx]
           [-qy  qx   0]
eigenquaternionsrotation