matlab - Rodrigues 公式将旋转向量转换为旋转矩阵

Question

我试图了解 3D 旋转向量到旋转矩阵的转换。假设我有一个 3D 旋转矢量 [abg]。从 Trucco 等人的“3D 计算机视觉介绍技术”中，我相信我可以将其表示为每个轴 x、y、z 的旋转矩阵的乘积。

但更多时候我看到使用罗德里格斯公式从旋转向量到矩阵的转换，该公式在下图中给出了 A.17

我在 Matlab 中测试这两个（我在执行 Rodrigues 的 Matlab 图像处理工具箱中使用内置的 rotationVectorToMatrix 函数），我得到的小旋转结果非常接近，例如

alpha = 1 * (pi/180);
beta = 2 * (pi/180);
gamma = 3 * (pi/180); 
R = [(cos(beta) * cos(gamma)) (-cos(beta)*sin(gamma)) sin(beta);
 sin(alpha) * sin(beta) * cos(gamma) + cos(alpha)*sin(gamma) ...
  -sin(alpha) * sin(beta) * sin(gamma) + cos(alpha) * cos(gamma) ...
  -sin(alpha) * cos(beta); ...
  -cos(alpha)*sin(beta)*cos(gamma) + sin(alpha)*sin(gamma) ...
  cos(alpha) * sin(beta) * sin(gamma) + sin(alpha) * cos(gamma) ...
  cos(alpha) * cos(gamma)]

 Rm = rotationVectorToMatrix([alpha beta gamma])'

我明白了

R =

    0.9980   -0.0523    0.0349
    0.0529    0.9984   -0.0174
   -0.0339    0.0193    0.9985
Rm = 
    0.9980   -0.0520    0.0353
    0.0526    0.9985   -0.0165
   -0.0344    0.0184    0.9992

但是随着我的角度变大，它们会发散一点，例如，如果我这样做

alpha = 10 * (pi/180);
beta = 20 * (pi/180);
gamma = 30 * (pi/180);

我明白了

R =

    0.8138   -0.4698    0.3420
    0.5438    0.8232   -0.1632
   -0.2049    0.3188    0.8529


Rm =

    0.8089   -0.4578    0.3689
    0.5166    0.8530   -0.0742
   -0.2807    0.2506    0.9265

再次，我真的只是想在这里获得更好的理解，这些从旋转向量转换为矩阵的方法是否等效？我应该一直使用 Rodriguez 方法吗？如果是，为什么？谢谢你的帮助。

标签： matlabmathcomputer-visionimage-rotation

Answer 1

“旋转矢量”假设角度是同时的。因此，使用欧拉角不是假设顺序角度的正确比较。对于小角度，您会得到一些接近的东西，但对于较大的角度，预计会有显着差异。

适当的比较是四元数，四元数也假定与旋转矢量具有相同意义的同时角度。所以像

V = [alpha beta gamma];
angle = norm(V);
q = [cos(angle/2) sin(angle/2)*V/angle];

然后与此进行比较。例如，

quat2dcm(q)

编辑

如果您没有 MATLAB Aerospace Toolbox，则可以手动进行此转换。Aerospace Toolbox 使用标量向量顺序、右链、右手 Hamilton 约定。所以转换将是：

qw = q(1); qv = q(2:4); % note qv is a row vector here
skew = @(v)[0 -v(3) v(2);v(3) 0 -v(1);-v(2) v(1) 0];
dcm = (qw^2 - qv*qv')*eye(3) + 2*qv'*qv - 2*qw*skew(qv) % right-chain Hamilton

机器人工具箱使用左链约定顺便说一句，因此如果您要与该工具箱中的函数进行比较，则需要翻转叉积偏斜项的符号。例如，

dcm = (qw^2 - qv*qv')*eye(3) + 2*qv'*qv + 2*qw*skew(qv) % left-chain Hamilton

如果您要与左手四元数约定（又名 JPL）进行比较，则翻转符号上方的叉积偏斜项。所以归结为

% right-chain right-handed Hamilton OR left-chain left-handed JPL
dcm = (qw^2 - qv*qv')*eye(3) + 2*qv'*qv - 2*qw*skew(qv)

% left-chain right-handed Hamilton OR right-chain left-handed JPL
dcm = (qw^2 - qv*qv')*eye(3) + 2*qv'*qv + 2*qw*skew(qv)

右链表示未修改的四元数出现在三元数旋转操作的右侧（通常用于两个不同坐标系之间的被动坐标系转换）：

vnew = q^-1 * v * q

左链表示未修改的四元数出现在三元数旋转操作的左侧（通常用于同一坐标系内的主动向量旋转）：

vnew = q * v * q^-1

右手表示四元数虚数单位像常规叉积项一样相乘。例如，

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

左手意味着四元数虚数单位像常规叉积项的负数一样相乘。即，像左手坐标系。例如，

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

而且，当然，如果您使用的是向量标量顺序的四元数，则需要与上面不同地选择标量和向量部分。