How to find Rotation matrix mean?
26 views (last 30 days)
Show older comments
Hi,
I am trying to find out the mean matrix of a collection of rotation matrices.
I found out this Algorithm:
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?
0 Comments
Answers (3)
CLEDAT Emmanuel
on 2 Apr 2019
Edited: CLEDAT Emmanuel
on 11 Jun 2019
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
0 Comments
the cyclist
on 6 Sep 2015
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.
0 Comments
Pariterre
on 25 Jul 2019
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
0 Comments
See Also
Categories
Find more on Quadratic Programming and Cone Programming in Help Center and File Exchange
Products
Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!