a0factm.m

基于OpenCV的计算机视觉技术实现.rar

function [m34, c0, a0, r1, r2, r3, t0] = a0FactM(M)
%c0=[u0,v0]; a0=[ax,ay]; t0=[tx, ty, tz];

%Factorization of calibration matrix
%
%by s.y.chen 2001-12-5
%
%INPUT: M 3*4.
%
%OUTPUT: [m34, u0, v0, ax, ay, r1, r2, r3, tx, ty, tz]
%
%Method: Ma SongDe <<Computer Vision>>(chinese), p58

[m, n]=size(M);

if [m, n]~=[3, 4],
  disp('error: size does not match, M must be 3-by-4 matrix');
  return;
end;

m=M(:,1:3);
m1=m(1,:);
m2=m(2,:);
m3=m(3,:);

m34=1.0/sqrt(m3*m3');
r3=m34*m3;
u0=m34*m34*m1*m3';
v0=m34*m34*m2*m3';
m13v=cross(m1,m3);
ax=m34*m34*sqrt(m13v*m13v');
m23v=cross(m2,m3);
ay=m34*m34*sqrt(m23v*m23v');

r1=m34/ax*(m1-u0*m3);
r2=m34/ay*(m2-v0*m3);
tz=m34;
tx=m34/ax*(M(1,4)-u0);
ty=m34/ay*(M(2,4)-v0);
c0=[u0,v0]; a0=[ax,ay]; t0=[tx, ty, tz];