function xyzPoints = getXYZfromDepth_fast(rcPoints, cameraPosition, cameraOut, cameraUp, cameraRight, focalLength, imageHeight, imageWidth, depthMin, depthMax, depthImage)
% rcPoints - the points to get the XYZ for.
% cameraPosition - [0 0 0]
% cameraOut - unit vector pointing out from camera.
% cameraUp - similar
% cameraRight - similar
% focalLength - Have to try different values.
% imageHeight - m.
% imageWidth - n.
% depthMin - 0 for all depth images in the synthetic data.
% depthMax - varies by object class.
% depthImage - the depth image.
% http://ai.stanford.edu/~asaxena/learninggrasp/data.html


%% First convert the depth image from the RGB configuration to the regular 0->1 space.
%depthImage = 1/3 * (depthImage(:,:,1) + (1 + depthImage(:,:,2)) + (2 + depthImage(:,:,3)));
% Assume this is done by getCorrectedValuesStanfordData.  Also, there is a
% missing gamma correction here.

%% Now tranform into XYZ space
rows = size(depthImage, 1);
cols = size(depthImage, 2);
upStep = imageHeight / rows * cameraUp;
rightStep = imageWidth / cols * cameraRight;

xyzPoints = zeros(0,3);
if size(cameraPosition,1) == 3;
	cameraPosition = cameraPosition';
end;
if size(upStep,1) == 3;
	upStep = upStep';
end;
if size(rightStep,1) == 3;
	rightStep = rightStep';
end;

% $$$ xyzPoints2 = zeros(size(rcPoints,2), 3);
% $$$ i=1;
% $$$ for point = rcPoints;
% $$$ 	r = point(1);
% $$$ 	c = point(2);
% $$$ 	vec = focalLength * cameraOut + ((rows + 1) / 2 - r) * upStep + (c - (cols + 1) / 2) * rightStep;
% $$$ 	unitVec = vec / norm(vec);
% $$$ 	xyzPoints2(i,:) = [depthImage(r, c) * unitVec + cameraPosition];
% $$$         i=i+1;
% $$$ end

n = size(rcPoints,2);
t1 = focalLength * repmat(cameraOut, [n 1]); %Term 1.
t2f1 = ((rows+1)/2) - repmat(rcPoints(1,:)', [1 3]); %Term 1, factor 1.
t2f2 = repmat(upStep, [n 1]);
t3f1 = repmat(rcPoints(2,:)', [1 3]) - ((cols + 1)/2);
t3f2 = repmat(rightStep, [n 1]);

vec = t1 + (t2f1 .* t2f2) + (t3f1 .* t3f2);
unitVec = vec ./ repmat(sqrt(sum(vec.^2,2)), [1 3]);
idx = sub2ind(size(depthImage), rcPoints(1,:), rcPoints(2,:));
xyzPoints = repmat(depthImage(idx)', [1 3]) .* unitVec ...
            + repmat(cameraPosition, [n 1]);


return