Averaging rotation matrices and calculating the variability
32 views (last 30 days)
Show older comments
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
0 Comments
Answers (2)
Bruno Luong
on 17 Aug 2023
Edited: Bruno Luong
on 17 Aug 2023
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
% Compute the standard deviation
dQ = pagemtimes(Qmean', Q);
[theta, u] = CayleyMethod(dQ);
stdQ = sqrt(sum(theta.^2)/(n-1)); % in radian
stdQdeg = rad2deg(stdQ)
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 Comments
Saksham
on 17 Aug 2023
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:
- mean - https://in.mathworks.com/help/matlab/ref/mean.html
- std - https://in.mathworks.com/help/matlab/ref/std.html
I hope the above shared resources will be helpful for the query.
Sincerely,
Saksham
0 Comments
See Also
Categories
Find more on Coordinate Transformations in Help Center and File Exchange
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!