Averaging rotation matrices and calculating the variability
30 次查看(过去 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
0 个评论
回答(2 个)
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
% 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 个评论
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:
- 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 个评论
另请参阅
类别
在 Help Center 和 File Exchange 中查找有关 Coordinate Transformations 的更多信息
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!