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