transformation - 计算 2 3D 笛卡尔坐标系之间转换的四元数

Question

我有两个已知单位向量的笛卡尔坐标系:

系统 A(x_A,y_A,z_A)

和

系统 B(x_B,y_B,z_B)

两个系统共享同一个原点 (0,0,0)。我正在尝试计算一个四元数，以便系统 B 中的向量可以在系统 A 中表示。

我熟悉四元数的数学概念。我已经从这里实现了所需的数学:http://content.gpwiki.org/index.php/OpenGL%3aTutorials%3aUsing_Quaternions_to_represent_rotation

一种可能的解决方案是计算欧拉角并将它们用于 3 个四元数。将它们相乘会得到最后一个，这样我就可以转换我的向量:

v(A) = q*v(B)*q_conj

但这将再次包含万向节锁定，这就是一开始不使用欧拉角的原因。

知道如何解决这个问题吗？

Answer 1

您可以通过本文中描述的方法计算表示从一个坐标系到另一个坐标系的最佳可能变换的四元数:

Paul J. Besl 和 Neil D. McKay
“3-D 形状注册方法”，传感器融合 IV:控制范式和数据结构，586(1992 年 4 月 30 日)； http://dx.doi.org/10.1117/12.57955

该论文不是开放获取的，但我可以向您展示 Python 实现:

def get_quaternion(lst1,lst2,matchlist=None):
    if not matchlist:
        matchlist=range(len(lst1))
    M=np.matrix([[0,0,0],[0,0,0],[0,0,0]])

    for i,coord1 in enumerate(lst1):
        x=np.matrix(np.outer(coord1,lst2[matchlist[i]]))
        M=M+x

    N11=float(M[0][:,0]+M[1][:,1]+M[2][:,2])
    N22=float(M[0][:,0]-M[1][:,1]-M[2][:,2])
    N33=float(-M[0][:,0]+M[1][:,1]-M[2][:,2])
    N44=float(-M[0][:,0]-M[1][:,1]+M[2][:,2])
    N12=float(M[1][:,2]-M[2][:,1])
    N13=float(M[2][:,0]-M[0][:,2])
    N14=float(M[0][:,1]-M[1][:,0])
    N21=float(N12)
    N23=float(M[0][:,1]+M[1][:,0])
    N24=float(M[2][:,0]+M[0][:,2])
    N31=float(N13)
    N32=float(N23)
    N34=float(M[1][:,2]+M[2][:,1])
    N41=float(N14)
    N42=float(N24)
    N43=float(N34)

    N=np.matrix([[N11,N12,N13,N14],\
              [N21,N22,N23,N24],\
              [N31,N32,N33,N34],\
              [N41,N42,N43,N44]])


    values,vectors=np.linalg.eig(N)
    w=list(values)
    mw=max(w)
    quat= vectors[:,w.index(mw)]
    quat=np.array(quat).reshape(-1,).tolist()
    return quat

此函数返回您要查找的四元数。参数 lst1 和 lst2 是 numpy.arrays 列表，其中每个数组代表一个 3D 向量。如果两个列表的长度都是 3(并且包含正交单位向量)，则四元数应该是精确的变换。如果您提供更长的列表，您将获得最小化两个点集之间差异的四元数。
可选的 matchlist 参数用于告诉函数应该将 lst2 的哪个点转换为 lst1 中的哪个点。如果未提供匹配列表，则该函数假定 lst1 中的第一个点应与 lst2 中的第一个点匹配，依此类推...

C++ 中 3 个点集的类似函数如下:

#include <Eigen/Dense>
#include <Eigen/Geometry>

using namespace Eigen;

/// Determine rotation quaternion from coordinate system 1 (vectors
/// x1, y1, z1) to coordinate system 2 (vectors x2, y2, z2)
Quaterniond QuaternionRot(Vector3d x1, Vector3d y1, Vector3d z1,
                          Vector3d x2, Vector3d y2, Vector3d z2) {

    Matrix3d M = x1*x2.transpose() + y1*y2.transpose() + z1*z2.transpose();

    Matrix4d N;
    N << M(0,0)+M(1,1)+M(2,2)   ,M(1,2)-M(2,1)          , M(2,0)-M(0,2)         , M(0,1)-M(1,0),
         M(1,2)-M(2,1)          ,M(0,0)-M(1,1)-M(2,2)   , M(0,1)+M(1,0)         , M(2,0)+M(0,2),
         M(2,0)-M(0,2)          ,M(0,1)+M(1,0)          ,-M(0,0)+M(1,1)-M(2,2)  , M(1,2)+M(2,1),
         M(0,1)-M(1,0)          ,M(2,0)+M(0,2)          , M(1,2)+M(2,1)         ,-M(0,0)-M(1,1)+M(2,2);

    EigenSolver<Matrix4d> N_es(N);
    Vector4d::Index maxIndex;
    N_es.eigenvalues().real().maxCoeff(&maxIndex);

    Vector4d ev_max = N_es.eigenvectors().col(maxIndex).real();

    Quaterniond quat(ev_max(0), ev_max(1), ev_max(2), ev_max(3));
    quat.normalize();

    return quat;
}