essentialdiscrete.m

包括计算机科学和工程、信号处理、物理学、应用数学和统计学

%-------------------------------------------------------------
%  Jana Kosecka,  UC Berkeley,  June 1998 
%  Motion estimation from point correspondences  
%  as described in:
%  "Motion Recovery from Image Sequences: 
%   Discrete Viewpoint vs. Differential Viewpoint."
%   by Yi Ma, Jana Kosecka, Shankar S. Sastry, ECCV'98.
%  
%  SVD to get essential matrix and then pullout the R, p up to scale 
%  function [est_trans, est_rot]  = essential_discrete(p,q)
%  p are coordinates of the first image 
%  q are coordinates of the second image
%  constraint of the form q'*That*R*p = 0 
%  where lambda2*q = R*(lambda1 *p) + T 
%  April 2002  - positive depth constraint was
%  modified to reflect the g action as in the book (chapter 5)

% Algorith, 5.1 in Chapter 5, "An introduction to 3-D Vision"
% by Y. Ma, S. Soatto, J. Kosecka, S. Sastry (MASKS)
%
% Code distributed free for non-commercial use
% Copyright (c) MASKS, 2003

% ---------------------------------------------------------------------

function [T0, R0]  = essentialDiscrete(p,q)

n = size(p);
NPOINTS = n(2);

% set up matrix A such that A*[v1,v2,v3,s1,s2,s3,s4,s5,s6]' = 0
A = zeros(NPOINTS, 9);

if NPOINTS < 9
     error('Too few mesurements')
     return;
end

for i = 1:NPOINTS
  A(i,:) = kron(p(:,i),q(:,i))';
  end
r = rank(A);

if r < 8 
  warning('Measurement matrix rank defficient')
  T0 = 0; R = [];
end;

[U,S,V] = svd(A);

% pick the eigenvector corresponding to the smallest eigenvalue
e = V(:,9);
e = (round(1.0e+10*e))*(1.0e-10);
% essential matrix 
E = reshape(e, 3, 3);  

% then four possibilities are 
Rzp = [0 -1 0 ; 1 0 0 ; 0 0 1 ]; % rotation by pi/2
Rzn = [0 1 0 ; -1 0 0 ; 0 0 1 ]; % rotation by -pi/2

[U,S,V] = svd(E);
S = diag([1,1,0]);
detu = det(U);
detv = det(V);
if detu < 0 & detv < 0
   U = -U; V = -V;
   % break;
elseif detu < 0 & detv > 0
   S1 = Rzp*S;
   U  = -U*rot_matrix([S1(3,2), S1(1,3) S1(2,1)],pi)*Rzp;
   V  = V*Rzp;
   % break;
elseif detu > 0 & detv < 0
   S1 = Rzp*S;
   U = U*rot_matrix([S1(3,2), S1(1,3) S1(2,1)],pi)*Rzp;
   V = -V*Rzp;
   % break;
end

R(:,:,1) = (U*Rzp'*V');
Th(:,:,1) = (U*Rzp*S*U');
t(:,1)  = [-Th(2,3,1), Th(1,3,1), -Th(1,2,1)]';
[omega(:,1),theta(1)] = exp_matrix(R(:,:,1));
R(:,:,2) = (U*Rzn'*V');
Th(:,:,2)  = (U*Rzn*S*U');
t(:,2)  = [-Th(2,3,2), Th(1,3,2), -Th(1,2,2)]';
[omega(:,2),theta(2)] = exp_matrix(R(:,:,2));        

[U,S,V] = svd(-E);
S = diag([1,1,0]);
detu = det(U);
detv = det(V);

if detu < 0 & detv < 0
   U = -U; V = -V;
   % break
elseif detu < 0 & detv > 0
   S1 = Rzp*S;
   U  = -U*rot_matrix([S1(3,2), S1(1,3) S1(2,1)],pi)*Rzp;
   V  = V*Rzp;
   % break   
elseif detu > 0 & detv < 0
   S1 = Rzp*S;
   U = U*rot_matrix([S1(3,2), S1(1,3) S1(2,1)],pi)*Rzp;
   V = -V*Rzp;
   % break
end

R(:,:,3) = (U*Rzp'*V');
Th(:,:,3) = U*Rzp*S*U';
t(:,3) = [-Th(2,3,3), Th(1,3,3), -Th(1,2,3)]';
[omega(:,3),theta(3)] = exp_matrix(R(:,:,3));       
R(:,:,4) = (U*Rzn'*V');
Th(:,:,4)   = U*Rzn*S*U';
t(:,4)  = [-Th(2,3,4), Th(1,3,4), -Th(1,2,4)]';
[omega(:,4),theta(4)] = exp_matrix(R(:,:,4));       

index = 0;
posdepth = zeros(1,4);
   
% =============================================================
% pick the correct solution based on positive depth constraint
% there are two ways (below 2. is used):
% 1. Compute both scales and pick the solution where the majority is 
%    positive in both frames
% 2. Compute volume, which has to be positive if the two scales have 
%     the same sign and then check whether one of the scale is positive
%     (similar solution suggested by Kanatani, 1993 book).
% =============================================================
for i = 1:4
  for j = 1:NPOINTS
    % c (a x b) (That*q)'*That*R*p > 0 
    % if the depths have the same sign the triple product has to 
    % be greater then 0 
    volume(j) =  triple_product(t(:,i), R(:,:,i)*p(:,j), Th(:,:,i)*q(:,j));
    alpha1(j) = -(skew(q(:,j))*t(:,i))'*...
	(skew(q(:,j))*R(:,:,i)*p(:,j)) ...
	/(norm(skew(q(:,j))*t(:,i)))^2;
    alpha2(j) = (skew(R(:,:,i)*p(:,j))*q(:,j))'*...
	(skew(R(:,:,i)*p(:,j))*t(:,i)) ...
	/norm(skew(R(:,:,i)*p(:,j))*q(:,j))^2;
  end
  vol = sum(sign(volume));
  depth = sum(sign(alpha1));
  depth2 = sum(sign(alpha2));
  % pause
  posdepth(i) = vol +  depth;
end     % end for all motions

 [val, index] = max(posdepth);
 index_final = index;
 T0 = t(:,index);
 R0 = R(:,:,index);