如何将旋转矩阵转换为四元数 [英] How to convert rotation matrix to quaternion

问题描述

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

示例(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,2) + D(2,1) ) / ( 4*qmax )
        qy = qmax
        qz =  ( D(2,3) + D(3,2) ) / ( 4*qmax )
        qw = ±( D(1,3) - D(3,1) ) / ( 4*qmax )
elseif( M == D(3,3) )
        qx =  ( D(1,3) + D(3,1) ) / ( 4*qmax )
        qy =  ( D(2,3) + D(3,2) ) / ( 4*qmax )
        qz = qmax
        qw = ±( D(1,3) - D(3,1) ) / ( 4*qmax )
else
        qx = ±( D(3,2) - D(2,3) ) / ( 4*qmax )
        qy = ±( D(1,3) - D(3,1) ) / ( 4*qmax )
        qz = ±( D(2,1) - D(1,2) ) / ( 4*qmax )
        qw = qmax
endif

请注意，四元数本身就存在符号歧义。上面的算法任意选择最大元素qmax的符号为正，但将该符号选择为负号同样有效(即，基本上颠倒结果的所有符号)。应由用户根据应用程序确定哪个选项更合适。

±选择是基于您使用的四元数约定进行的：

选择+表示哈密尔顿左链约定或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]

这篇关于如何将旋转矩阵转换为四元数的文章就介绍到这了，希望我们推荐的答案对大家有所帮助，也希望大家多多支持IT屋！