/*! Convert Kinect's 2D position to Unity's Vector2
* Kinect reads and stores body joints using its own "Joint" structure. For displaying and further 2D analysis purposes,
* we need to convert to Unity's Vector2 structure.
*
* \param kinectJoint Position of the body joints in Kinect's Joint structure.
* \param scale Size of the screen where the model will be placed.
* \param position Position of the screen where the model will be placed.
* \return Position of the body joints in Unity's Vector2 structure, more suitable for further analysis.
*/
public static UnityEngine.Vector2[] convertToUnity2DPosition(Dictionary <JointType, Windows.Kinect.Joint> kinectJoint, UnityEngine.Vector2 scale, UnityEngine.Vector2 position)
{
// CameraSpacePoint is a representation of 3D position. The units are in meters.
// CameraSpacePoint is part of Kinect's Joint structure, so we need to first extract only the vector of 3D positions.
CameraSpacePoint[] cameraPoints = new CameraSpacePoint[25];
for (JointType jt = JointType.SpineBase; jt <= JointType.ThumbRight; jt++)
{
cameraPoints[(int)jt] = kinectJoint[jt].Position;
}
// ColorSpacePoint is a representation of 2D position.
// Kinect maps 3D points to 2D points
ColorSpacePoint[] colorPoints = new ColorSpacePoint[25];
_Mapper.MapCameraPointsToColorSpace(cameraPoints, colorPoints);
// Vector2 is Unity's representation of a 2D position.
// The conversion involves the scaling to the screen where the rendering will happen.
UnityEngine.Vector2[] colorJointPosition = new UnityEngine.Vector2[25];
for (JointType jt = JointType.SpineBase; jt <= JointType.ThumbRight; jt++)
{
colorJointPosition[(int)jt] = new UnityEngine.Vector2(colorPoints[(int)jt].X, colorPoints[(int)jt].Y);
colorJointPosition[(int)jt].x = (colorJointPosition[(int)jt].x * scale.x / 1920.0f) - scale.x / 2.0f + position.x;
colorJointPosition[(int)jt].y = scale.y / 2.0f - (colorJointPosition[(int)jt].y * scale.y / 1080.0f) + position.y;
}
return(colorJointPosition);
}
public double[][] CameraToColor(double[][] points)
{
CameraSpacePoint[] cameraSpacePoints = new CameraSpacePoint[points.Length];
ColorSpacePoint[] colorSpacePoints = new ColorSpacePoint[points.Length];
CameraSpacePoint cameraSpacePoint;
ColorSpacePoint colorSpacePoint;
double[] point;
double[][] colorSpacePointsRes = new double[colorSpacePoints.Length][];
for (int i = 0; i < points.Length; i++)
{
point = points[i];
if (point != null)
{
cameraSpacePoint.X = (float)point[0];
cameraSpacePoint.Y = (float)point[1];
cameraSpacePoint.Z = (float)point[2];
cameraSpacePoints[i] = cameraSpacePoint;
}
}
mapper.MapCameraPointsToColorSpace(cameraSpacePoints, colorSpacePoints);
for (int i = 0; i < colorSpacePoints.Length; i++)
{
colorSpacePoint = colorSpacePoints[i];
if (colorSpacePoint != null && points[i] != null)
{
point = new double[3];
point[0] = colorSpacePoint.X;
point[1] = colorSpacePoint.Y;
point[2] = points[i][2];
colorSpacePointsRes[i] = point;
}
else
{
colorSpacePointsRes[i] = null;
}
}
return(colorSpacePointsRes);
}
public static void calculateProject( CoordinateMapper coordinateMapper, String outputFilename)
{
CameraSpacePoint[] spacePointBasics = new CameraSpacePoint[] {
new CameraSpacePoint { X = -0.1f, Y = 0.0f, Z = 1.0f },
new CameraSpacePoint { X = -0.7f, Y = 0.0f, Z = 1.0f },
new CameraSpacePoint { X = 0.0f, Y = -0.1f, Z = 1.0f },
new CameraSpacePoint { X = 0.0f, Y = -0.7f, Z = 1.0f },
new CameraSpacePoint { X = 0.7f, Y = 0.0f, Z = 1.0f },
new CameraSpacePoint { X = 0.35f, Y = 0.0f, Z = 1.0f },
new CameraSpacePoint { X = 0.0f, Y = 0.3f, Z = 1.0f },
new CameraSpacePoint { X = 0.0f, Y = 0.0f, Z = 1.0f },
//Skeleton_Joint_Locations 0.51399,0.163888,1.20627,0.494376,0.362051,1.19127
new CameraSpacePoint { X = 0.51399f, Y = 0.163888f, Z = 1.20627f },
new CameraSpacePoint { X = 0.494376f, Y = 0.362051f, Z = 1.19127f },
//Skeleton_Joint_Locations_Orig 0.534418,-0.159339,1.38631,0.523629,0.0243074,1.30818
new CameraSpacePoint { X = 0.534418f, Y = -0.159339f, Z = 1.38631f },
new CameraSpacePoint { X = 0.523629f, Y = 0.0243074f, Z = 1.30818f }, };
DepthSpacePoint[] spaceBasicToDepth = new DepthSpacePoint[spacePointBasics.Count()];
coordinateMapper.MapCameraPointsToDepthSpace(spacePointBasics, spaceBasicToDepth);
ColorSpacePoint[] spaceBasicToColor = new ColorSpacePoint[spacePointBasics.Count()];
coordinateMapper.MapCameraPointsToColorSpace(spacePointBasics, spaceBasicToColor);
ColorSpacePoint[] spaceBasicToDepthToColor = new ColorSpacePoint[spacePointBasics.Count()];
coordinateMapper.MapDepthPointsToColorSpace(spaceBasicToDepth, Enumerable.Repeat((ushort)1000, spacePointBasics.Count()).ToArray(), spaceBasicToDepthToColor);
Console.WriteLine("Camera space points to depth space points");
foreach (var t in spaceBasicToDepth)
{
Console.WriteLine(t.ToSString());
}
Console.WriteLine("Camera space points to color space points");
foreach (var t in spaceBasicToColor)
{
Console.WriteLine(t.ToSString());
}
Console.WriteLine("Camera space points to depth space points then to color space points");
foreach (var t in spaceBasicToDepthToColor)
{
Console.WriteLine(t.ToSString());
}
DepthSpacePoint[] depthBasics = new DepthSpacePoint[]
{
new DepthSpacePoint() { X = 0.0f, Y = 30.0f },
new DepthSpacePoint() { X = 0.0f, Y = 380.0f },
new DepthSpacePoint() { X = 511.0f, Y = 30.0f },
new DepthSpacePoint() { X = 511.0f, Y = 380.0f },
new DepthSpacePoint() { X = 262.7343f, Y = 203.6235f }, // Center of origin
};
int noOfPoints = depthBasics.Count();
ColorSpacePoint[] depthBasicToColor = new ColorSpacePoint[noOfPoints];
CameraSpacePoint[] depthBasicToCamera = new CameraSpacePoint[noOfPoints];
// Depth used for depth field are the same as z-axis
// Not the distance from IR sensor to the point
ushort[] distances = new ushort[] { 1, 10, 100, 1000, 2000, 40000 };
foreach (var distance in distances)
{
coordinateMapper.MapDepthPointsToColorSpace(depthBasics, Enumerable.Repeat(distance, noOfPoints).ToArray(), depthBasicToColor);
Console.WriteLine("Projection from depth to color ");
Console.WriteLine("z-axis = " + distance);
foreach (var point in depthBasicToColor)
{
Console.WriteLine(point.ToSString());
}
coordinateMapper.MapDepthPointsToCameraSpace(depthBasics, Enumerable.Repeat(distance, noOfPoints).ToArray(), depthBasicToCamera);
Console.WriteLine("Projection from depth to camera ");
Console.WriteLine("z-axis = " + distance);
foreach (var point in depthBasicToCamera)
{
Console.WriteLine(point.ToSString());
}
for (int i = 0; i < noOfPoints; i++)
{
Console.WriteLine(projectDepthPixelToCameraSpacePoint(new Point3(depthBasics[i].X, depthBasics[i].Y, distance)).ToSString());
}
}
// Let's X_ir(P) being P.X in depth image, X_rgb(P) being P.X in RGB
// The calculation here is just an estimation, doesn't take into account camera calibration
// Solve the problem on the z-plane of 1 meter
// Center of depth field has the same X, Y as the zero point of coordinate space
System.Drawing.PointF centerDepthField_ir = new System.Drawing.PointF(spaceBasicToDepth[4].X, spaceBasicToDepth[4].Y);
System.Drawing.PointF centerDepthField_rgb = new System.Drawing.PointF(spaceBasicToColor[4].X, spaceBasicToColor[4].Y);
// It is difficult to get the center point of rgb field directly, so let's assume it is the center of rgb field
System.Drawing.PointF centerRgbField_rgb = new System.Drawing.PointF(964.5f, 544.5f);
// Vector from centerRgbFieldInRgb to centerDepthFieldInRgb
System.Drawing.PointF translation = new System.Drawing.PointF(centerDepthField_rgb.X - centerRgbField_rgb.X,
centerDepthField_rgb.Y - centerRgbField_rgb.Y);
// Ratio of depth unit / rgb unit
float ratioX = (spaceBasicToDepth[2].X - centerDepthField_ir.X) / (spaceBasicToColor[2].X - centerDepthField_rgb.X);
float ratioY = (spaceBasicToDepth[3].Y - centerDepthField_ir.Y) / (spaceBasicToColor[3].Y - centerDepthField_rgb.Y);
ColorSpacePoint[,] shortRange = new ColorSpacePoint[512, 424];
ColorSpacePoint[,] longRange = new ColorSpacePoint[512, 424];
for ( int X = 0; X < 511; X ++ )
for ( int Y = 0; Y < 423; Y ++ )
{
Point3 p1 = new Point3 { X = X, Y = Y, Z = 500 };
Point3 p2 = new Point3 { X = X, Y = Y, Z = 8000 };
var p1Projected = coordinateMapper.MapDepthPointToColorSpace(new DepthSpacePoint { X = p1.X, Y = p1.Y }, (ushort)p1.Z);
var p2Projected = coordinateMapper.MapDepthPointToColorSpace(new DepthSpacePoint { X = p2.X, Y = p2.Y }, (ushort)p2.Z);
shortRange[X, Y] = p1Projected;
longRange[X, Y] = p2Projected;
}
// Write these projected points into a file
if ( !File.Exists(COORDINATE_MAPPING) )
{
var coordinateWriter = new DepthCoordinateMappingWriter(COORDINATE_MAPPING);
coordinateWriter.write(512, 424, 500, 8000, shortRange, longRange);
}
// Let's get a random space point with a depth
Point3[] PointAs = new Point3[] { new Point3 { X = 300f, Y = 300f, Z = 500 },
new Point3 { X = 400f, Y = 400f, Z = 500 },
new Point3 { X = 300f, Y = 300f, Z = 1000 },
new Point3 { X = 400f, Y = 400f, Z = 1000 },
new Point3 { X = 300f, Y = 300f, Z = 2000 },
new Point3 { X = 400f, Y = 400f, Z = 2000 },
new Point3 { X = 100f, Y = 100f, Z = 1000 },
new Point3 { X = 100f, Y = 100f, Z = 2000 },
new Point3 { X = 0f, Y = 100f, Z = 1000 },
new Point3 { X = 0f, Y = 100f, Z = 2000 },
new Point3 { X = 50f, Y = 100f, Z = 1000 },
new Point3 { X = 50f, Y = 100f, Z = 2000 },
new Point3 { X = 50f, Y = 50f, Z = 1000 },
new Point3 { X = 50f, Y = 50f, Z = 2000 },
new Point3 { X = 500f, Y = 50f, Z = 1000 },
new Point3 { X = 500f, Y = 50f, Z = 2000 },
new Point3 { X = 10f, Y = 10f, Z = 1000 },
new Point3 { X = 10f, Y = 10f, Z = 2000 },
new Point3 { X = 500f, Y = 200f, Z = 1000 },
new Point3 { X = 500f, Y = 200f, Z = 2000 },
new Point3 { X = depthBasics[4].X, Y = depthBasics[4].Y, Z = 2000 },
new Point3 { X = depthBasics[4].X, Y = depthBasics[4].Y, Z = 1000 },
new Point3 { X = depthBasics[4].X, Y = depthBasics[4].Y, Z = 4000 },
new Point3 { X = depthBasics[4].X, Y = depthBasics[4].Y, Z = 500 },
new Point3 { X = spaceBasicToDepth[2].X, Y = spaceBasicToDepth[2].Y, Z = 1000 },
new Point3 { X = spaceBasicToDepth[2].X, Y = spaceBasicToDepth[2].Y, Z = 2000 },
};
foreach (Point3 PointA in PointAs)
{
// Project randomDepthPoint using ray from IR camera on the 1-z plane
// It still has X_ir = 300, y_ir = 300
System.Drawing.PointF projectedA_ir = new System.Drawing.PointF(PointA.X, PointA.Y);
// Let's find projectedA_rgb
System.Drawing.PointF projectedA_rgb = new System.Drawing.PointF(translation.X * 1000 / PointA.Z + (projectedA_ir.X - centerDepthField_ir.X) / ratioX + centerRgbField_rgb.X,
translation.Y * 1000 / PointA.Z + (projectedA_ir.Y - centerDepthField_ir.Y) / ratioY + centerRgbField_rgb.Y);
// Test
Console.WriteLine("======Test=====");
Console.WriteLine(PointA.X + ", " + PointA.Y + ", " + PointA.Z);
Console.WriteLine(projectedPoint(shortRange, longRange, 500, 8000, PointA));
Console.WriteLine(projectedA_rgb);
Console.WriteLine(coordinateMapper.MapDepthPointToColorSpace(new DepthSpacePoint { X = PointA.X, Y = PointA.Y }, (ushort)PointA.Z).ToSString());
}
}
