Averaging rotation matrices and calculating the variability

41 次查看(过去 30 天)
Hi
I have multiple rotation matrices in the form of cell aray (attached).
I would like to calculate an average or a mean matrix and variability around the mean (standard deviation)
So far I have tried converting the rotation matrices to quaternions and using the meanrot to calculate the quaternion mean. But I do not know how to calculate the variability (STD or variance).
Reading around this, one approach may be to calculate the diffrence between the quaternion mean and each quaternion and porceed (not sure how)
The other way is to use the vector for the mean and the vectors for each rotation matrix; then calculate the diffrence between mean vector and each vector.
I am not sure which is mathematically correct and also how to do that.
Thanks

回答(2 个)

Bruno Luong
Bruno Luong 2023-8-17
编辑:Bruno Luong 2023-8-17
It is not quite straightforward to compute the mean of rotation vector.
The mean should be performed on SO(3) and any attemps to take the mean of rotation matrix is incorrect (the result is no longer an orthonormal matrix). Same with quaternion, you end up with a quaternion mean that is not unitary, so falling outside the 3D rotation represented by quaternion.
It is most convenient working with axis-angle representation
Here is the code to compute the mean and standard deviation
s=load('tfmcell.mat');
Q=cat(3,s.transformcell{:});
n = size(Q,3);
% Compute the Rotation mean
Qmean = eye(3);
for k=1:n
w = (k-1)/k;
Qk = Q(:,:,k);
Qmean = weightedsum(Qmean, Qk, w);
end
Qmean
Qmean = 3×3
0.9990 0.0018 0.0451 0.0025 0.9956 -0.0936 -0.0450 0.0937 0.9946
% Compute the standard deviation
dQ = pagemtimes(Qmean', Q);
[theta, u] = CayleyMethod(dQ);
stdQ = sqrt(sum(theta.^2)/(n-1)); % in radian
stdQdeg = rad2deg(stdQ)
stdQdeg = 3.2785
function Q = weightedsum(Q1, Q2, w1)
R = Q2'*Q1;
[theta, u] = CayleyMethod(R);
v = w1*theta * u;
Rw = rotationVectorToRotationMatrix(v);
Q = Q2 * Rw;
end
function [R] = rotationVectorToRotationMatrix(Rxyz)
% [R] = rotationVectorToRotationMatrix(Rxyz)
% Compute Rotation matrix R from rotation axis Rxyz
% INPUT:
% Rxyz is (3 x n) array each column is the rotation vector
% OUTPUT:
% R: (3 x 3 x n) array, R(:,:,k) is the rotation matrix corresponding to
% Rxyz(:,k)
% https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula
% See also: Rotmat2Rotvec, TCPVector2TransformationMatrix
theta = sqrt(sum(Rxyz.^2,1)); %norm
theta = max(theta,eps()); % prevent zero-division
R = Rxyz./theta; %normalisation
R = reshape(R,3,1, []);
n = size(R,3);
z = zeros(1,1,n);
K=[ z, -R(3,:,:) , R(2,:,:);
R(3,:,:), z, -R(1,:,:);
-R(2,:,:), R(1,:,:), z];
K2 = zeros(3,3,n);
for k=1:n
K2(:,:,k) = K(:,:,k)^2; % matrix multiplication
end
I = repmat(eye(3),1,1,n);
theta = reshape(theta,1,1,n);
R = I + sin(theta).*K + (1-cos(theta)).*K2;
end
%%
function [theta, u] = CayleyMethod(A)
% Ref: "A Survey on the Computation of Quaternions from Rotation Matrices",
% Soheil Sarabandi, Federico Thomas
% J. Mechanisms Robotics. Apr 2019
p = size(A,3);
R = reshape(A,9,p);
C = [R(6,:)-R(8,:);
R(7,:)-R(3,:);
R(2,:)-R(4,:)];
D = [R(6,:)+R(8,:);
R(7,:)+R(3,:);
R(2,:)+R(4,:)];
C2 = C.^2;
D2 = D.^2;
E0 = (R(1,:)+R(5,:)+R(9,:)+1).^2 + C2(1,:) + C2(2,:) + C2(3,:);
Exyz2 = [ ...
C2(1,:) + D2(2,:) + D2(3,:) + (R(1,:)-R(5,:)-R(9,:)+1).^2;
C2(2,:) + D2(3,:) + D2(1,:) + (R(5,:)-R(9,:)-R(1,:)+1).^2;
C2(3,:) + D2(1,:) + D2(2,:) + (R(9,:)-R(1,:)-R(5,:)+1).^2
];
c = sqrt(E0);
s = sqrt(sum(Exyz2,1));
theta = 2*atan2(s, c);
u = sign(C).*sqrt(Exyz2)./s;
isId = s == 0;
nId = sum(isId,2);
u(:,isId) = repmat([1; 0; 0], 1, nId);
theta(isId) = 0;
end
  3 个评论

请先登录,再进行评论。


Saksham
Saksham 2023-8-17
Hi Mel A,
I understand that you have multiple rotation matrices, and you wish to calculate their mean and standard deviation.
The ‘mean’ function can be helpful to take mean of the matrices.
The ‘std’ function is used to calculate standard deviation.
For more information, refer to the following documentations:
I hope the above shared resources will be helpful for the query.
Sincerely,
Saksham

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by