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
