generateVectorFieldNewton.m

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

translationVectorFilename = 'translationVectorNewton.flo';
calibrationVectorFilename = 'calibrationVectorNewton.flo';

% Calibration data from T265
K0 = [285.722 0 420.135; 0 286.759 403.394; 0 0 1];
D0 = [-0.00659769 0.0473251 -0.0458264 0.00897725 0];
K1 = [284.936 0 428.136; 0 286.006 398.921; 0 0 1];
D1 = [-0.00492777 0.0391601 -0.0353147 0.0051312 0];
R = [0.999985 0.00061035 -0.00553418;
    -0.00058935 0.999993 0.00379553;
    0.00553646 -0.00379221 0.999977];
t = [-0.0641854; -0.000218299; 0.000111253];

% Scaling to use smaller input images
scaling = 1.0;
height = (800/scaling);
width = (848/scaling);
K0 = K0/scaling;
K1 = K1/scaling;
D0 = D0/scaling;
D1 = D1/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);
x0out = xprime0;
y0out = yprime0;

%% Newton Method %Create the surface
for iter=1:4
    xold = x0out(200,200);
    r = sqrt(x0out.^2 + y0out.^2);
    theta = atan(r);
    thetad = theta.*(1 + D0(1).*theta.^2 + D0(2).*theta.^4 + D0(3).*theta.^6 + D0(4).*theta.^8);
    f1 = xprime0 - (thetad ./ r) .* x0out;
    f2 = yprime0 - (thetad ./ r) .* y0out;
    Drx = x0out./r;
    Dry = y0out./r;
    Dthetax = 1./(1 + r.^2) .* Drx;
    Dthetay = 1./(1 + r.^2) .* Dry;
    Dthetadx = Dthetax .* (1 + D0(1).*theta.^2 + D0(2).*theta.^4 + D0(3).*theta.^6 + D0(4).*theta.^8) +...
        thetad.*Dthetax.*(2*D0(1).*thetad + 4*D0(2)*theta.^3 + 6*D0(3)*theta.^5 + 8*D0(4)*theta.^7);
    Dthetady = Dthetay .* (1 + D0(1).*theta.^2 + D0(2).*theta.^4 + D0(3).*theta.^6 + D0(4).*theta.^8) +...
        thetad.*Dthetay.*(2*D0(1).*thetad + 4*D0(2)*theta.^3 + 6*D0(3)*theta.^5 + 8*D0(4)*theta.^7);
    fprime1 = -x0out.*(r.*Dthetadx - thetad.*Drx)./(r.^2) - thetad./r;
    fprime2 = -y0out.*(r.*Dthetady - thetad.*Dry)./(r.^2) - thetad./r;
    x0out = x0out - f1 ./ fprime1;
    y0out = y0out - f2 ./ fprime2;
end

%% Reproject surface to frame 1
Zscale = 1.0; 
X = x0out * Zscale;
Y = y0out * Zscale;
Z = (1.0 + zeros(height, width))*Zscale; %plane at Z=1
translateScale = 1.0;
%K1 = K0;

%% First direction
tscale = t*translateScale;
XX = X + tscale(1);
YY = Y + tscale(2);
ZZ = Z + tscale(3);

a = XX./ZZ;
b = YY./ZZ;
r = sqrt(a.^2 + b.^2);
theta = atan2(r,1);
thetad = theta.*(1 + D1(1)*theta.^2 + D1(2)*theta.^4 + D1(3)*theta.^6 + D1(4)*theta.^8);
xprime1 = a.*(thetad./r);
yprime1 = b.*(thetad./r);
u1 = K0(1,1) * xprime1 + K0(1,3); % Should be K0 because surface is already corrected
v1 = K0(2,2) * yprime1 + K0(2,3);
vectorxforward = u1 - u0;
vectoryforward = v1 - v0;

%% Opposite direction
tscale = t*(-translateScale);
XX = X + tscale(1);
YY = Y + tscale(2);
ZZ = Z + tscale(3);

a = XX./ZZ;
b = YY./ZZ;
r = sqrt(a.^2 + b.^2);
theta = atan2(r,1);
thetad = theta.*(1 + D1(1)*theta.^2 + D1(2)*theta.^4 + D1(3)*theta.^6 + D1(4)*theta.^8);
xprime1 = a.*(thetad./r);
yprime1 = b.*(thetad./r);
u1 = K0(1,1) * xprime1 + K0(1,3);
v1 = K0(2,2) * yprime1 + K0(2,3);
vectorxbackward = u1 - u0;
vectorybackward = v1 - v0;

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

%% Remove Rotation by warping the image with the rotation vector (CALIBRATION FIELD)
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;

a = XX./ZZ;
b = YY./ZZ;
r = sqrt(a.^2 + b.^2);
theta = atan2(r,1);
thetad = theta.*(1 + D1(1)*theta.^2 + D1(2)*theta.^4 + D1(3)*theta.^6 + D1(4)*theta.^8);
xprime1 = a.*(thetad./r);
yprime1 = b.*(thetad./r);
u1 = K1(1,1) * xprime1 + K1(1,3); % Should be K1 to include calibration
v1 = K1(2,2) * yprime1 + K1(2,3);
calibrationVectorX = u1 - u0;
calibrationVectorY = v1 - v0;

%% Write to Files
translationVector(:,:,1) = vectorx;
translationVector(:,:,2) = vectory;
calibrationVector(:,:,1) = calibrationVectorX;
calibrationVector(:,:,2) = calibrationVectorY;
writeFlowFile(translationVector, translationVectorFilename);
writeFlowFile(calibrationVector, calibrationVectorFilename);

%% Display
flowScale = 50;
debugX = width/2;
debugY = height/2;
quiverSampling = 10;
figure('name','Calibration Vector'), imshow(computeColor(calibrationVectorX/flowScale, calibrationVectorY/flowScale));
u0s = zeros(height/10, floor(width/10) + 1);
v0s = zeros(height/10, floor(width/10) + 1);
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);
hold on
h0 = quiver(u0s, v0s, normRotationVectorxs, normRotationVectorys);

figure('name','Translation Vector'), imshow(computeColor(vectorx/flowScale, vectory/flowScale));

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

hold on
h1 = quiver(u0s, v0s, normVectorxs, normVectorys);
% set(h1,'AutoScale','on', 'AutoScaleFactor', 1)