robotics专项课程Perception Assignment 4： Structure from Motion

function F = EstimateFundamentalMatrix(x1, x2)
%% EstimateFundamentalMatrix
% Estimate the fundamental matrix from two image point correspondences 
% Inputs:
%     x1 - size (N x 2) matrix of points in image 1
%     x2 - size (N x 2) matrix of points in image 2, each row corresponding
%       to x1
% Output:
%    F - size (3 x 3) fundamental matrix with rank 2
A = zeros(8,9);
for i = 1:8
    A(i,:) = [x1(i,1)*x2(i,1) x1(i,1)*x2(i,2) x1(i,1) x1(i,2)*x2(i,1) x1(i,2)*x2(i,2) x1(i,2) x2(i,1) x2(i,2) 1];
end
[~,~,V] = svd(A);
x = V(9, :)';
F = reshape(x,3,3);
if rank(F) == 3
    [u,d,v] = svd(F);
    d(3,3) = 0;
    F = u*d*v';    
end
end

function E = EssentialMatrixFromFundamentalMatrix(F,K)
%% EssentialMatrixFromFundamentalMatrix
% Use the camera calibration matrix to esimate the Essential matrix
% Inputs:
%     K - size (3 x 3) camera calibration (intrinsics) matrix
%     F - size (3 x 3) fundamental matrix from EstimateFundamentalMatrix
% Outputs:
%     E - size (3 x 3) Essential matrix with singular values (1,1,0)

E = K'*F*K;
[u,~,v] = svd(E);
d = diag([1 1 0]);
E = u*d*v';
end