private void CreatePointCloud(ushort[] depthData, Texture2D colorData)
{
int pointCloudSize = scaledH * scaledW;
Vector3[] _pointCloud = new Vector3[pointCloudSize]; //(int)(frameDesc.Width/skip + frameDesc.Height/skip)];
Color[] _colorCloud = new Color[pointCloudSize]; //same size as the point cloud
//now we cycle through the depth frame
int i = 0; //this is the index that tracks both the _pointCloud and the _colorCloud.
for (int x = 0; x < W; x += skip)
{
for (int y = 0; y < H; y += skip)
{
int offset = x + y * W;
ushort depth = depthData[offset];
DepthSpacePoint Pd = new DepthSpacePoint();
Pd.X = x;
Pd.Y = y;
//now that i have my depth spacepoint lets map it to the color space
ColorSpacePoint Cp = _Mapper.MapDepthPointToColorSpace(Pd, depth);
if (depth != 0)
{
pCloudReady = true;
}
Vector3 temp = depthToPointCloudPos(x, y, depth);
_colorCloud[i] = colorData.GetPixel((int)Cp.X, (int)Cp.Y); //we always save the color
/*
* So the point of this is that we want to reduce draw calls by not rendering things
* that are being culled by the window the user picked. But we dont wanna loose the data
*/
if ((Mathf.Abs(temp.x) <= maxW) && (Mathf.Abs(temp.y) <= maxH) && (Mathf.Abs(temp.z) <= maxD))
{ //impliment depth narrowing -- only the points within the window
_pointCloud[i] = temp;
}
else if ((Mathf.Abs(temp.x) <= maxW) && (Mathf.Abs(temp.y) <= maxH) && (Mathf.Abs(temp.z) > maxD))
{
if (project)
{
Vector3 test = projectToBack(temp);
_pointCloud[i] = test;
}
}
else
{
_pointCloud[i] = Vector3.zero;
};
i++;
}
}
pointCloudSize = i;
pointCloud = _pointCloud; //pointCloud = tools.ChopAt(_pointCloud,i);
colorCloud = _colorCloud; //colorCloud = tools.ChopAt(_colorCloud,i);
pCloudSize = pointCloudSize;
}
internal Finger(DepthPointEx point, CoordinateMapper coordinateMapper)
{
ushort depth = (ushort)point.Z;
DepthPoint = new DepthSpacePoint
{
X = point.X,
Y = point.Y
};
ColorPoint = coordinateMapper.MapDepthPointToColorSpace(DepthPoint, (ushort)point.Z);
CameraPoint = coordinateMapper.MapDepthPointToCameraSpace(DepthPoint, (ushort)point.Z);
}
internal Finger(DepthPointEx point, CoordinateMapper coordinateMapper)
{
ushort depth = (ushort)point.Z;
DepthPoint = new DepthSpacePoint
{
X = point.X,
Y = point.Y
};
ColorPoint = coordinateMapper.MapDepthPointToColorSpace(DepthPoint, (ushort)point.Z);
CameraPoint = coordinateMapper.MapDepthPointToCameraSpace(DepthPoint, (ushort)point.Z);
}
public unsafe void Update(ushort[] depthData, byte[] bodyIndexData, Body body)
{
double handLength = 0.0;
double barLength = 0.0;
double barHeight = 0.0;
double angle = 0.0;
Joint shoulderLeft = body.Joints[JointType.ShoulderLeft];
Joint shoulderRight = body.Joints[JointType.ShoulderRight];
Joint chest = body.Joints[JointType.SpineShoulder];
Joint waist = body.Joints[JointType.SpineBase];
Joint elbowLeft = body.Joints[JointType.ElbowLeft];
Joint elbowRight = body.Joints[JointType.ElbowRight];
Joint handLeft = body.Joints[JointType.HandLeft];
Joint handRight = body.Joints[JointType.HandRight];
Joint footLeft = body.Joints[JointType.FootLeft];
Joint footRight = body.Joints[JointType.FootRight];
if (waist.TrackingState == TrackingState.NotTracked)
{
return;
}
if (waist.Position.Z < 2.0f || waist.Position.Z > 4.5f)
{
return;
}
if (shoulderLeft.TrackingState != TrackingState.NotTracked && shoulderRight.TrackingState != TrackingState.NotTracked &&
elbowLeft.TrackingState != TrackingState.NotTracked && elbowRight.TrackingState != TrackingState.NotTracked &&
handLeft.TrackingState != TrackingState.NotTracked && handRight.TrackingState != TrackingState.NotTracked)
{
handLength =
shoulderLeft.Position.Length(shoulderRight.Position) +
shoulderLeft.Position.Length(elbowLeft.Position) +
shoulderRight.Position.Length(elbowRight.Position) +
elbowLeft.Position.Length(handLeft.Position) +
elbowRight.Position.Length(handRight.Position);
}
CoordinateMapper.MapColorFrameToDepthSpace(depthData, _depthPoints);
fixed(DepthSpacePoint *colorMappedToDepthPointsPointer = _depthPoints)
{
int minimumX = int.MaxValue;
int maximumX = int.MinValue;
int minimumY = int.MaxValue;
int maximumY = int.MinValue;
ushort minimumDistance = 0;
ushort maximumdistance = 0;
for (int colorIndex = 0; colorIndex < _depthPoints.Length; ++colorIndex)
{
float colorMappedToDepthX = colorMappedToDepthPointsPointer[colorIndex].X;
float colorMappedToDepthY = colorMappedToDepthPointsPointer[colorIndex].Y;
if (!float.IsNegativeInfinity(colorMappedToDepthX) &&
!float.IsNegativeInfinity(colorMappedToDepthY))
{
int depthX = (int)(colorMappedToDepthX + 0.5f);
int depthY = (int)(colorMappedToDepthY + 0.5f);
if ((depthX >= 0) && (depthX < DepthWidth) && (depthY >= 0) && (depthY < DepthHeight))
{
int depthIndex = (depthY * DepthWidth) + depthX;
ushort depth = depthData[depthIndex];
if (bodyIndexData[depthIndex] != 0xff)
{
if (depthX < minimumX)
{
minimumX = depthX;
minimumY = depthY;
minimumDistance = depth;
}
if (depthX > maximumX)
{
maximumX = depthX;
maximumY = depthY;
maximumdistance = depth;
}
continue;
}
}
}
}
DepthSpacePoint depthMinimum = new DepthSpacePoint
{
X = minimumX,
Y = minimumY
};
DepthSpacePoint depthMaximum = new DepthSpacePoint
{
X = maximumX,
Y = maximumY
};
CameraSpacePoint cameraMinimum = CoordinateMapper.MapDepthPointToCameraSpace(depthMinimum, minimumDistance);
CameraSpacePoint cameraMaximum = CoordinateMapper.MapDepthPointToCameraSpace(depthMaximum, maximumdistance);
ColorSpacePoint colorMinimum = CoordinateMapper.MapDepthPointToColorSpace(depthMinimum, minimumDistance);
ColorSpacePoint colorMaximum = CoordinateMapper.MapDepthPointToColorSpace(depthMaximum, maximumdistance);
CameraSpacePoint cameraTrail = new CameraSpacePoint
{
X = (cameraMinimum.X + cameraMaximum.X) / 2f,
Y = (cameraMinimum.Y + cameraMaximum.Y) / 2f,
Z = (cameraMinimum.Z + cameraMaximum.Z) / 2f
};
ColorSpacePoint colorTrail = new ColorSpacePoint
{
X = (colorMinimum.X + colorMaximum.X) / 2f,
Y = (colorMinimum.Y + colorMaximum.Y) / 2f
};
DepthSpacePoint depthTrail = new DepthSpacePoint
{
X = (depthMinimum.X + depthMaximum.X) / 2f,
Y = (depthMinimum.Y + depthMaximum.Y) / 2f
};
CameraSpacePoint feet = new CameraSpacePoint
{
X = (footLeft.Position.X + footRight.Position.X) / 2f,
Y = (footLeft.Position.Y + footRight.Position.Y) / 2f,
Z = (footLeft.Position.Z + footRight.Position.Z) / 2f
};
CameraSpacePoint projection = new CameraSpacePoint
{
X = cameraTrail.X,
Y = feet.Y,
Z = cameraTrail.Z
};
barLength = cameraMinimum.Length(cameraMaximum);
barHeight = cameraTrail.Length(projection);
angle = cameraMinimum.Angle(cameraMaximum, new CameraSpacePoint {
X = cameraMaximum.X, Y = cameraMinimum.Y, Z = (cameraMaximum.Z + cameraMinimum.Z) / 2f
});
if (angle > 180.0)
{
angle = 360.0 - angle;
}
if (cameraMinimum.Y < cameraMaximum.Y)
{
angle = -angle;
}
if (barLength > handLength)
{
BarDetectionResult result = new BarDetectionResult
{
Minimum = new MultiPoint
{
CameraPoint = cameraMinimum,
ColorPoint = colorMinimum,
DepthPoint = depthMinimum
},
Maximum = new MultiPoint
{
CameraPoint = cameraMaximum,
ColorPoint = colorMaximum,
DepthPoint = depthMaximum
},
Trail = new MultiPoint
{
CameraPoint = cameraTrail,
ColorPoint = colorTrail,
DepthPoint = depthTrail
},
BarHeight = barHeight + HEIGHT_DIFFERENCE,
BarLength = barLength,
Angle = angle
};
BarDetected?.Invoke(this, result);
}
}
}
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());
}
}
