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); }
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()); } }