谢邀请。
目的:寻找两组对应3D点之间的Rotation(R)和Translation(T)。
我用的是Absolute Orientation Quaternion
Horn 的 《Closed-form solution of absolute orientation using unit quaternions》
方法比较老, 如果有其他推荐请忽略。。。。
Paper大致逻辑是:
1. 两组中各取3个点, 建立两组新坐标系 (3个点: 一个原点。 另两个和原点可以定义一个面。 两个中的一个和原点构成一个轴。 垂直于面并过原点的方向为另一个轴。 垂直于两个轴为第三个轴。三个相互垂直的轴就是一个新坐标系啦~)
2. 原目的是寻找两组3D点之间的Rotation(R)和Translation(T). 现在则是寻找新定义的两个坐标系间的R,T。 因为我们有第四个点, 第四个点可以通过三个坐标系表示:原有的Global Coordinate, 以及1中新定义的两个坐标系。
于是Paper开始推公式找关系。 。。。详情请见paper:(
Index of /bkph/papers)
注意: 虽然四个点就能解出来R,T. 但这是在对应点无误差的假设下。 这个方法是至少提供4组对应点,实际使用中通常要提供远多于四个点来降低误差对结果的影响。
ETH Zurich 写了个matlab code
使用时请注意Copyright:
========================
% 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
%
% Notes: Minimum 3D point number is N > 4
========================
如下:
function [s R T err] = absoluteOrientationQuaternion( A, B, doScale)
%Argument check
if(nargin<3)
doScale=1;
end
%Return argument check
if(nargout<1)
usage()
error('Specify at least 1 return arguments.');
end
%Test size of point sets
[c1 r1] = size(A);
[c2 r2] = size(B);
if(r1~=r2)
usage()
error('Point sets need to have same size.');
end
if(c1~=3 | c2~=3)
usage()
error('Need points of dimension 3');
end
if(r1<4)
usage()
error('Need at least 4 point pairs');
end
%Number of points
Na = r1;
%Compute the centroid of each point set
Ca = mean(A,2);
Cb = mean(B,2);
%Remove the centroid
An = A - repmat(Ca,1,Na);
Bn = B - repmat(Cb,1,Na);
%Compute the quaternions
M = zeros(4,4);
for i=1:Na
%Shortcuts
a = [0;An(:,i)];
b = [0;Bn(:,i)];
%Crossproducts
Ma = [ a(1) -a(2) -a(3) -a(4) ;
a(2) a(1) a(4) -a(3) ;
a(3) -a(4) a(1) a(2) ;
a(4) a(3) -a(2) a(1) ];
Mb = [ b(1) -b(2) -b(3) -b(4) ;
b(2) b(1) -b(4) b(3) ;
b(3) b(4) b(1) -b(2) ;
b(4) -b(3) b(2) b(1) ];
%Add up
M = M + Ma'*Mb;
end
%Compute eigenvalues
[E D] = eig(M);
%Compute the rotation matrix
e = E(:,4);
M1 = [ e(1) -e(2) -e(3) -e(4) ;
e(2) e(1) e(4) -e(3) ;
e(3) -e(4) e(1) e(2) ;
e(4) e(3) -e(2) e(1) ];
M2 = [ e(1) -e(2) -e(3) -e(4) ;
e(2) e(1) -e(4) e(3) ;
e(3) e(4) e(1) -e(2) ;
e(4) -e(3) e(2) e(1) ];
R = M1'*M2;
%Retrieve the 3x3 rotation matrix
R = R(2:4,2:4);
%Compute the scale factor if necessary
if(doScale)
a =0; b=0;
for i=1:Na
a = a + Bn(:,i)'*R*An(:,i);
b = b + Bn(:,i)'*Bn(:,i);
end
s = b/a;
else
s = 1;
end
%Compute the final translation
T = Cb - s*R*Ca;
%Compute the residual error
if(nargout>3)
err =0;
for i=1:Na
d = (B(:,i) - (s*R*A(:,i) + T));
err = err + norm(d);
end
err = (err)/Na;
end
%Displayed if an error occurs
function usage()
disp('Usage:')
disp('[s R T error] = absoluteOrientationQuaternion( A, B, doScale)')
disp(' ')
disp('Return values:')
disp('s The scale factor')
disp('R The 3x3 rotation matrix')
disp('T The 3x1 translation vector')
disp('err Residual error (optional)')
disp(' ')
disp('Input arguments:')
disp('A 3xN matrix representing the N 3D points')
disp('B 3xN matrix representing the N 3D points')
disp('doScale Optional flag indicating whether to estimate the uniform scale factor as well [default=0]')
disp(' ')