// A method equivalent to RegistrationImpl::depth_to_color() in registration.cpp of libfreenect2.
private static Vector2 DepthToColor(float mx,
float my,
KinectColorIntrinsics colorCameraParams,
KinectIrIntrinsics irCameraParams)
{
mx = (mx - irCameraParams.Cx) * DEPTH_Q;
my = (my - irCameraParams.Cy) * DEPTH_Q;
float wx = mx * mx * mx * colorCameraParams.MxX3y0
+ my * my * my * colorCameraParams.MxX0y3
+ mx * mx * my * colorCameraParams.MxX2y1
+ mx * my * my * colorCameraParams.MxX1y2
+ mx * mx * colorCameraParams.MxX2y0
+ my * my * colorCameraParams.MxX0y2
+ mx * my * colorCameraParams.MxX1y1
+ mx * colorCameraParams.MxX1y0
+ my * colorCameraParams.MxX0y1
+ colorCameraParams.MxX0y0;
float wy = mx * mx * mx * colorCameraParams.MyX3y0
+ my * my * my * colorCameraParams.MyX0y3
+ mx * mx * my * colorCameraParams.MyX2y1
+ mx * my * my * colorCameraParams.MyX1y2
+ mx * mx * colorCameraParams.MyX2y0
+ my * my * colorCameraParams.MyX0y2
+ mx * my * colorCameraParams.MyX1y1
+ mx * colorCameraParams.MyX1y0
+ my * colorCameraParams.MyX0y1
+ colorCameraParams.MyX0y0;
float rx = (wx / (colorCameraParams.Fx * COLOR_Q)) - (colorCameraParams.ShiftM / colorCameraParams.ShiftD);
float ry = (wy / COLOR_Q) + colorCameraParams.Cy;
return(new Vector2(rx, ry));
}
public KinectScreen(KinectColorIntrinsics colorIntrinsics,
KinectIrIntrinsics irIntrinsics)
{
const int KINECT_DEPTH_WIDTH = 512;
const int KINECT_DEPTH_HEIGHT = 424;
Vertices = new Vector3[KINECT_DEPTH_WIDTH * KINECT_DEPTH_HEIGHT];
Uv = new Vector2[KINECT_DEPTH_WIDTH * KINECT_DEPTH_HEIGHT];
Uv2 = new Vector2[KINECT_DEPTH_WIDTH * KINECT_DEPTH_HEIGHT];
for (int y = 0; y < KINECT_DEPTH_HEIGHT; ++y)
{
for (int x = 0; x < KINECT_DEPTH_WIDTH; ++x)
{
var distort = Distort(x, y, irIntrinsics);
var depthToColor = DepthToColor(x, y, colorIntrinsics, irIntrinsics);
// In line 356~357 of RegistrationImpl::getPointXYZ, r (row) and c (column) are directly used in calculating x and y.
// The negative sign in front of the x value below comes from the difference between Kinect's cooridnate system and Unity's.
// The negative sign in front of the y value below is a conversion of the rows and columns into x's and y's where the origin is
// not top-left, but bottom-left.
Vertices[x + y * KINECT_DEPTH_WIDTH] = new Vector3(-(x + 0.5f - irIntrinsics.Cx) / irIntrinsics.Fx,
-(y + 0.5f - irIntrinsics.Cy) / irIntrinsics.Fy,
1.0f);
// This can be inferred from how the values of "Frame* registered" are obtained.
Uv[x + y * KINECT_DEPTH_WIDTH] = depthToColor;
// This can be inferred from how the values of "Frame* undistorted" are obtained.
Uv2[x + y * KINECT_DEPTH_WIDTH] = new Vector2(distort.x / (KINECT_DEPTH_WIDTH - 1), distort.y / (KINECT_DEPTH_HEIGHT - 1));
}
}
ColorIntrinsics = colorIntrinsics;
}
// Parses a message that contains a KinectIntrinsics and uses the KinectIntrinsics to build a KinectScreen
// that gets used for properly rendering the Kinect frame pixels in 3D.
private static KinectScreen CreateKinectScreenFromIntrinsicsMessage(byte[] message)
{
int cursor = 1;
KinectColorIntrinsics colorIntrinsics;
{
float fx = BitConverter.ToSingle(message, cursor);
cursor += 4;
float fy = BitConverter.ToSingle(message, cursor);
cursor += 4;
float cx = BitConverter.ToSingle(message, cursor);
cursor += 4;
float cy = BitConverter.ToSingle(message, cursor);
cursor += 4;
float shiftD = BitConverter.ToSingle(message, cursor);
cursor += 4;
float shiftM = BitConverter.ToSingle(message, cursor);
cursor += 4;
float mxX3y0 = BitConverter.ToSingle(message, cursor);
cursor += 4;
float mxX0y3 = BitConverter.ToSingle(message, cursor);
cursor += 4;
float mxX2y1 = BitConverter.ToSingle(message, cursor);
cursor += 4;
float mxX1y2 = BitConverter.ToSingle(message, cursor);
cursor += 4;
float mxX2y0 = BitConverter.ToSingle(message, cursor);
cursor += 4;
float mxX0y2 = BitConverter.ToSingle(message, cursor);
cursor += 4;
float mxX1y1 = BitConverter.ToSingle(message, cursor);
cursor += 4;
float mxX1y0 = BitConverter.ToSingle(message, cursor);
cursor += 4;
float mxX0y1 = BitConverter.ToSingle(message, cursor);
cursor += 4;
float mxX0y0 = BitConverter.ToSingle(message, cursor);
cursor += 4;
float myX3y0 = BitConverter.ToSingle(message, cursor);
cursor += 4;
float myX0y3 = BitConverter.ToSingle(message, cursor);
cursor += 4;
float myX2y1 = BitConverter.ToSingle(message, cursor);
cursor += 4;
float myX1y2 = BitConverter.ToSingle(message, cursor);
cursor += 4;
float myX2y0 = BitConverter.ToSingle(message, cursor);
cursor += 4;
float myX0y2 = BitConverter.ToSingle(message, cursor);
cursor += 4;
float myX1y1 = BitConverter.ToSingle(message, cursor);
cursor += 4;
float myX1y0 = BitConverter.ToSingle(message, cursor);
cursor += 4;
float myX0y1 = BitConverter.ToSingle(message, cursor);
cursor += 4;
float myX0y0 = BitConverter.ToSingle(message, cursor);
cursor += 4;
colorIntrinsics = new KinectColorIntrinsics(fx: fx,
fy: fy,
cx: cx,
cy: cy,
shiftD: shiftD,
shiftM: shiftM,
mxX3y0: mxX3y0,
mxX0y3: mxX0y3,
mxX2y1: mxX2y1,
mxX1y2: mxX1y2,
mxX2y0: mxX2y0,
mxX0y2: mxX0y2,
mxX1y1: mxX1y1,
mxX1y0: mxX1y0,
mxX0y1: mxX0y1,
mxX0y0: mxX0y0,
myX3y0: myX3y0,
myX0y3: myX0y3,
myX2y1: myX2y1,
myX1y2: myX1y2,
myX2y0: myX2y0,
myX0y2: myX0y2,
myX1y1: myX1y1,
myX1y0: myX1y0,
myX0y1: myX0y1,
myX0y0: myX0y0);
}
KinectIrIntrinsics irIntrinsics;
{
float fx = BitConverter.ToSingle(message, cursor);
cursor += 4;
float fy = BitConverter.ToSingle(message, cursor);
cursor += 4;
float cx = BitConverter.ToSingle(message, cursor);
cursor += 4;
float cy = BitConverter.ToSingle(message, cursor);
cursor += 4;
float k1 = BitConverter.ToSingle(message, cursor);
cursor += 4;
float k2 = BitConverter.ToSingle(message, cursor);
cursor += 4;
float k3 = BitConverter.ToSingle(message, cursor);
cursor += 4;
float p1 = BitConverter.ToSingle(message, cursor);
cursor += 4;
float p2 = BitConverter.ToSingle(message, cursor);
irIntrinsics = new KinectIrIntrinsics(fx: fx,
fy: fy,
cx: cx,
cy: cy,
k1: k1,
k2: k2,
k3: k3,
p1: p1,
p2: p2);
}
return(new KinectScreen(colorIntrinsics, irIntrinsics));
}
