Averaging rotation matrices and calculating the variability

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

Answer 1

[Bruno Luong]

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

[Bruno Luong]

In this thread from 2019 the paper R. Hartley, J. Trumpf, Y. Dai, and H. Li, “Rotation Averaging” International Journal of Computer Vision 103, pp. 267–305, DOI 10.1007/s11263-012-0601-0, July 2013 http://users.cecs.anu.edu.au/~hongdong/rotationaveraging.pdf

provides one algorithm to compute the mean. The section 5.3 Geodesic L2 -Mean describes an iterative method, implemented below.

The Hartley method is iterative so more costly but it seems to be more accurate since in this specific example the estimated standard deviation from it is smaller (the mean R supposes to minimize the L2 norm to the population,

May be a good strategy is use mine as starting point then iterate few times to improve the accuracy.

s=load('tfmcell.mat');
Q=cat(3,s.transformcell{:});
n = size(Q,3);
% Compute the Rotation mean, Bruno Luong non iterative method
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 Rotation mean, Hartley iterative method
R = Q(:,:,1);
while true
    dQ = pagemtimes(R', Q);
    [theta, u] = CayleyMethod(dQ);
    r = mean(theta.*u, 2);
    if norm(r) <= eps
        Qmean_Harley = R;
        break
    end
    R = R * rotationVectorToRotationMatrix(r);
end
Qmean_Harley

Qmean_Harley = 3×3

0.9990 0.0018 0.0451 0.0025 0.9956 -0.0936 -0.0450 0.0937 0.9946

% Check if bothe methods give the same result up to round-off
norm(Qmean_Harley-Qmean, 'fro')

ans = 1.2485e-06

stdQ = ComputeStd(Q, Qmean)

stdQ = 0.0572

stdQ_Harley = ComputeStd(Q, Qmean_Harley)

stdQ_Harley = 0.0572

stdQ_Harley - stdQ 

ans = -7.0444e-12

function stdQ = ComputeStd(Q, Qmean)
% Compute the standard deviation
dQ = pagemtimes(Qmean', Q);
[theta, u] = CayleyMethod(dQ);
n = size(Q,3);
stdQ = sqrt(sum(theta.^2)/(n-1)); % in radian
end

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
% NOTE: R is exp[Rxyz]x according to Hartley and al
% 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
% NOTE: thet*u is log[A] according to Hartley and al
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

[Bruno Luong]

I post the function in file exchange here

[Pengfree_Yao]

Great job! I implemented rotation averaging using the above code.

Answer 2

[Saksham]

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