The camera calibration "Dual Absolute Quadric" cost function isn't converging

Question

• I tried to implement the cost function of the Dual Absolute Quadric in Matlab according to the following equation mentioned in this paper, with this data.

• My problem is that the results didn't converge. The code is down.

main code

%---------------------
% clear and close all
%---------------------
clearvars
close all
clc
%---------------------
% Data type long
%---------------------
format long g
%---------------------
% Read data
%---------------------
load('data.mat')
%---------------------------
% Display The Initial Guess
%---------------------------
disp('=======================================================')
disp('Initial Intrinsic parameters: ');
disp(A);
disp('xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx')

%=========================================================================
DualAbsoluteQuadric = Optimize(A,@DAQ);
%---------------------
% Display The Results
%---------------------
disp('=======================================================')
disp('Dual Absoute Quadric cost function: ');
disp(DualAbsoluteQuadric);
disp('xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx')

The optimization function used is:

function output = Optimize(A,func)
%------------------------------
options = optimoptions('lsqnonlin','Algorithm','levenberg-marquardt',...
    'Display','iter','FunctionTolerance',1e-16,'Tolx',1e-16,...
    'MaxFunctionEvaluations', 1000, 'MaxIterations',39,...
    'OptimalityTolerance',1e-16);
%------------------------------
% NonLinear Optimization
%------------------------------
output_line = lsqnonlin(func,[A(1,1), A(1,3), A(2,2), A(2,3), A(1,2)],...
    [],[],options);
%------------------------------------------------------------------------
output = Reshape(output_line);

The Dual Absolute Quadric Function:

function cost = DAQ(params)

Aj = [params(1)   params(5) params(2) ;

    0        params(3) params(4) ;
    0           0         1     ];

Ai = [params(1) params(5)   params(2) ;
    0      params(3)   params(4) ;
    0           0         1     ];
% W^-1 (IAC Image of the Absolute Conic)
W_inv = Ai * Aj';
%----------------
%Find plane at infinity from MQM' ~ w (Dual Absolute Quadric)
Plane_at_infinity = PlaneAtInfinity(W_inv);

%Find H_Infty = [e21]F+e21*n'
Homography_at_infty = H_Infty(Plane_at_infinity);
%----------------
% Initialization
%----------------
global Fs;
% Initialize the cost as a vector
% (N-1 * N-2)/2: 9*8/2 = 36
vector_size = (size(Fs,3)-1)*(size(Fs,4)-2)/2;
cost = zeros(1, vector_size);
% Cost Function
k = 0;
loop_size = 3 * vector_size;
Second_Term = W_inv / norm(W_inv,'fro');
for i=1:3:loop_size
    k = k+1;
    First_Term  = Homography_at_infty(:,i:i+2) * W_inv * ((Homography_at_infty(:,i:i+2))');
    First_Term  = First_Term / norm(First_Term, 'fro');
    cost(k) = norm(First_Term - Second_Term,'fro');
end
end

Plane at infinity function:

function P_infty = PlaneAtInfinity(W_inv)

global PPM;

% Symbolic variables

X = sym('X', 'real');
Y = sym('Y', 'real');
Z = sym('Z', 'real');
L2 = sym('L2','real');

n = [X; Y; Z];

% DAQ
Q = [W_inv       ,   (W_inv * n)    ;
    (n' * W_inv) , (n' * W_inv * n)];

% Get one only camera matrix (any)
M = PPM(:, :, 3);

% Autocalibration equation
m = M * Q * M';

% solve linear equations
solution = solve(m(1, 1) == (L2 * W_inv(1, 1)), ...
    m(2, 2) == (L2 * W_inv(2, 2)), ...
    m(3, 3) == (L2 * W_inv(3, 3)), ...
    m(1, 3) == (L2 * W_inv(1, 3)));

P_infty = [double(solution.X(1)) double(solution.Y(1))...
    double(solution.Z(1))]';

Homography at infinity function:

function H_Inf = H_Infty(planeInf)

global Fs;
k = 1;
% (3 x 3) x ((N-1)*(N-2) /2)
H_Inf = zeros(3,3*(size(Fs,3)-1)*(size(Fs,4)-2)/2);%(3*3)*36

for i = 2:size(Fs,3)
    for j = i+1:size(Fs,4)

        [~, ~, V]  = svd(Fs(:,:,i,j)');
        epip = V(:,end);

        H_Inf(:,k:k+2) =  epipole(Fs(:,:,i,j)) * Fs(:,:,i,j)+ epip * planeInf';
        k = k+3;        
    end
end       
end

Reshape function:

function output = Reshape(input)
%---------------------
%  Reshape Intrinsics
%---------------------
% K = [a         skew       u0     ;
%      0           B        v0     ;
%      0           0        1     ];

output = [input(1) input(5) input(2) ;
    0       input(3)   input(4)   ;
    0           0         1    ];

end

Epipole Function:

function epip = epipole(Fs)
% SVD Decompostition of (Fs)^T
[~,~,V]  = svd(Fs');
% Get the epipole from the last vector of the SVD
epi = V(:,end);
% Reshape the Vector into Matrix
epip = [ 0     -epi(3)  epi(2);
    epi(3)   0     -epi(1);
    -epi(2)  epi(1)    0  ];

end

Answer 1

The plane at infinity has to be calculated as following:

function plane = computePlaneAtInfinity(P, K)

%Input
%   P - Projection matrices
%   K - Approximate Values of Intrinsics
%
%Output
%   plane - coordinate of plane at infinity

% Compute the DIAC W^-1 
W_invert = K * K';

% Construct Symbolic Variables to Solve for Plane at Infinity
% X,Y,Z is the coordinate of plane at infinity
% XX is the scale
X = sym('X', 'real'); 
Y = sym('Y', 'real');
Z = sym('Z', 'real');
XX = sym('XX', 'real');

% Define Normal to Plane at Infinity
N = [X; Y; Z]; 

% Equation of Dual Absolute Quadric (DAQ)
Q = [W_invert, (W_invert * N); (N' * W_invert), (N' * W_invert * N)]; 

% Select Any One Projection Matrix
M = P(:, :, 2);

% Left hand side of the equation
LHS = M * Q * M';

% Solve for [X, Y, Z] considering the System of Linear Equations
% We need 4 equations for 4 variables X,Y,Z,XX
S = solve(LHS(1, 1) == (XX * W_invert(1, 1)), ...
          LHS(1, 2) == (XX * W_invert(1, 2)), ...
          LHS(1, 3) == (XX * W_invert(1, 3)), ...
          LHS(2, 2) == (XX * W_invert(2, 2)));

plane = [double(S.X(1)); double(S.Y(1)); double(S.Z(1))];

end