Focal length calibration from two views: method and analysis of singular cases

 

下面是程序的代码，仿真数据读者可以自己生成，注意第一个摄像机要与世界坐标重合，拍摄两幅图像时除焦距外其他内参数已知，且焦距固定。

main.m文件

clc
clear

%标定摄像机的焦距大小
%摄像机模型为：
%K=[
%       t*f,0,u0;
%       0,f,v0;
%       0,0,1
%       ];
%参考文献：
%[1]    P.Sturm,Z.L.Cheng,P.C.Y.Chen,A.N.Poo,《Focal length calibration
%         from two views: method and analysis of singular cases》
%[2]    P.Sturm,《On Focal Length Calibration from Two Views》
%程序设计：Wenbin WANG
%设计时间：2009/05/20

%Read Image Coordinate
NumImg = 2;% to two-view
for i = 1 : NumImg
    Img(:,:,i) = textread(strcat('Img', num2str(i), '.txt'));
end

%Calculate Fundamental Matrix-----i-th and 1-th,(i>=2)
F = fundmatrix([Img(:,:,1)'; Img(:,:,2)']);

%Read Camera Parameters including u0,v0,t
%程序假设两幅图像的焦距是固定的
u0 = 500;%第一个相机
v0 = 400;
t0 = 1.2;

u1 = 600;%第二个相机
v1 = 500;
t1 = 1;

Ac0 = [t0,0,u0;0,1,v0;0,0,1];
Ac1 = [t1,0,0;0,1,0;u1,v1,1];

G = Ac1*F*Ac0;
[U S V] = svd(G);

a = S(1,1);
b = S(2,2);
U31 = U(3,1);
U32 = U(3,2);
V31 = V(3,1);
V32 = V(3,2);

%构造[2]中的方程（4）
Coff1 = [
    a^2*(1-U31^2)*(1-V31^2)-b^2*(1-U32^2)*(1-V32^2),...
    a^2*(U31^2+V31^2-2*U31^2*V31^2)-b^2*(U32^2+V32^2-2*U32^2*V32^2),...
    a^2*U31^2*V31^2-b^2*U32^2*V32^2
    ];
X = roots(Coff1);

%用方程（2）（3）排除误解，得到正确的解
Coff2 = a*U31*U32*(1-V31^2)+b*V31*V32*(1-U32^2)+...
    a*V31*V32*(1-U31^2)+b*U31*U32*(1-V32^2);
Coff3 = U32*V31*(a*U31*V31+b*U32*V32)+V32*U31*(a*U31*V31+b*U32*V32);
min1 = X(1)*Coff2 + Coff3;
min2 = X(2)*Coff2 + Coff3;

if abs(min1) < abs(min2)
    f = sqrt(X(1));
else
    f = sqrt(X(2));
end

disp('恢复出的焦距为：')
f

fundmatrix.m文件

% FUNDMATRIX - computes fundamental matrix from 8 or more points
%
% Function computes the fundamental matrix from 8 or more matching points in
% a stereo pair of images. The normalised 8 point algorithm given by
% Hartley and Zisserman p265 is used. To achieve accurate results it is
% recommended that 12 or more points are used
%
% Usage:   [F, e1, e2] = fundmatrix(x1, x2)
%          [F, e1, e2] = fundmatrix(x)
%
% Arguments:
%          x1, x2 - Two sets of corresponding 3xN set of homogeneous
%          points.
%        
%          x      - If a single argument is supplied it is assumed that it
%                   is in the form x = [x1; x2]
% Returns:
%          F      - The 3x3 fundamental matrix such that x2'*F*x1 = 0.
%          e1     - The epipole in image 1 such that F*e1 = 0
%          e2     - The epipole in image 2 such that F'*e2 = 0
%

% Copyright (c) 2002-2005 Peter Kovesi
% School of Computer Science & Software Engineering
% The University of Western Australia
% http://www.csse.uwa.edu.au/
%
% Permission is hereby granted, free of charge, to any person obtaining a copy
% of this software and associated documentation files (the "Software"), to deal
% in the Software without restriction, subject to the following conditions:
%
% The above copyright notice and this permission notice shall be included in
% all copies or substantial portions of the Software.
%
% The Software is provided "as is", without warranty of any kind.

% Feb 2002 - Original version.
% May 2003 - Tidied up and numerically improved.
% Feb 2004 - Single argument allowed to enable use with RANSAC.
% Mar 2005 - Epipole calculation added, 'economy' SVD used.
% Aug 2005 - Octave compatibility

function F = fundmatrix(varargin)
   
    [x1, x2, npts] = checkargs(varargin(:));
    v = version; Octave = v(1)<'5'; % Crude Octave test
   
    % Normalise each set of points so that the origin
    % is at centroid and mean distance from origin is sqrt(2).
    % normalise2dpts also ensures the scale parameter is 1.
    [x1, T1] = normalise2dpts(x1);
    [x2, T2] = normalise2dpts(x2);
   
    % Build the constraint matrix
    A = [x2(1,:)'.*x1(1,:)'   x2(1,:)'.*x1(2,:)' x2(1,:)' ...
         x2(2,:)'.*x1(1,:)'   x2(2,:)'.*x1(2,:)' x2(2,:)' ...
         x1(1,:)'             x1(2,:)'            ones(npts,1) ];      

    if Octave
[U,D,V] = svd(A);   % Don't seem to be able to use the economy
                            % decomposition under Octave here
    else
[U,D,V] = svd(A,0); % Under MATLAB use the economy decomposition
    end

    % Extract fundamental matrix from the column of V corresponding to
    % smallest singular value.
    F = reshape(V(:,9),3,3)';
   
    % Enforce constraint that fundamental matrix has rank 2 by performing
    % a svd and then reconstructing with the two largest singular values.
    [U,D,V] = svd(F,0);
    F = U*diag([D(1,1) D(2,2) 0])*V';
   
    % Denormalise
    F = T2'*F*T1;
   
%--------------------------------------------------------------------------
% Function to check argument values and set defaults

function [x1, x2, npts] = checkargs(arg);
   
    if length(arg) == 2
        x1 = arg{1};
        x2 = arg{2};
        if ~all(size(x1)==size(x2))
            error('x1 and x2 must have the same size');
        elseif size(x1,1) ~= 3
            error('x1 and x2 must be 3xN');
        end
       
    elseif length(arg) == 1
        if size(arg{1},1) ~= 6
            error('Single argument x must be 6xN');
        else
            x1 = arg{1}(1:3,:);
            x2 = arg{1}(4:6,:);
        end
    else
        error('Wrong number of arguments supplied');
    end
     
    npts = size(x1,2);
    if npts < 8
        error('At least 8 points are needed to compute the fundamental matrix');
    end
   

normalise2dpts.m文件

% NORMALISE2DPTS - normalises 2D homogeneous points
%
% Function translates and normalises a set of 2D homogeneous points
% so that their centroid is at the origin and their mean distance from
% the origin is sqrt(2). This process typically improves the
% conditioning of any equations used to solve homographies, fundamental
% matrices etc.
%
% Usage:   [newpts, T] = normalise2dpts(pts)
%
% Argument:
%   pts - 3xN array of 2D homogeneous coordinates
%
% Returns:
%   newpts - 3xN array of transformed 2D homogeneous coordinates. The
%             scaling parameter is normalised to 1 unless the point is at
%             infinity.
%   T      - The 3x3 transformation matrix, newpts = T*pts
%          
% If there are some points at infinity the normalisation transform
% is calculated using just the finite points. Being a scaling and
% translating transform this will not affect the points at infinity.

% Peter Kovesi
% School of Computer Science & Software Engineering
% The University of Western Australia
% pk at csse uwa edu au
% http://www.csse.uwa.edu.au/~pk
%
% May 2003      - Original version
% February 2004 - Modified to deal with points at infinity.

function [newpts, T] = normalise2dpts(pts)

    if size(pts,1) ~= 3
        error('pts must be 3xN');
    end
   
    % Find the indices of the points that are not at infinity
    finiteind = find(abs(pts(3,:)) > eps);
   
    if length(finiteind) ~= size(pts,2)
        warning('Some points are at infinity');
    end
   
    % For the finite points ensure homogeneous coords have scale of 1
    pts(1,finiteind) = pts(1,finiteind)./pts(3,finiteind);
    pts(2,finiteind) = pts(2,finiteind)./pts(3,finiteind);
    pts(3,finiteind) = 1;
   
    c = mean(pts(1:2,finiteind)')';            % Centroid of finite points
    newp(1,finiteind) = pts(1,finiteind)-c(1); % Shift origin to centroid.
    newp(2,finiteind) = pts(2,finiteind)-c(2);
   
    meandist = mean(sqrt(newp(1,finiteind).^2 + newp(2,finiteind).^2));
   
    scale = sqrt(2)/meandist;
   
    T = [scale   0   -scale*c(1)
         0     scale -scale*c(2)
         0       0      1      ];
   
    newpts = T*pts;