function depth = disparity2depth(disparity, baseline)
%TRIGONOMETRIC TRANSFORMATIONS TO CALCULATE DEPTH FROM VERTICAL DISPARITY

%INPUTS:
%   disparity - disparity image with each pixel value corressponding to the
%               disparity in number of pixels
%   baseline - Distance between the two cameras

%OUTPUT:
%   depth - depth map with each pixel corresponding to depth in meters

[H,W] = size(disparity);
[~, al1] = meshgrid(linspace(pi-pi/W, -pi+pi/W, W), linspace(pi/2-pi/2/H, -pi/2+pi/2/H, H));
scale = pi/H;  %scale pixel -> angle(rad)

al2 = scale*disparity + al1;
PA = baseline*sin(pi/2-al2)./sin(abs(scale*disparity));
depth = sqrt(PA.^2 + (baseline/2)^2 - 2.*PA.*(baseline)/2.*cos(pi/2+al1));
end
