对四元数旋转应用镜像效果的有效方法?

Efficient way to apply mirror effect on quaternion rotation?

四元数表示旋转 - 它们不包含有关缩放或镜像的信息。然而,仍然可以镜像旋转的效果。

考虑 x-y 平面上的镜像(我们也可以称之为沿 z 轴的镜像)。在 x-y 平面上镜像的围绕 x 轴的旋转将被否定。同样绕 y 轴旋转。但是,绕 z 轴的旋转将保持不变。

另一个示例:在 x-y 平面中镜像的绕轴 (1,1,1) 旋转 90º 将绕 (1,1,-1) 旋转 -90º。为了帮助直觉,如果您可以可视化轴的描述和指示旋转的圆形箭头,则镜像该可视化表示新的旋转应该是什么。

我找到了一种计算旋转镜像的方法,如下所示:

这仅支持沿主轴 x、y 和 z 的镜像,因为这就是我所需要的。它适用于任意旋转。

但是,从四元数到角轴的转换以及从角轴到四元数的转换成本很高。我想知道是否有一种方法可以直接对四元数本身进行转换,但我对四元数数学的理解不足以让我自己得到任何结果。

(由于高效计算方法的重要性,发布在 Whosebug 而不是数学相关的论坛上。)

我们可以检查 3D 中所有旋转和反射的集合,这称为 Orthogonal group O(3)。它可以看作是行列式为 +1 或 -1 的正交矩阵集。所有旋转都具有行列式 +1,纯反射具有行列式 -1。 O(3) 的另一个成员是点 (x,y,z)->(-x,-y,-z) 的反转,这在 3D 中有 det -1,我们稍后会谈到这一点。如果我们将组中的两个转换组合在一起,您将乘以它们的行列式。因此,两个旋转组合产生另一个旋转 (+1 * +1 = +1),旋转与反射组合产生反射 (+1 * -1 = -1),两个反射组合产生旋转 (-1 * -1 = +1).

我们可以将 O(3) 限制为仅具有行列式 +1 的 O(3) 以形成 Special Orthogonal Group SO(3)。这只包含旋转。

现在单位四元数的集合是SO(3)的双覆盖,也就是说每次旋转对应两个单位四元数。准确地说,如果 a+b i+c j+d k 是一个单位四元数,那么 a-b i-c j-d k 代表相同的旋转,你可以认为这是围绕向量 (b,c,d) 旋转 ø 与旋转相同通过向量 (-b,-c,-d) 周围的 -ø。

注意所有单位四元数的行列式都是+1,所以有none对应的是纯反射。这就是为什么不能使用四元数来表示反射的原因。

你可以做的是使用反演。现在,反射之后的反转是旋转。例如在 x=0 中反映并反转,与在 y=0 中反映和在 z=0 中反映相同。这与绕 x 轴旋转 180º 相同。您可以对任何反射执行相同的过程。


我们可以通过使用法向量 n = (a,b,c) 来定义一个通过原点的平面。 reflection 向量 v(x,y,z) 在该平面中由

给出

v - 2 (v . n ) / ( n . n) n

= (x,y,z) - 2 (a x+b y+c z) / (a^2+b^2+c^2) (a,b,c)

特别是 x-y 平面具有法线 (0,0,1),因此反射是

(x,y,z) - 2 z (0,0,1) = (x,y,-z)

Quaternions and spatial rotation 从轴角公式中得到一个很好的四元数公式。

p = cos(ø/2) + (x i + y j + z k) sin(ø/2)

这是一个四元数 W + X i + Y j + Z k with W=cos(ø/2), X = x sin(ø/2), Y = y sin(ø/2), Z = z sin(ø/2)

改变旋转方向会翻转半角的sin,但cos不变,给出

p' = cos(ø/2) - (x i + y j + z k) sin(ø/2)

现在如果我们考虑在 x-y 平面中反映相应的向量给出

q = cos(ø/2) + (x i + y j - z k) sin(ø/2)

我们可能想改变给定的旋转方向

q' = cos(ø/2) + (- x i - y j + z k ) sin(ø/2)

= W - X i - Y j + Z k

我认为这符合您的回答。

我们可以将其推广到具有单位长度法线 (a,b,c) 的一般平面中的反射。令 d 为点积 (a,b,c).(x,y,z)。 (x,y,z) 的反射是

(x,y,z) - 2 d (a,b,c) = (x - 2 d a, y - 2 d b, z - 2 d c)

这个的旋转四元数

q = cos(ø/2) - ((x - 2 d a) i + ((y - 2 d b) j + (z - 2 d c) k) sin(ø/2)

q = cos(ø/2) - (x i + y j + z k) sin(ø/2) + 2 d sin(ø/2) (a i + b j + c k)

= W - X i - Y j - Z k + 2 d ( X,Y,Z).(a,b,c) (a i + b j + c k)

我做了一些进一步的分析,似乎四元数 (w, x, y, z) 的效果可以像这样镜像:

  • 通过翻转四元数的 y 和 z 元素沿 x 轴旋转的镜像效果。
  • 通过翻转四元数的 x 和 z 元素沿 y 轴旋转的镜像效果。
  • 通过翻转四元数的 x 和 y 元素沿 z 轴旋转的镜像效果。

永远不需要触及四元数的 w 元素。

不幸的是,我对四元数的理解仍然不够好,无法解释为什么会这样,但我是从轴角格式转换的实现中推导出来的,在实现这个解决方案之后,它的工作原理与以及我对它进行的所有测试中的原始版本。

请注意,镜像不是旋转,因此通常您不能将其烘焙成四元数(不过我很可能误解了您的问题)。镜像变换矩阵的3x3分量为

M = I-2(n*nT) 

其中 I 是单位 3x3 矩阵,n 是镜像平面的法线表示为 3x1 矩阵,nT 是 n 作为 1x3 矩阵(因此 n*nT 是 3x(1x1)x3=3x3 矩阵)。

现在,如果您想要 'mirror' 的四元数 q 是最后一个变换,则另一侧的最后一个变换将只是 M*q(同样,这将是一个通用的 3x3 矩阵,而不是一般的可表示为四元数)

我刚刚花了很多时间想出这个问题的明确答案,所以我把它张贴在这里以供记录。

简介

如其他答案中所述,镜像效果不能表示为旋转。然而,给定从坐标系 C1 到坐标系 C2 的旋转 R1to2,我们可能有兴趣在对 C1 和 C2 应用相同的镜像效果时有效地计算等效旋转(例如,我面临将左手坐标系中给定的输入四元数转换为代表相同旋转但在右手坐标系中的四元数的问题。

在旋转矩阵方面,可以这样理解:

R_mirroredC1_to_mirroredC2 = M_mirrorC2 * R_C1_to_C2 * M_mirrorC1

这里,R_C1_to_C2R_mirroredC1_to_mirroredC2都代表有效的旋转,那么在处理四元数时,如何从q_C1_to_C2高效地计算q_mirroredC1_to_mirroredC2

解决方案

以下假设q_C1_to_C2=[w,x,y,z]:

  • 如果 C1 和 C2 沿 X 轴镜像(即 M_mirrorC1=M_mirrorC2=diag_3x3(-1,1,1)),则 q_mirroredC1_to_mirroredC2=[w,x,-y,-z]
  • 如果 C1 和 C2 沿 Y 轴镜像(即 M_mirrorC1=M_mirrorC2=diag_3x3(1,-1,1)),则 q_mirroredC1_to_mirroredC2=[w,-x,y,-z]
  • 如果 C1 和 C2 沿 Z 轴镜像(即 M_mirrorC1=M_mirrorC2=diag_3x3(1,1,-1)),则 q_mirroredC1_to_mirroredC2=[w,-x,-y,z]

当考虑 C1 和 C2 的不同镜像轴时,我们有以下几点:

  • 如果 C1 沿 X 轴镜像,C2 沿 Y 轴镜像(即 M_mirrorC1=diag_3x3(-1,1,1) & M_mirrorC2=diag_3x3(1,-1,1)),则 q_mirroredC1_to_mirroredC2=[z,y,x,w]
  • 如果 C1 沿 X 轴镜像,C2 沿 Z 轴镜像(即 M_mirrorC1=diag_3x3(-1,1,1) & M_mirrorC2=diag_3x3(1,1,-1)),则 q_mirroredC1_to_mirroredC2=[-y,z,-w,x]

  • 如果 C1 沿 Y 轴镜像,C2 沿 X 轴镜像(即 M_mirrorC1=diag_3x3(1,-1,1) & M_mirrorC2=diag_3x3(-1,1,1)),则 q_mirroredC1_to_mirroredC2=[z,-y,-x,w]

  • 如果 C1 沿 Y 轴镜像,C2 沿 Z 轴镜像(即 M_mirrorC1=diag_3x3(1,-1,1) & M_mirrorC2=diag_3x3(1,1,-1)),则 q_mirroredC1_to_mirroredC2=[x,w,z,y]

  • 如果 C1 沿 Z 轴镜像,C2 沿 X 轴镜像(即 M_mirrorC1=diag_3x3(1,1,-1) & M_mirrorC2=diag_3x3(-1,1,1)),则 q_mirroredC1_to_mirroredC2=[y,z,w,x]

  • 如果 C1 沿 Z 轴镜像,C2 沿 Y 轴镜像(即 M_mirrorC1=diag_3x3(1,1,-1) & M_mirrorC2=diag_3x3(1,-1,1)),则 q_mirroredC1_to_mirroredC2=[x,w,-z,-y]

测试程序

这是一个基于 OpenCV 的小 c++ 程序来测试这一切:

#include <opencv2/opencv.hpp>
#define CST_PI 3.1415926535897932384626433832795

// Random rotation matrix uniformly sampled from SO3 (see "Fast random rotation matrices" by J.Arvo)
cv::Matx<double,3,3> get_random_rotmat()
{
    double theta1 = 2*CST_PI*cv::randu<double>();
    double theta2 = 2*CST_PI*cv::randu<double>();
    double x3 = cv::randu<double>();
    cv::Matx<double,3,3> R(std::cos(theta1),std::sin(theta1),0,-std::sin(theta1),std::cos(theta1),0,0,0,1);
    cv::Matx<double,3,1> v(std::cos(theta2)*std::sqrt(x3),std::sin(theta2)*std::sqrt(x3),std::sqrt(1-x3));
    return -1*(cv::Matx<double,3,3>::eye()-2*v*v.t())*R;
}

cv::Matx<double,4,1> rotmat2quatwxyz(const cv::Matx<double,3,3> &R)
{
    // Implementation from Ceres 1.10
    const double trace = R(0,0) + R(1,1) + R(2,2);
    cv::Matx<double,4,1> quat_wxyz;
    if (trace >= 0.0) {
        double t = sqrt(trace + 1.0);
        quat_wxyz(0) = 0.5 * t;
        t = 0.5 / t;
        quat_wxyz(1) = (R(2,1) - R(1,2)) * t;
        quat_wxyz(2) = (R(0,2) - R(2,0)) * t;
        quat_wxyz(3) = (R(1,0) - R(0,1)) * t;
    } else {
        int i = 0;
        if (R(1, 1) > R(0, 0))
            i = 1;
        if (R(2, 2) > R(i, i))
            i = 2;

        const int j = (i + 1) % 3;
        const int k = (j + 1) % 3;
        double t = sqrt(R(i, i) - R(j, j) - R(k, k) + 1.0);
        quat_wxyz(i + 1) = 0.5 * t;
        t = 0.5 / t;
        quat_wxyz(0)     = (R(k,j) - R(j,k)) * t;
        quat_wxyz(j + 1) = (R(j,i) + R(i,j)) * t;
        quat_wxyz(k + 1) = (R(k,i) + R(i,k)) * t;
    }
    // Check that the w element is positive
    if(quat_wxyz(0)<0)
        quat_wxyz *= -1;    // quat and -quat represent the same rotation, but to make quaternion comparison easier, we always use the one with positive w
    return quat_wxyz;
}

cv::Matx<double,4,1> apply_quaternion_trick(const unsigned int item_permuts[4], const int sign_flips[4], const cv::Matx<double,4,1>& quat_wxyz)
{
    // Flip the sign of the x and z components
    cv::Matx<double,4,1> quat_flipped(sign_flips[0]*quat_wxyz(item_permuts[0]),sign_flips[1]*quat_wxyz(item_permuts[1]),sign_flips[2]*quat_wxyz(item_permuts[2]),sign_flips[3]*quat_wxyz(item_permuts[3]));
    // Check that the w element is positive
    if(quat_flipped(0)<0)
        quat_flipped *= -1; // quat and -quat represent the same rotation, but to make quaternion comparison easier, we always use the one with positive w
    return quat_flipped;
}

void detect_quaternion_trick(const cv::Matx<double,4,1> &quat_regular, const cv::Matx<double,4,1> &quat_flipped, unsigned int item_permuts[4], int sign_flips[4])
{
    if(abs(quat_regular(0))==abs(quat_flipped(0))) {
        item_permuts[0]=0;
        sign_flips[0] = (quat_regular(0)/quat_flipped(0)>0 ? 1 : -1);
    }
    else if(abs(quat_regular(0))==abs(quat_flipped(1))) {
        item_permuts[1]=0;
        sign_flips[1] = (quat_regular(0)/quat_flipped(1)>0 ? 1 : -1);
    }
    else if(abs(quat_regular(0))==abs(quat_flipped(2))) {
        item_permuts[2]=0;
        sign_flips[2] = (quat_regular(0)/quat_flipped(2)>0 ? 1 : -1);
    }
    else if(abs(quat_regular(0))==abs(quat_flipped(3))) {
        item_permuts[3]=0;
        sign_flips[3] = (quat_regular(0)/quat_flipped(3)>0 ? 1 : -1);
    }
    if(abs(quat_regular(1))==abs(quat_flipped(0))) {
        item_permuts[0]=1;
        sign_flips[0] = (quat_regular(1)/quat_flipped(0)>0 ? 1 : -1);
    }
    else if(abs(quat_regular(1))==abs(quat_flipped(1))) {
        item_permuts[1]=1;
        sign_flips[1] = (quat_regular(1)/quat_flipped(1)>0 ? 1 : -1);
    }
    else if(abs(quat_regular(1))==abs(quat_flipped(2))) {
        item_permuts[2]=1;
        sign_flips[2] = (quat_regular(1)/quat_flipped(2)>0 ? 1 : -1);
    }
    else if(abs(quat_regular(1))==abs(quat_flipped(3))) {
        item_permuts[3]=1;
        sign_flips[3] = (quat_regular(1)/quat_flipped(3)>0 ? 1 : -1);
    }
    if(abs(quat_regular(2))==abs(quat_flipped(0))) {
        item_permuts[0]=2;
        sign_flips[0] = (quat_regular(2)/quat_flipped(0)>0 ? 1 : -1);
    }
    else if(abs(quat_regular(2))==abs(quat_flipped(1))) {
        item_permuts[1]=2;
        sign_flips[1] = (quat_regular(2)/quat_flipped(1)>0 ? 1 : -1);
    }
    else if(abs(quat_regular(2))==abs(quat_flipped(2))) {
        item_permuts[2]=2;
        sign_flips[2] = (quat_regular(2)/quat_flipped(2)>0 ? 1 : -1);
    }
    else if(abs(quat_regular(2))==abs(quat_flipped(3))) {
        item_permuts[3]=2;
        sign_flips[3] = (quat_regular(2)/quat_flipped(3)>0 ? 1 : -1);
    }
    if(abs(quat_regular(3))==abs(quat_flipped(0))) {
        item_permuts[0]=3;
        sign_flips[0] = (quat_regular(3)/quat_flipped(0)>0 ? 1 : -1);
    }
    else if(abs(quat_regular(3))==abs(quat_flipped(1))) {
        item_permuts[1]=3;
        sign_flips[1] = (quat_regular(3)/quat_flipped(1)>0 ? 1 : -1);
    }
    else if(abs(quat_regular(3))==abs(quat_flipped(2))) {
        item_permuts[2]=3;
        sign_flips[2] = (quat_regular(3)/quat_flipped(2)>0 ? 1 : -1);
    }
    else if(abs(quat_regular(3))==abs(quat_flipped(3))) {
        item_permuts[3]=3;
        sign_flips[3] = (quat_regular(3)/quat_flipped(3)>0 ? 1 : -1);
    }
}

int main(int argc, char **argv)
{
    cv::Matx<double,3,3> M_xflip(-1,0,0,0,1,0,0,0,1);
    cv::Matx<double,3,3> M_yflip(1,0,0,0,-1,0,0,0,1);
    cv::Matx<double,3,3> M_zflip(1,0,0,0,1,0,0,0,-1);

    // Let the user choose the configuration
    char im,om;
    std::cout << "Enter the axis (x,y,z) along which input ref is flipped:" << std::endl;
    std::cin >> im;
    std::cout << "Enter the axis (x,y,z) along which output ref is flipped:" << std::endl;
    std::cin >> om;
    cv::Matx<double,3,3> M_iflip,M_oflip;
    if(im=='x') M_iflip=M_xflip;
    else if(im=='y') M_iflip=M_yflip;
    else if(im=='z') M_iflip=M_zflip;
    if(om=='x') M_oflip=M_xflip;
    else if(om=='y') M_oflip=M_yflip;
    else if(om=='z') M_oflip=M_zflip;

    // Generate random quaternions until we find one where no two elements are equal
    cv::Matx<double,3,3> R;
    cv::Matx<double,4,1> quat_regular,quat_flipped;
    do {
        R = get_random_rotmat();
        quat_regular = rotmat2quatwxyz(R);
    } while(quat_regular(0)==quat_regular(1) || quat_regular(0)==quat_regular(2) || quat_regular(0)==quat_regular(3) ||
            quat_regular(1)==quat_regular(2) || quat_regular(1)==quat_regular(3) ||
            quat_regular(2)==quat_regular(3));

    // Determine and display the appropriate quaternion trick
    quat_flipped = rotmat2quatwxyz(M_oflip*R*M_iflip);
    unsigned int item_permuts[4]={0,1,2,3};
    int sign_flips[4]={1,1,1,1};
    detect_quaternion_trick(quat_regular,quat_flipped,item_permuts,sign_flips);
    char str_quat[4]={'w','x','y','z'};
    std::cout << std::endl << "When iref is flipped along the " << im << "-axis and oref along the " << om << "-axis:" << std::endl;
    std::cout << "resulting_quat=[" << (sign_flips[0]>0?"":"-") << str_quat[item_permuts[0]] << ","
                                    << (sign_flips[1]>0?"":"-") << str_quat[item_permuts[1]] << ","
                                    << (sign_flips[2]>0?"":"-") << str_quat[item_permuts[2]] << ","
                                    << (sign_flips[3]>0?"":"-") << str_quat[item_permuts[3]] << "], where initial_quat=[w,x,y,z]" << std::endl;

    // Test this trick on several random rotation matrices
    unsigned int n_errors = 0, n_tests = 10000;
    std::cout << std::endl << "Performing " << n_tests << " tests on random rotation matrices:" << std::endl;
    for(unsigned int i=0; i<n_tests; ++i) {

        // Get a random rotation matrix and the corresponding quaternion
        cv::Matx<double,3,3> R = get_random_rotmat();
        cv::Matx<double,4,1> quat_regular = rotmat2quatwxyz(R);
        // Get the quaternion corresponding to the flipped coordinate frames, via the sign trick and via computation on rotation matrices
        cv::Matx<double,4,1> quat_tricked = apply_quaternion_trick(item_permuts,sign_flips,quat_regular);
        cv::Matx<double,4,1> quat_flipped = rotmat2quatwxyz(M_oflip*R*M_iflip);
        // Check that both results are identical
        if(cv::norm(quat_tricked-quat_flipped,cv::NORM_INF)>1e-6) {
            std::cout << "Error (idx=" << i << ")!"
                      << "\n   quat_regular=" << quat_regular.t()
                      << "\n   quat_tricked=" << quat_tricked.t()
                      << "\n   quat_flipped=" << quat_flipped.t() << std::endl;
            ++n_errors;
        }
    }
    std::cout << n_errors << " errors on " << n_tests << " tests." << std::endl;
    system("pause");
    return 0;
}

对于通过网络搜索来到这里并正在寻找数学的任何人,那么:


反思

通过平面ax+by+cz=0反射点'p',使用四元数:

n = 0+(a,b,c)
p = 0+(x,y,z)

其中 'n' 是一个单位双矢量(如果你愿意,也可以是纯四元数)

p' = npn

那么p'就是反射点。

如果你用第二次反射作曲'm':

p' = mnpnm = (mn)p(mn)^*

是一个轮换。

旋转和反射按预期合成。


统一缩放

由于标量积交换并且可以被分解,那么如果我们有单位四元数的旋转 'Q' 或单位双矢量 'b' 的反射(或任何组合)乘以一些非零缩放值 's' 导致 s^2 的统一缩放。并且由于 (sqrt(s0)*sqrt(s1))^2 = s0*s1,这些统一缩放值按预期组合。

然而,这一点可能没有意义,因为在代码中我们希望能够假定单位幅度值以降低运行时复杂性。

有一种更容易和面向程序员的方式来思考这个问题。假设您想在坐标系中反转 z 轴(即将 z 轴翻转为 -z)。现在将四元数视为滚动、俯仰和偏航方面的方向向量。当您翻转 z 轴时,请注意横滚和俯仰的符号是相反的,但偏航的符号保持不变。

现在您可以使用以下将欧拉角转换为四元数的代码找到对四元数的净影响(我将此代码放入维基百科):

static Quaterniond toQuaternion(double pitch, double roll, double yaw)
{
    Quaterniond q;
    double t0 = std::cos(yaw * 0.5f);
    double t1 = std::sin(yaw * 0.5f);
    double t2 = std::cos(roll * 0.5f);
    double t3 = std::sin(roll * 0.5f);
    double t4 = std::cos(pitch * 0.5f);
    double t5 = std::sin(pitch * 0.5f);

    q.w() = t0 * t2 * t4 + t1 * t3 * t5;
    q.x() = t0 * t3 * t4 - t1 * t2 * t5;
    q.y() = t0 * t2 * t5 + t1 * t3 * t4;
    q.z() = t1 * t2 * t4 - t0 * t3 * t5;
    return q;
}

使用基本三角函数,sin(-x) = -sin(x)cos(-x) = cos(x)。将此应用于上面的代码,您可以看到 t3 和 t5 的符号将翻转。这将导致 x 和 y 的符号翻转。

所以当你反转 z 轴时,

Q'(w, x, y, z) = Q(w, -x, -y, z)

同样,您可以计算出轴反转的任何其他组合并找出对四元数的影响。

PS:如果有人想知道为什么有人会需要这个……我需要在上面转换来自控制无人机的 MavLink/Pixhawk 系统的四元数。源系统使用 NED 坐标系,但通常的 3D 环境(如 Unreal)使用 NEU 坐标系,这需要将 z 轴转换为 -z 才能正确使用四元数。