Absolute Orientation

[s R T error] = absoluteOrientationQuaternion( A, B, doScale)

Computes the orientation and position (and optionally the uniform scale factor) for the transformation between two corresponding 3D point sets Ai and Bi such as they are related by:

Bi = sR*Ai+T

Implementation is based on the paper by Berthold K.P. Horn:
"Closed-from solution of absolute orientation using unit quaternions"
The paper can be downloaded here:
http://people.csail.mit.edu/bkph/papers/Absolute_Orientation.pdf

Authors:
Dr. Christian Wengert, Dr. Gerald Bianchi

Copyright:
ETH Zurich, Computer Vision Laboratory, Switzerland

Parameters:
A 3xN matrix representing the N 3D points
B 3xN matrix representing the N 3D points
doScale Flag indicating whether to estimate the uniform scale factor as well [default=0]

Return:
s The scale factor
R The 3x3 rotation matrix
T The 3x1 translation vector
err Residual error (optional)

Notes: Minimum 3D point number is N > 4

The residual error is being computed as the sum of the residuals:

for i=1:Npts
d = (B(:,i) - (s*R*A(:,i) + T));
err = err + norm(d);
end

Example:

s=0.7;
R = [0.36 0.48 -0.8 ; -0.8 0.6 0 ; 0.48 0.64 0.6];
T= [45 -78 98]';
X = [ 0.272132 0.538001 0.755920 0.582317;
0.728957 0.089360 0.507490 0.100513;
0.578818 0.779569 0.136677 0.785203];
Y = s*R*X+repmat(T,1,4);

%Compute
[s2 R2 T2 error] = absoluteOrientationQuaternion( X, Y, 1);

error = 0;

%Add noise
Noise = [
-0.23 -0.01 0.03 -0.06;
0.07 -0.09 -0.037 -0.08;
0.009 0.09 -0.056 0.012];

Y = Y+Noise;
%Compute
[s2 R2 T2 error] = absoluteOrientationQuaternion( X, Y, 1);

error = 0.33