Home > FR3DSource > zBestRotation.m

zBestRotation

PURPOSE ^

zBestRotation(X,Y) finds the least squares rotation

SYNOPSIS ^

function [R] = zBestRotation(A,B)

DESCRIPTION ^

 zBestRotation(X,Y) finds the least squares rotation
 of points X onto points Y

 X and Y are n by 3 matrices containing the locations of corresponding points
 What is returned is the best fit to Y = X*R'
 R is the 3x3 rotation matrix

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SOURCE CODE ^

0001 % zBestRotation(X,Y) finds the least squares rotation
0002 % of points X onto points Y
0003 %
0004 % X and Y are n by 3 matrices containing the locations of corresponding points
0005 % What is returned is the best fit to Y = X*R'
0006 % R is the 3x3 rotation matrix
0007 
0008 % It is assumed that the means have been subtracted from X and Y
0009 
0010 function [R] = zBestRotation(A,B)
0011 
0012 n = length(A(:,1));                 % number of data points
0013 
0014 M = zeros(3,3);                     % initialize the matrix M
0015 
0016 for i=1:n,                          % go through all data points
0017   M = M + A(i,:)' * B(i,:);         % "outer product" of two vectors -> matrix
0018 end
0019 
0020 [v,d] = eig(M'*M);                  % eigenvector matrix, eigenvalue matrix
0021 [e,i] = sort(-diag(d));             % sort the eigenvalues in decreasing order
0022 
0023 a = v(:,i(1));                      % eigenvector with largest eigenvalue
0024 b = v(:,i(2));
0025 c = v(:,i(3));
0026 
0027 Ma = M*a/sqrt(-e(1));
0028 Mb = M*b/sqrt(-e(2));
0029 
0030 if det([a b c]) > 0,
0031   g = cross(Ma,Mb);
0032 else 
0033   g = -cross(Ma,Mb);
0034 end
0035 
0036 R = [a b c]*[Ma Mb g]';
0037

Generated on Fri 03-Apr-2009 09:52:35 by m2html © 2003