我有两个已知单位向量的笛卡尔坐标系:
系统(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_j
但这将再次包含Gimbal Lock，这也是为什么一开始不使用欧拉角的原因。
你知道怎么解决这个问题吗?
可以用本文描述的方法计算表示从一个坐标系到另一个坐标系的最佳变换的四元数:
Paul J. Besl和Neil D. McKay"三维形状的配准方法"，传感器融合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的列表。数组，其中每个数组代表一个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;
}
你在用什么语言?如果是c++，请随意使用我的开源库:
http://sourceforge.net/p/transengine/code/HEAD/tree/transQuaternion/简而言之，您需要将向量转换为四元数，进行计算，然后将四元数转换为变换矩阵。
下面是一个代码片段:来自vector的四元数:
cQuat nTrans::quatFromVec( Vec vec ) {
float angle = vec.v[3];
float s_angle = sin( angle / 2);
float c_angle = cos( angle / 2);
return (cQuat( c_angle, vec.v[0]*s_angle, vec.v[1]*s_angle,
vec.v[2]*s_angle )).normalized();
}
对于来自四元数的矩阵:
Matrix nTrans::matFromQuat( cQuat q ) {
Matrix t;
q = q.normalized();
t.M[0][0] = ( 1 - (2*q.y*q.y + 2*q.z*q.z) );
t.M[0][1] = ( 2*q.x*q.y + 2*q.w*q.z);
t.M[0][2] = ( 2*q.x*q.z - 2*q.w*q.y);
t.M[0][3] = 0;
t.M[1][0] = ( 2*q.x*q.y - 2*q.w*q.z);
t.M[1][1] = ( 1 - (2*q.x*q.x + 2*q.z*q.z) );
t.M[1][2] = ( 2*q.y*q.z + 2*q.w*q.x);
t.M[1][3] = 0;
t.M[2][0] = ( 2*q.x*q.z + 2*q.w*q.y);
t.M[2][1] = ( 2*q.y*q.z - 2*q.w*q.x);
t.M[2][2] = ( 1 - (2*q.x*q.x + 2*q.y*q.y) );
t.M[2][3] = 0;
t.M[3][0] = 0;
t.M[3][1] = 0;
t.M[3][2] = 0;
t.M[3][3] = 1;
return t;
}
我刚刚遇到了同样的问题。我在寻找解决方案的轨道上，但我被卡住了。
所以，你需要两个在两个坐标系中都已知的向量。在我的例子中，我在一个设备的坐标系中有两个正交向量(重力和磁场)，我想找到从设备坐标旋转到全局方向的四元数(其中北是正Y，"上"是正Z)。所以，在我的例子中，我已经测量了设备坐标空间中的向量，我定义了向量本身来形成全局系统的正交基。
话虽如此，考虑四元数的轴角解释，存在一些向量V，设备的坐标可以旋转某个角度以匹配全局坐标。我称我的(负)重力矢量为G，磁场为M(两者都归一化)。
V、G和M都描述了单位球上的点。Z_dev和Y_dev(设备坐标系统的Z和Y基)也是如此。目标是找到一个将G映射到Z_dev和M映射到Y_dev的旋转。为了使V将G旋转到Z_dev上，G和V定义的点之间的距离必须与V和Z_dev定义的点之间的距离相同。在方程:
|V - G| = |V - Z_dev|
这个方程的解形成一个平面(所有点与G和Z_dev等距)。但是，V被限制为单位长度，这意味着解是一个以原点为中心的环——仍然有无限个数的点。
但是，同样的情况也适用于Y_dev, M和V:
|V - M| = |V - Y_dev|
这个问题的解决方案也是一个以原点为中心的环。这些环有两个交点，其中一个是另一个的负数。或者是一个有效的旋转轴(在一种情况下，旋转角度将为负)。
使用上面的两个方程，以及这些向量都是单位长度的事实，你应该能够解出v
然后你只需要找到旋转的角度，你应该能够用从V到相应基的向量(对我来说是G和Z_dev)来做。
最后，我在解V的代数题快要结束的时候被弄糊涂了。但不管怎样，我认为你需要的一切都在这里——也许你会比我运气好。
定义3x3矩阵A和B，因此A的列是x_A,x_B和x_C, B的列也是类似的定义。那么从坐标系A到B的变换T是解TA = B，所以T = BA^{-1}。从变换的旋转矩阵T可以使用标准方法计算四元数。
你需要将B相对于A的方向表示为四元数q，那么B中的任何向量都可以转换为A中的向量，例如，通过使用由q导出的旋转矩阵R vectorInA = R*vectorInB。
在这个网站上有一个Matlab/Octave库的演示脚本(包括一个很好的可视化):http://simonbox.info/index.php/blog/86-rocket-news/92-quaternions-to-model-rotations
你可以只使用四元数代数计算你想要的。
给定两个单位向量v1和v2，可以直接将它们嵌入到四元数代数中，得到相应的纯四元数q1和q2。旋转四元数Q使两个向量对齐，使得:
Q Q <子> 1子> * = 2 Q <子>
由:
给出Q= <子> 1 (Q <子> 1 + Q <子> 2子> 1子> | |)
以上乘积为四元数乘积