function [ pcd ] = depth_img_to_xyz( depth , params )
%DEPTH_TO_XYZ Summary of this function goes here
%   Detailed explanation goes here
% TODO Austin

% Convert to double if needed
if ~isa(depth, 'double')
    depth = double(depth);
end

% If the maximum depth is greater than 300 then
% we probably need to convert mm to meters.
if max(depth(:)) > 300
    depth = depth/1000; % Convert mm to m
end

% Our grid should have the same dimensions as the input
[x_grid, y_grid] = meshgrid(1:size(depth, 2), 1:size(depth, 1));
% Undo scaling
x_grid = ((x_grid - 1) / params.scale) + 1;
y_grid = ((y_grid - 1) / params.scale) + 1;
% Undo crop shift
x_grid = x_grid + (params.crop(3) - 1);
y_grid = y_grid + (params.crop(1) - 1);

pcd = zeros(size(depth, 1), size(depth, 2), 3);
pcd(:,:,1) = (x_grid - params.center(1)) .* depth / params.focal_length(1);
pcd(:,:,2) = (y_grid - params.center(2)) .* depth / params.focal_length(2);
pcd(:,:,3) = depth;
end

