convertDisparityToDepthEquidistant.m

% Convert correspondence to depth (3d) - for equidistant model dataset
close all
clear all

% Change these two files (f and calibFile)
f = readFlowFile('im203.flo');
calibFile = fopen('im203.xml');

u = f(:,:,1);
v = f(:,:,2);
focal = 800.0/(2*pi/3); %FOV used when the dataset was generated

% Read calibration file
for g=1:14
    tline = fgetl(calibFile);
end
[rr, ~] = fscanf(calibFile, '%f');
for g=1:6
    tline = fgetl(calibFile);
end
[tt, ~] = fscanf(calibFile, '%f');
R = [rr(1) rr(2) rr(3); rr(4) rr(5) rr(6); rr(7) rr(8) rr(9)];
t = [tt(1) tt(2) tt(3)]';
fclose(calibFile);
t1 = t(1);
t2 = t(2);
t3 = t(3);

% Set intrinsic parameters
cx = 400;
cy = 400;
K0 = [focal 0 400; 0 focal 400; 0 0 1];
K1 = K0;
scaling = 1.0;
height = (800/scaling);
width = (800/scaling);

% Create Mapping
u0 = repmat(1:width, height, 1);
v0 = repmat((1:height)', 1, width);

% Project from first camera
xprime0 = (u0 - K0(1,3));%/K0(1,1);
yprime0 = (v0 - K0(2,3));%/K0(2,2);
alpha0 = sqrt(xprime0.*xprime0 + yprime0.*yprime0);
theta0 = alpha0 / focal;
alphabar0 = focal.*tan(theta0);
phi0 = atan2(yprime0, xprime0);
xbar0 = alphabar0.*cos(phi0);
ybar0 = alphabar0.*sin(phi0);

% Project from second camera
xprime1 = xprime0 + u;
yprime1 = yprime0 + v;
alpha1 = sqrt(xprime1.*xprime1 + yprime1.*yprime1);
theta1 = alpha1./focal;
alphabar1 = focal.*tan(theta1);
phi1 = atan2(yprime1, xprime1);
xbar1 = alphabar1.*cos(phi1);
ybar1 = alphabar1.*sin(phi1);

% Triangulation
a = xbar0/focal;
b = ybar0/focal;
c = xbar1/focal;
d = ybar1/focal;
if (t1 ~= 0)
    Z = (t1 - c.*t3)./(-a + c);
else
    Z = (t2 - d.*t3)./(-b + d);
end
X = a.*Z;
Y = b.*Z;

% Visualize
imshow(Z, [0 5], 'ColorMap', jet);