kinect V2 get real xyz points from raw depth image in matlab without visionKinectDepthToSkeleton or depthToPointCloud or pcfromkinect

Question

There are many ways to get real xyz from raw depth image like vision.internal.visionKinectDepthToSkeleton or depthToPointCloud or pcfromkinect .
The problem is that it's not clear how these functions work exactly . All these functions use vision.internal.visionKinectDepthToSkeleton and visionKinectDepthToSkeleton is a .p file and encrypted .
Why I want to know how do they work ? Cause I want to write an Inverse function for them that get the xyz points and convert it to 424*512 depth image.
In kinect V1 , there is a clear formula for converting raw depth image data to xyz but I couldn't find any working solution for kinect V2.
It's better for me working in matlab environment but feel free using anything you want.
Thanks in advance !

Answer 1

I have the following code which does the job. It reads the depth image and converts into X,Y,Z values. I might have got it from the "libfreenect2" forum. As far as the intrinsic parameters are concerned, you could use the values that I have been using or get you own using the calibration toolbox along with a checker board pattern.

function [Xw, Yw, Zw] = Depth2World_v2(fileName, maxDepth)
% Extrinsic parameters of the depth camera. These values are collected
%from the dicussion forum.
fx=367.286994337726;        % Focal length in X and Y
fy=367.286855347968;
cx=255.165695200749;        % Principle point in X and Y
cy=211.824600345805;
k1=0.0914203770220268;
k2=-0.269349746097515;
k3=0.0925671408453617;
p1=0;
p2=0;

% Read the depth image:
% Ex: fileName = 'depthImg_0018.ppm';
imgPixels = imread(fileName);
% imgPixels = imgPixels (:, end:-1:1);
x3D = zeros(size(imgPixels));
y3D = zeros(size(imgPixels));
z3D = zeros(size(imgPixels));

% Evaluate coordinates from depth image:
[maxR, maxC] = size(imgPixels);
for r=1:maxR
    for c=1:maxC
        % The depth value is equal to intensity. But it is stored in mm.
        d = double(imgPixels(r,c)) / 1000;
        z3D(r,c) = d;
        x3D(r,c) = (c - cx) * z3D(r,c) / fx;
        y3D(r,c) = (r - cy) * z3D(r,c) / fy;
    end
end

Here is a link explaining the camera model based on which the above equations are derived for X, Y and Z.