How to find Rotation matrix mean?

Hi,

I am trying to find out the mean matrix of a collection of rotation matrices.

I found out this Algorithm:

http://users.cecs.anu.edu.au/~hongdong/rotationaveraging.pdf (on page 18)

But I am having a problem with the log10(transpose( R)*Ri) part, since this gives me a matrix and not a single number.

this is my code:

err=1e-5.*ones(3,3);
R=Rotations{1};

Temp=0;
    for i=1:length(Rotations)
        Temp=log10(transpose(R).*Rotations{i})+Temp;
    end
    r=(1/length(Rotations)).*Temp;

while (abs(r))>err

      if (abs(r))<err
          R=abs(R);
          return;
      else R=R.*exp(r);
      end

      Temp=0;
      for i=1:length(Rotations)
          Temp=log10(transpose(R).*Rotations{i})+Temp;
      end
      r=(1/length(Rotations)).*Temp;

end

R=abs(R);

and this is matrix Rotations:

Rotations{1}=[0.999885000000000  -0.00115950000000000  -0.0151475000000000;
0.000987358000000000  0.999935000000000  -0.0113668000000000;
0.0151597000000000  0.0113506000000000  0.999821000000000]

Rotations{2}=[0.999908000000000  -0.00154131000000000  -0.0134458000000000;
0.00140370000000000  0.999947000000000  -0.0102382000000000;
0.0134608000000000  0.0102184000000000  0.999857000000000]

What am I doing wrong?

Answer 1

[CLEDAT Emmanuel]

Hello,

The following code is an implementation (and some explanations) of the matrix rotation mean as described in the publication:

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

The first function to be defined is the Skew-Symetrixc matrix (as in https://en.wikipedia.org/wiki/Skew-symmetric_matrix)

function cross_ten_x = cross_ten( x )
% Skew symetric matrix permiting to express the cross product: x * .


    X = x(1);
    Y = x(2);
    Z = x(3);
    cross_ten_x = [  0 , -Z ,  Y ;...
                     Z ,  0 , -X ;...
                    -Y ,  X ,  0 ];

end

In math, we use the notation: This function have a nice property:

for any vector v, is the rotation matrix around the axis defined by v, and of angle .

Then, we could define the inverse transformation

function v = lie_log( R )
% Lie-Logarithm
% Function such that: 
%     expm(cross_ten(       lie_log(R)       )) = R

    [V,D] = eig(R);

    [theta,i] = max(angle(diag(D)));
    [   ~ ,k] = min(abs(diag(D)-1));

    v1 = theta*V(:,k);
    v2 =-theta*V(:,k);

    R1 = expm(cross_ten( v1 ));
    R2 = expm(cross_ten( v2 ));

    if max(angle(eig(R * R2'))) > max(angle(eig(R * R1')))
        v = v1;
    else
        v = v2;
    end

end

Finally, you could process the rotation matrix mean.

Let Rotations, be the cell array contenning the rotations, then, the followinc code is the implementation of the reference you cited .

%% Example of Input
Rotations = { [     0.5415   -0.6881    0.4831;... R1
                    0.8392    0.4769   -0.2613;...
                   -0.0506    0.5469    0.8357];...
              [     0.5422   -0.6857    0.4857;... R2
                    0.8386    0.4775   -0.2620;...
                   -0.0522    0.5494    0.8339];...
              [     0.5430   -0.6857    0.4848;... R3
                    0.8381    0.4784   -0.2620;...
                   -0.0523    0.5486    0.8344];...
              [     0.5400   -0.6889    0.4835;... R4
                    0.8401    0.4764   -0.2593;...
                   -0.0517    0.5462    0.8360];...
              [     0.5429   -0.6861    0.4843;... R5
                    0.8381    0.4794   -0.2603;...
                   -0.0535    0.5472    0.8353] }
               
%% Algorithm for Matrix averaging
                   
nb_it_max = 20;
tol_r     = 1e-10;   % [1] 
number_rotations = length(Rotations);

R = Rotations{1} % First approx of R [1]
for nb_it = 1:nb_it_max % [2]

    list_r = nan(3,number_rotations);  % [3]
    for i = 1:number_rotations
        list_r(:,i) = lie_log( R' * Rotations{i} );
    end
    r = mean(list_r,2);
    
    if norm(r) < tol_r % [4]
        break
    end

    R = R * expm(cross_ten( r )); % Update [7]
    
end % [8]
if nb_it == nb_it_max
    error('the maximum number of iteration where reached')
end
R

The [*] indicate the number of the line, refering to page 18 of http://users.cecs.anu.edu.au/~hongdong/rotationaveraging.pdf

Answer 2

[the cyclist]

I searched "log" in the paper, and found this sentence:

"We resolve this by defining log R to be the angle–axis vector of length no more than π, which is uniquely defined unless R is a rotation through π radians, in which case we let log R be one of the two possible vectors of length π representing this rotation"

This suggests to me that the MATLAB log function is not what you need to apply.

Answer 3

[Pariterre]

Hello!

I know this topic is quite old! But I add some informations to it anyway :)

I have implemented, I'd say more naively, a similar algorithm, but based on an optimization. In short, the distance to the mean of each element of the matrix is minimized with the constraint of having a norm 1 matrix.

The results are pretty similar (the more the values to average are near, the closer the 2 methods get). However, I am not sure why they are not identical. I did not investigated this. The major benefit of using this algorithm is it is much faster. For the following code (averageRT being the optimization and averageRT2 being the Lie geometry), the optimization method runs 2 order of magnitude faster.

R = fromAngleToMatrix(rand(3, 10000), 'xyz');

tic 
R = averageRT(R)
toc


tic
R2 = averageRT2(R)
toc

R1 =

    0.7687   -0.4195    0.4829
    0.6236    0.6596   -0.4196
   -0.1425    0.6237    0.7686

Elapsed time is 0.015201 seconds.

R2 =

    0.7689   -0.4195    0.4825
    0.6234    0.6597   -0.4198
   -0.1422    0.6235    0.7688

Elapsed time is 17.498694 seconds.

To test my algorithm you can download it from here : https://www.mathworks.com/matlabcentral/fileexchange/72272-averagert Please note that fromAngleToMatrix (https://www.mathworks.com/matlabcentral/fileexchange/72269-fromangletomatrix) and fromMatrixToAngle (https://www.mathworks.com/matlabcentral/fileexchange/72271-frommatrixtoangle) are needed