stereo_vs_mono.m

clear all
close all

%% config
%true depth
% sampling time 
ST = 0.033 %33ms
END = 5.0 % 5sec simulation
STEREO_NOISE_S = 1; % 1px sigma for stereo disparity noise
MONO_NOISE_S = 1; % 1?? sigma for monocular distance estimation noise

%% make ground truth data 
t = (0:ST:END).'; %time 
Z = 0.3 + 0.1*sin(t);% 300mm +- 100mm

%% Noisy Observation 
BF = 0.065*400; % base line * focal length
StereoNoise = STEREO_NOISE_S * randn(length(t),1);
Disp = BF./Z + StereoNoise;

figure(1);
plot(t,Disp)
title('Measured Disparity')
xlabel('time [s]')
ylabel('disparity [pix]')
grid on;
figure(2)
plot(t,BF./Disp,'r',t,Z,'b--')
title('Estimated Depth')
xlabel('time [s]')
ylabel('Depth [m]')
legend('measured','groudn truth')
grid on;

%% Noisy Monocluar Obserbation
Z0 = 0.3 % the real distance template is taken
IMG_SIZE = 300/2% 300 times 300 pix template
MonoNoise = MONO_NOISE_S*randn(length(t),1); % sigma = 1px image noise 
Snoise = 2./(Z0./Z * IMG_SIZE).* MonoNoise;
Scale = Z0./Z + Snoise;

figure(3);
plot(t,Scale,'r',t,Z0./Z,'b--')
title('Estimated Scaling')
xlabel('time [s]')
ylabel('Scaling')
legend('measured','groudn truth')
grid on;

%% How to merge this information?
% take log of scaling
LS = log(Scale);
DLS = LS(2:length(t)) - LS(1:length(t)-1);
figure(4)
plot(t,log(Scale),t(2:length(t)),DLS)

% covariance
P = MONO_NOISE_S * 4
Q = STEREO_NOISE_S
%%
BF_ = BF*1.01;
lz_init = -log(Z(1));
LZ = zeros(size(Z));
LZ(1) = lz_init;
rawLZ = LZ;
Cov = zeros(size(Z));
for i=2:length(t)
    % Estimate
    lzhat = LZ(i-1) + DLS(i-1);
    Pnew = Cov(i-1) + P;
    H = exp(lzhat)*BF_;
    Kgain = Pnew * H /(H*P*H+Q)
    % update
    LZ(i) =lzhat + Kgain*(Disp(i)-exp(lzhat)*BF_);
    Cov(i) = (1-Kgain*H)*Pnew ;
    rawLZ(i) = rawLZ(i-1)+DLS(i-1);
end
%%
figure(5);
plot(t,LZ,'r',t,-log(Z),'b--',t,rawLZ,'m-.',t,log(Disp/BF_),'k')
title('Estimated Scaling')
xlabel('time [s]')
ylabel('Scaling')
legend('KF','groudn truth','RAW')
grid on;

figure(6)
plot(t,Cov)