function [Obstacles,Obs_point] = OD(depth,azRange,altRange)
%OBSTACLE DETECTION ALGORITHM 
%INPUTS:
%   depth - depth map
%   azRange - azimuthal range of the chopped image
%   altRange - altitude range of the chopped image
%OUTPUT:
%   Obstacles - binary image with 1s corresponding to obstacle pixel of the
%               depth map no
%   Obs_point - array with indices corresponding to obstacles in the depth
%               map


[H,W] = size(depth);
azRange = azRange.*pi./180;
altRange = altRange.*pi./180;
[az, alt] = meshgrid(linspace(azRange(1), azRange(2), W), linspace(altRange(1), altRange(2), H));

%Transforming 3D points corresponding to pixels in the depth map from
%sherical to cartesan coordinate system
[x,y,z] = sph2cart(az, alt, depth);
scale_alt = H/pi;

%Cone parameters
Hmax = 1.5;
HT = 0.2;
thetaT = 70*pi/180;
tan_thetaT = tan(thetaT);


counter = 0;
Obs_point = [];

%loop through each pixel
for ii = 1:H
    for jj = 1:W
        disp(counter)
        %height of the projected triangle
        h_tri = round(scale_alt*Hmax/(depth(ii,jj)));
        
        %needn't consider triangles protruding above the image plane
        if ii-h_tri<=0
            continue
        end
        hT = round(scale_alt*HT/depth(ii,jj));
        
        p1 = [x(ii,jj),y(ii,jj),z(ii,jj)]; %base point
        inCone = [];
        
        %loop through pixels in the triangle
        for p = (ii-h_tri):(ii-hT)
            for q = jj -round( (ii-p)/tan_thetaT ):jj +round( (ii-p)/tan_thetaT )
                %check boundary conditions
                if q<=0 || q>W
                    continue
                end
                p2 = [x(p,q),y(p,q),z(p,q)];  %triangle point

                %compatibility check
                if obstacleCheck(p1,p2,HT,Hmax,thetaT)
                    Obs_point(end+1,:) = [p q];
                end
            end
        end
        counter = counter+1;        
    end
end

Obstacles = zeros(H,W);
for n = 1:size(Obs_point,1)
    point = Obs_point(n,:);
    Obstacles(point(1),point(2)) = 1;
end
end