generateVectorFieldEquidistant.m

%% Generate vector field from fisheye stereo from faro
close all
clear all

% Change this file (calibFile)
calibFile = fopen('im203.xml');

calibrationFilename = 'calibrationField.flo';
translationFilename = 'trajectoryField.flo';

focal = 800.0/(2*pi/3); % Specific for the dataset
cx = 400;
cy = 400;
K0 = [focal 0 cx; 0 focal cy; 0 0 1];
K1 = K0;

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);

scaling = 1.0;
height = (800/scaling);
width = (800/scaling);

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

xprime0 = (u0 - K0(1,3));%/K0(1,1);
yprime0 = (v0 - K0(2,3));%/K0(2,2);
theta = sqrt(xprime0.*xprime0 + yprime0.*yprime0) / focal;

Xradius = 1.0;
Z = Xradius*cos(theta);
phi = atan2(yprime0, xprime0);
X = Xradius*sin(theta).*cos(phi);
Y = Xradius*sin(theta).*sin(phi);

% Convert XYZ to second camera
%% Forward direction
translateScale = 1.0;
tscale = t*translateScale;

XX = X + tscale(1);
YY = Y + tscale(2);
ZZ = Z + tscale(3);

XXradius = sqrt(XX.^2 + YY.^2);
theta2 = atan2(XXradius, ZZ);
alpha2 = focal*theta2;
phi2 = atan2(YY, XX);
xprime1 = alpha2 .* cos(phi2);
yprime1 = alpha2 .* sin(phi2);
u1 = xprime1 + K1(1,3);
v1 = yprime1 + K1(2,3);
vectorxforward = u1 - u0;
vectoryforward = v1 - v0;

%% Backward direction
translateScale = 0.01;
tscale = t*(-translateScale);

XX = X + tscale(1);
YY = Y + tscale(2);
ZZ = Z + tscale(3);

XXradius = sqrt(XX.^2 + YY.^2);
theta2 = atan2(XXradius, ZZ);
alpha2 = focal*theta2;
phi2 = atan2(YY, XX);
xprime1 = alpha2 .* cos(phi2);
yprime1 = alpha2 .* sin(phi2);
u1 = xprime1 + K1(1,3);
v1 = yprime1 + K1(2,3);
vectorxbackward = u1 - u0;
vectorybackward = v1 - v0;

vectorx = 0.5*(vectorxforward - vectorxbackward);
vectory = 0.5*(vectoryforward - vectorybackward);
% Normalize
magnitude = sqrt(vectorx.^2 + vectory.^2);
vectorx = vectorx ./ magnitude;
vectory = vectory ./ magnitude;

translationVector(:,:,1) = vectorx;
translationVector(:,:,2) = vectory;
writeFlowFile(translationVector, translationFilename);

%% Calibration Vector
XX = R(1,1)*X + R(1,2)*Y + R(1,3)*Z;
YY = R(2,1)*X + R(2,2)*Y + R(2,3)*Z;
ZZ = R(3,1)*X + R(3,2)*Y + R(3,3)*Z;

XXradius = sqrt(XX.^2 + YY.^2);
theta2 = atan2(XXradius, ZZ);
alpha2 = focal*theta2;
phi2 = atan2(YY, XX);
xprime1 = alpha2 .* cos(phi2);
yprime1 = alpha2 .* sin(phi2);
u1 = xprime1 + K1(1,3);
v1 = yprime1 + K1(2,3);

calibrationVectorX = u1 - u0;
calibrationVectorY = v1 - v0;

calibrationVector(:,:,1) = calibrationVectorX;
calibrationVector(:,:,2) = calibrationVectorY;
writeFlowFile(calibrationVector, calibrationFilename);

%% Display
quiverSampling = 10;
u0s = zeros(height/10, floor(width/10) + 1);
v0s = zeros(height/10, floor(width/10) + 1);
vectorxs = zeros(height/10, floor(width/10) + 1);
vectorys = zeros(height/10, floor(width/10) + 1);
for j=1:quiverSampling:height
    for i=1:quiverSampling:width
        u0s(int32(j/quiverSampling) + 1, int32(i/quiverSampling) + 1) = u0(j,i);
        v0s(int32(j/quiverSampling) + 1, int32(i/quiverSampling) + 1) = v0(j,i);
        vectorxs(int32(j/quiverSampling) + 1, int32(i/quiverSampling) + 1) = vectorx(j,i);
        vectorys(int32(j/quiverSampling) + 1, int32(i/quiverSampling) + 1) = vectory(j,i);
    end
end
normVectorxs = vectorxs./sqrt(vectorxs.^2 + vectorys.^2);
normVectorys = vectorys./sqrt(vectorxs.^2 + vectorys.^2);

calibrationVectorXs = zeros(height/10, floor(width/10) + 1);
calibrationVectorYs = zeros(height/10, floor(width/10) + 1);
for j=1:quiverSampling:height
    for i=1:quiverSampling:width
        u0s(int32(j/quiverSampling) + 1, int32(i/quiverSampling) + 1) = u0(j,i);
        v0s(int32(j/quiverSampling) + 1, int32(i/quiverSampling) + 1) = v0(j,i);
        calibrationVectorXs(int32(j/quiverSampling) + 1, int32(i/quiverSampling) + 1) = calibrationVectorX(j,i);
        calibrationVectorYs(int32(j/quiverSampling) + 1, int32(i/quiverSampling) + 1) = calibrationVectorY(j,i);
    end
end
normRotationVectorxs = calibrationVectorXs./sqrt(calibrationVectorXs.^2 + calibrationVectorYs.^2);
normRotationVectorys = calibrationVectorYs./sqrt(calibrationVectorXs.^2 + calibrationVectorYs.^2);

figure('name','Calibration Vector'), imshow(computeColor(calibrationVectorX/50, calibrationVectorY/50));
hold on
h0 = quiver(u0s, v0s, normRotationVectorxs, normRotationVectorys);
hold off

figure('name','Translation Vector'), imshow(computeColor(vectorx/5, vectory/5));
hold on
h1 = quiver(u0s, v0s, normVectorxs, normVectorys);
axis equal
hold off