internal static void CopyToFrameToDrawingContext(this HighDefinitionFaceFrame highDefinitionFaceFrame, DrawingContext context, bool useDepthSpace = true, byte bodyIndex = 1, double pointRadius = 2F) { var faceAlignment = new FaceAlignment(); var coordinateMapper = highDefinitionFaceFrame.HighDefinitionFaceFrameSource.KinectSensor.CoordinateMapper; var brush = BodyIndexColor.GetBrushFromBodyIndex(bodyIndex); highDefinitionFaceFrame.GetAndRefreshFaceAlignmentResult(faceAlignment); var faceModel = new FaceModel(); var vertices = faceModel.CalculateVerticesForAlignment(faceAlignment); if (vertices.Count > 0) { for (int index = 0; index < vertices.Count; index++) { CameraSpacePoint vertice = vertices[index]; DepthSpacePoint point = coordinateMapper.MapCameraPointToDepthSpace(vertice); if (float.IsInfinity(point.X) || float.IsInfinity(point.Y)) { return; } context.DrawEllipse(brush, null, point.GetPoint(), pointRadius, pointRadius); } } }
/// <summary> /// Maps the 3D point to its corresponding 2D point. /// </summary> /// <param name="position">The 3D space point.</param> /// <returns>The X, Y coordinates of the point.</returns> public Point GetPoint(CameraSpacePoint position) { Point point = new Point(); switch (Visualization) { case Visualization.Color: { ColorSpacePoint colorPoint = CoordinateMapper.MapCameraPointToColorSpace(position); point.X = float.IsInfinity(colorPoint.X) ? 0.0 : colorPoint.X; point.Y = float.IsInfinity(colorPoint.Y) ? 0.0 : colorPoint.Y; } break; case Visualization.Depth: case Visualization.Infrared: { DepthSpacePoint depthPoint = CoordinateMapper.MapCameraPointToDepthSpace(position); point.X = float.IsInfinity(depthPoint.X) ? 0.0 : depthPoint.X; point.Y = float.IsInfinity(depthPoint.Y) ? 0.0 : depthPoint.Y; } break; default: break; } return(point); }
public static void DrawPoint(this Canvas canvas, Joint joint) { if (joint.TrackingState == TrackingState.NotTracked) { return; } KinectSensor _sensor; _sensor = KinectSensor.GetDefault(); CameraSpacePoint cameraPoint = joint.Position; ColorSpacePoint colorPoint = _sensor.CoordinateMapper.MapCameraPointToColorSpace(cameraPoint); DepthSpacePoint depthPoint = _sensor.CoordinateMapper.MapCameraPointToDepthSpace(cameraPoint); Debug.WriteLine("cameraPointX", cameraPoint.X.ToString());//, colorPoint.X, depthPoint.X); Debug.WriteLine("colorPointX", colorPoint.X.ToString()); Debug.WriteLine("depthPointX", depthPoint.X.ToString()); joint = joint.ScaleTo(canvas.ActualWidth, canvas.ActualHeight); Ellipse ellipse = new Ellipse { Width = 20, Height = 20, Fill = new SolidColorBrush(Colors.LightBlue) }; Canvas.SetLeft(ellipse, joint.Position.X - ellipse.Width / 2); Canvas.SetTop(ellipse, joint.Position.Y - ellipse.Height / 2); canvas.Children.Add(ellipse); }
private void RenderFacePoints() { /// <summary> /// Tarea a realizar por alumno /// Renderizar nube de puntos faciales /// </summary> /// ///////////////////////////////////////////////////////////////////////////////////////////////// /// Ellipse ellipse; if (_faceModel == null) { return; } var vertices = _faceModel.CalculateVerticesForAlignment(_faceAlignment); bool P1 = false; if (vertices.Count > 0) { for (int i = 0; i < vertices.Count; i++) { DepthSpacePoint point = _sensor.CoordinateMapper.MapCameraPointToDepthSpace(vertices[i]); if (!(float.IsInfinity(point.X)) && !(float.IsInfinity(point.Y))) { ellipse = _points[i]; Canvas.SetLeft(ellipse, point.X); Canvas.SetTop(ellipse, point.Y); } } } }
/// <summary> /// use the cameras mapper function to convert X and y's camara coordinates to world coordinates /// </summary> /// <param name="points"></param> /// <param name="zCoordinates"></param> /// <returns></returns> public void ScreenToWorldCoordinates(double[][] points) { if (points != null) { DepthSpacePoint depthSpacePoint; CameraSpacePoint lutValue; double[] point; for (int i = 0; i < points.Length; i++) { point = points[i]; depthSpacePoint = new DepthSpacePoint { X = (float)point[0], Y = (float)point[1] }; // Find the lutValue for the given set of coordinates lutValue = mapper.MapDepthPointToCameraSpace(depthSpacePoint, (ushort)(point[2])); // Convert coordinates using the found lutValue points[i][0] = lutValue.X; points[i][1] = lutValue.Y; points[i][2] = lutValue.Z; } } }
/// <summary> /// Converts a Kinect DepthSpacePoint into a Xenko Vector2 /// </summary> /// <param name="depthSpacePoint">Kinect DepthSpacePoint</param> /// <returns>Xenko Vector2</returns> public static Vector2 ToVector2(DepthSpacePoint depthSpacePoint) { Vector2 v2 = new Vector2(); TypeConverter.ToVector2(ref depthSpacePoint, out v2); return(v2); }
/// <summary> /// Converts the specified CameraSpacePoint into a 2-D point. /// </summary> /// <param name="position">The CameraSpacePoint to convert.</param> /// <param name="visualization">The type of the conversion (color, depth, or infrared).</param> /// <param name="coordinateMapper">The CoordinateMapper to make the conversion.</param> /// <returns>The corresponding 2-D point.</returns> public static Point ToPoint(this CameraSpacePoint position, Visualization visualization, CoordinateMapper coordinateMapper) { Point point = new Point(); switch (visualization) { case Visualization.Color: { ColorSpacePoint colorPoint = coordinateMapper.MapCameraPointToColorSpace(position); point.X = (int)(float.IsInfinity(colorPoint.X) ? 0.0 : colorPoint.X); point.Y = (int)(float.IsInfinity(colorPoint.Y) ? 0.0 : colorPoint.Y); } break; case Visualization.Depth: case Visualization.Infrared: { DepthSpacePoint depthPoint = coordinateMapper.MapCameraPointToDepthSpace(position); point.X = (int)(float.IsInfinity(depthPoint.X) ? 0.0 : depthPoint.X); point.Y = (int)(float.IsInfinity(depthPoint.Y) ? 0.0 : depthPoint.Y); } break; default: break; } return(point); }
public Vector3 MapDepthPointToSpaceCoords(KinectInterop.SensorData sensorData, Vector2 depthPos, ushort depthVal) { Vector3 vPoint = Vector3.zero; if (coordMapper != null && depthPos != Vector2.zero) { DepthSpacePoint depthPoint = new DepthSpacePoint(); depthPoint.X = depthPos.x; depthPoint.Y = depthPos.y; DepthSpacePoint[] depthPoints = new DepthSpacePoint[1]; depthPoints[0] = depthPoint; ushort[] depthVals = new ushort[1]; depthVals[0] = depthVal; CameraSpacePoint[] camPoints = new CameraSpacePoint[1]; coordMapper.MapDepthPointsToCameraSpace(depthPoints, depthVals, camPoints); CameraSpacePoint camPoint = camPoints[0]; vPoint.x = camPoint.X; vPoint.y = camPoint.Y; vPoint.z = camPoint.Z; } return(vPoint); }
private void OnFaceReaderHighDefFrameArrived(object sender, HighDefinitionFaceFrameArrivedEventArgs e) { using (HighDefinitionFaceFrame frame = e.FrameReference.AcquireFrame()) { if (frame != null && frame.IsFaceTracked) { frame.GetAndRefreshFaceAlignmentResult(_faceAlignment); if (_faceModel != null && _sensor != null) { CameraSpacePoint[] cameraSpacePoints = _faceModel.CalculateVerticesForAlignment(_faceAlignment).ToArray(); DepthSpacePoint[] depthSpacePoints = new DepthSpacePoint[cameraSpacePoints.Length]; if (cameraSpacePoints.Length > 0) { _sensor.CoordinateMapper.MapCameraPointsToDepthSpace(cameraSpacePoints, depthSpacePoints); } _faceState.Points = depthSpacePoints.ConvertToPointF(); if (this.OnFaceChanged != null) { this.OnFaceChanged(sender, _faceState); } } } } }
public CameraSpacePoint GetCamSpacePointFromMousePoint(Point mousePt, SpaceMode spMode) { CameraSpacePoint camPtFromMousePt = default(CameraSpacePoint); if (!IsSnapshotTaken) { return(camPtFromMousePt); } try { Tuple <float, float> dimensions = KinectExtensions.FrameDimensions[spMode]; float x_temp = (float)(mousePt.X * dimensions.Item1 / wCanvas.ActualWidth); float y_temp = (float)(mousePt.Y * dimensions.Item2 / wCanvas.ActualHeight); DepthSpacePoint depPtFromMousePt = dCoordinatesInColorFrame[(int)(x_temp + 0.5f) + (int)(y_temp + 0.5f) * (int)dimensions.Item1]; if (depPtFromMousePt.X == float.NegativeInfinity || depPtFromMousePt.Y == float.NegativeInfinity) { return(default(CameraSpacePoint)); } ushort depth = wallDepthData[(int)depPtFromMousePt.X + (int)(depPtFromMousePt.Y) * (int)KinectExtensions.FrameDimensions[SpaceMode.Depth].Item1]; camPtFromMousePt = wallMapper.MapDepthPointToCameraSpace(depPtFromMousePt, depth); } catch (Exception ex) { } return(camPtFromMousePt); }
/// <summary> /// Calculates the angle between the specified points. /// </summary> /// <param name="center">The center of the angle.</param> /// <param name="start">The start of the angle.</param> /// <param name="end">The end of the angle.</param> /// <returns>The angle, in degrees.</returns> public static double Angle(this DepthSpacePoint center, DepthSpacePoint start, DepthSpacePoint end) { Vector3 first = start.ToVector3() - center.ToVector3(); Vector3 second = end.ToVector3() - center.ToVector3(); return(Vector3.Angle(first, second)); }
private void _drawSurface(DepthSpacePoint a, DepthSpacePoint b, DepthSpacePoint c, DepthSpacePoint d) { _drawLine(a, b); _drawLine(b, c); _drawLine(c, d); _drawLine(d, a); }
public Vector2 MapSpacePointToDepthCoords(KinectInterop.SensorData sensorData, Vector3 spacePos) { Vector2 vPoint = Vector2.zero; if (coordMapper != null) { CameraSpacePoint camPoint = new CameraSpacePoint(); camPoint.X = spacePos.x; camPoint.Y = spacePos.y; camPoint.Z = spacePos.z; CameraSpacePoint[] camPoints = new CameraSpacePoint[1]; camPoints[0] = camPoint; DepthSpacePoint[] depthPoints = new DepthSpacePoint[1]; coordMapper.MapCameraPointsToDepthSpace(camPoints, depthPoints); DepthSpacePoint depthPoint = depthPoints[0]; if (depthPoint.X >= 0 && depthPoint.X < sensorData.depthImageWidth && depthPoint.Y >= 0 && depthPoint.Y < sensorData.depthImageHeight) { vPoint.x = depthPoint.X; vPoint.y = depthPoint.Y; } } return(vPoint); }
/// <summary> ///把相机空间里的点映射到指定的彩色或深度空间 /// </summary> /// <param name="bodyJoints"></param> /// <param name="isMapColor"></param> /// <returns></returns> public Dictionary <JointType, Joint2D> JointToJoint2Ds(IReadOnlyDictionary <JointType, Joint> bodyJoints, bool isMapColor = true) { //将关节点转换为2D深度(显示)空间 Dictionary <JointType, Joint2D> joints2 = new Dictionary <JointType, Joint2D>(); foreach (KeyValuePair <JointType, Joint> pair in bodyJoints) { //有时,推断关节的深度(Z)可能显示为负数 CameraSpacePoint position = pair.Value.Position; if (position.Z < 0) //深度为负值时 { position.Z = InferredZPositionClamp; } // 将点从相机空间映射到颜色空间。 ColorSpacePoint colorSpacePoint = CoordinateMapper.MapCameraPointToColorSpace(position); // 将点从相机空间映射到深度空间。 DepthSpacePoint depthSpacePoint = CoordinateMapper.MapCameraPointToDepthSpace(position); Point point = new Point { X = isMapColor ? colorSpacePoint.X : depthSpacePoint.X, Y = isMapColor ? colorSpacePoint.Y : depthSpacePoint.Y }; joints2[pair.Key] = new Joint2D() { Joint2DType = pair.Key, Position = point, TrackingState = pair.Value.TrackingState }; } return(joints2); }
/// <summary>ボディ情報を指定されたDrawingContextに書き込む</summary> private void DrawToDrawingContext(DrawingContext dc, Body body) { // Draw a transparent background to set the render size dc.DrawRectangle(Brushes.Black, null, new Rect(0.0, 0.0, kinectConnector.DisplayWidth, kinectConnector.DisplayHeight)); if (!body.IsTracked) { return; } this.DrawClippedEdges(body, dc); IReadOnlyDictionary <JointType, Joint> joints = body.Joints; // convert the joint points to depth (display) space Dictionary <JointType, Point> jointPoints = new Dictionary <JointType, Point>(); foreach (JointType jointType in joints.Keys) { //最小値をInferredZPositionClampでおさえることでマッパーがInfinityを吐くのを防止 CameraSpacePoint position = joints[jointType].Position; position.Z = Math.Max(position.Z, InferredZPositionClamp); DepthSpacePoint depthSpacePoint = kinectConnector.CoordinateMapper.MapCameraPointToDepthSpace(position); jointPoints[jointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y); } //ボーン DrawBones(joints, jointPoints, dc); //関節 DrawJoints(joints, jointPoints, dc); //両手 DrawHand(body.HandLeftState, jointPoints[JointType.HandLeft], dc); DrawHand(body.HandRightState, jointPoints[JointType.HandRight], dc); }
/// <summary> /// Draws one bone of a body (joint to joint) /// </summary> /// <param name="joints">joints to draw</param> /// <param name="jointPoints">translated positions of joints to draw</param> /// <param name="jointType0">first joint of bone to draw</param> /// <param name="jointType1">second joint of bone to draw</param> /// <param name="drawingContext">drawing context to draw to</param> /// /// <param name="drawingPen">specifies color to draw a specific bone</param> private void DrawBone(IReadOnlyDictionary <JointType, Joint> joints, IDictionary <JointType, CameraSpacePoint> jointPoints, JointType jointType0, JointType jointType1, DrawingContext drawingContext, Pen drawingPen) { Joint joint0 = joints[jointType0]; Joint joint1 = joints[jointType1]; // If we can't find either of these joints, exit if (joint0.TrackingState == TrackingState.NotTracked || joint1.TrackingState == TrackingState.NotTracked) { return; } // We assume all drawn bones are inferred unless BOTH joints are tracked Pen drawPen = this.inferredBonePen; if ((joint0.TrackingState == TrackingState.Tracked) && (joint1.TrackingState == TrackingState.Tracked)) { drawPen = drawingPen; } DepthSpacePoint item1 = this.coordinateMapper.MapCameraPointToDepthSpace(jointPoints[jointType0]); DepthSpacePoint item2 = this.coordinateMapper.MapCameraPointToDepthSpace(jointPoints[jointType1]); drawingContext.DrawLine(drawPen, new Point(item1.X, item1.Y), new Point(item2.X, item2.Y)); }
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; }
private async Task PlayFrames(CancellationToken cancelToken) { long timestamp; while ((line = stream.ReadLine()) != null) { timestamp = long.Parse(line.Split(';').Take(1).ToList()[0]); if (prevTimestamp > 0) { await Task.Delay((int)((timestamp - prevTimestamp) / speedFactor)); } //await Task.Delay(30); prevTimestamp = timestamp; foreach (var token in line.Split(';').Skip(1)) { DepthSpacePoint mapped = MapTokenToPoint(token); if (float.IsInfinity(mapped.X) || float.IsInfinity(mapped.Y) || mapped.X < 0 || mapped.Y < 0) { continue; } mainWindow.DisplayPoint(point, mapped); } cancelToken.ThrowIfCancellationRequested(); } }
private void DetectPageTurn(DepthSpacePoint dsp, Shape circle) { if (lastMarker != null) { bodyCanvas.Children.Remove(lastMarker); } if (detected == 0 && (dsp.Y < bodyCanvas.Height / 2) && (dsp.X < bodyCanvas.Width)) { lastPoint = dsp; detected = 1; lastMarker = circle; tDetected.Content = ""; bodyCanvas.Children.Add(circle); return; } if (detected > 0 && dsp.Y < (bodyCanvas.Height / 2) && (dsp.X < bodyCanvas.Width) && dsp.Y > lastPoint.Y + (bodyCanvas.Height / 32)) { detected++; lastMarker = circle; bodyCanvas.Children.Add(circle); } else { detected = 0; } if (detected > 3) { tDetected.Content = "Page Turn Detected"; GoToNextPage(); detected = 0; } }
/// <summary> /// Computes the face result text position by adding an offset to the corresponding /// body's head joint in camera space and then by projecting it to screen space /// </summary> /// <param name="faceIndex">the index of the face frame corresponding to a specific body in the FOV</param> /// <param name="faceTextLayout">the text layout position in screen space</param> /// <returns>success or failure</returns> private bool GetFaceTextPositionInColorSpace(int faceIndex, out Point faceTextLayout) { faceTextLayout = new Point(); bool isLayoutValid = false; Body body = this.bodies[faceIndex]; if (body.IsTracked) { var headJoint = body.Joints[JointType.Head].Position; CameraSpacePoint textPoint = new CameraSpacePoint() { X = headJoint.X + TextLayoutOffsetX, Y = headJoint.Y + TextLayoutOffsetY, Z = headJoint.Z }; //ColorSpacePoint textPointInColor = this.coordinateMapper.MapCameraPointToColorSpace(textPoint); //faceTextLayout.X = textPointInColor.X; //faceTextLayout.Y = textPointInColor.Y; DepthSpacePoint depthSpacePoint = this.coordinateMapper.MapCameraPointToDepthSpace(textPoint); faceTextLayout.X = depthSpacePoint.X; faceTextLayout.Y = depthSpacePoint.Y; isLayoutValid = true; } return(isLayoutValid); }
public Vector2 MapDepthPointToColorCoords(KinectInterop.SensorData sensorData, Vector2 depthPos, ushort depthVal) { Vector2 vPoint = Vector2.zero; if (coordMapper != null && depthPos != Vector2.zero) { DepthSpacePoint depthPoint = new DepthSpacePoint(); depthPoint.X = depthPos.x; depthPoint.Y = depthPos.y; DepthSpacePoint[] depthPoints = new DepthSpacePoint[1]; depthPoints[0] = depthPoint; ushort[] depthVals = new ushort[1]; depthVals[0] = depthVal; ColorSpacePoint[] colPoints = new ColorSpacePoint[1]; coordMapper.MapDepthPointsToColorSpace(depthPoints, depthVals, colPoints); ColorSpacePoint colPoint = colPoints[0]; vPoint.x = colPoint.X; vPoint.y = colPoint.Y; } return(vPoint); }
public void Draw(Body b, clsKinect ck, DrawingContext drawingContext) { /* foreach (JointType jointType in b.Joints.Keys) * { * DepthSpacePoint p = Point2D[jointType]; * // Brush drawBrush = new SolidColorBrush(Color.FromArgb(128, 0, 0, 255)); * drawingContext.DrawEllipse(Brushes.DeepPink, null, new System.Windows.Point(p.X, p.Y), 10, 10); * } */ DepthSpacePoint p = Point2D[JointType.HandRight]; drawingContext.DrawEllipse(Brushes.Blue, null, new System.Windows.Point(p.X, p.Y), 10, 10); p = Point2D[JointType.HandLeft]; drawingContext.DrawEllipse(Brushes.Blue, null, new System.Windows.Point(p.X, p.Y), 10, 10); Pen drawPen = new Pen(Brushes.Black, 5); foreach (clsLine line in SkeletonLines) { double x = Point2D[line.sp].X; double y = Point2D[line.sp].Y; Point a = new Point(x, y); x = Point2D[line.ep].X; y = Point2D[line.ep].Y; Point bb = new Point(x, y); drawingContext.DrawLine(drawPen, a, bb); } }
// --- body private void ProcessBodyData(MultiFrame multiFrame) { // NOTES (!!) // we assume there is only one body // we assume joints are always in the same order // 12 values per joint -> trackingStatus, posX, posY, posZ, depthX, depthY, colorX, colorY, orientX, orientY, orientZ, orientW // joint trackingstate enum: tracked = 2, nottracked = 0, inffered = 1? Body[] bodies = multiFrame.Bodies; Body body = null; foreach (var b in bodies) { if (b.IsTracked) { body = b; break; } } if (body == null) { multiFrame.BodyData = new float[25 * 12]; // return zeros if not found return; } int jointsCount = body.Joints.Count; float[] bodyData = new float[jointsCount * 12]; int idx = 0; foreach (var kvp in body.Joints) { var jointType = kvp.Key; var joint = kvp.Value; var jointOrientation = body.JointOrientations[jointType]; CameraSpacePoint position = joint.Position; if (position.Z < 0) { position.Z = 0.1f; // according to Kinect code sample (sometimes z < 0 and the mapping return -infinity) } DepthSpacePoint depthSpacePoint = coordMapper.MapCameraPointToDepthSpace(position); ColorSpacePoint colorSpacePoint = coordMapper.MapCameraPointToColorSpace(position); bodyData[idx++] = (int)joint.TrackingState; bodyData[idx++] = joint.Position.X; bodyData[idx++] = joint.Position.Y; bodyData[idx++] = joint.Position.Z; bodyData[idx++] = depthSpacePoint.X; bodyData[idx++] = depthSpacePoint.Y; bodyData[idx++] = colorSpacePoint.X; bodyData[idx++] = colorSpacePoint.Y; bodyData[idx++] = jointOrientation.Orientation.X; bodyData[idx++] = jointOrientation.Orientation.Y; bodyData[idx++] = jointOrientation.Orientation.Z; bodyData[idx++] = jointOrientation.Orientation.W; } multiFrame.BodyData = bodyData; }
public static Tuple <Point, Point> printEndProjection(Body body, double endAngle) { double accuteAngle; DepthSpacePoint pointInDepthspace = KinectSensor.GetDefault().CoordinateMapper.MapCameraPointToDepthSpace(body.Joints[JointType.ShoulderRight].Position); double linelenght = ((KinectSensor.GetDefault().CoordinateMapper.MapCameraPointToDepthSpace(body.Joints[JointType.SpineShoulder].Position).Y) - (KinectSensor.GetDefault().CoordinateMapper.MapCameraPointToDepthSpace(body.Joints[JointType.SpineBase].Position).Y)); // if (endAngle <= 180) // { accuteAngle = endAngle - 90.0; double projX = pointInDepthspace.X - linelenght * Math.Sin((Math.PI * accuteAngle) / 180); double projY = pointInDepthspace.Y + linelenght * Math.Cos((Math.PI * accuteAngle) / 180); // } // else // { // accuteAngle = endAngle - 180.0; // double projX = pointInDepthspace.X - linelenght * Math.Cos((Math.PI * accuteAngle) / 180); // double projY = pointInDepthspace.Y + linelenght * Math.Sin((Math.PI * accuteAngle) / 180); // } Point start = new Point(pointInDepthspace.X, pointInDepthspace.Y); Point end = new Point(projX, projY); return(Tuple.Create(start, end)); }
// Returns color space point private Dictionary <JointType, Point> GetJointsPoints(IReadOnlyDictionary <JointType, Kinect2KitJoint> joints) { Dictionary <JointType, Point> jointPoints = new Dictionary <JointType, Point>(); foreach (JointType jointType in joints.Keys) { CameraSpacePoint position = joints[jointType].CameraSpacePoint; if (position.Z < 0) { position.Z = 0.1f; } Point point = new Point(); DepthSpacePoint depthSpacePoint = this.coordinateMapper.MapCameraPointToDepthSpace(position); ColorSpacePoint colorSpacePoint = this.coordinateMapper.MapCameraPointToColorSpace(position); point.X = float.IsInfinity(colorSpacePoint.X) ? 0 : colorSpacePoint.X; point.Y = float.IsInfinity(colorSpacePoint.Y) ? 0 : colorSpacePoint.Y; if (point.X == 0 && point.Y == 0) { continue; } jointPoints[jointType] = point; } return(jointPoints); }
/// <summary> /// 深度画像座標からワールド空間座標へ変換するやつ /// </summary> /// <param name="depthSpacePoint"></param> /// <param name="depth"></param> /// <returns></returns> public CameraSpacePoint MapDepthPointToCameraSpace(DepthSpacePoint depthSpacePoint, ushort depth) { float distance = (float)depth / 1000; int index = (int)(this.depthWidth * depthSpacePoint.Y + depthSpacePoint.X); PointF cameraPoint = this.depthFrameToCameraSpaceTable[index]; return new CameraSpacePoint() { X = cameraPoint.X * distance, Y = cameraPoint.Y * distance, Z = distance }; }
public void DrawFrame(Body body, CoordinateMapper mapper, Rect depthFrameSize) #endif { if (Constants.bPaused == true) { this.Resize(); foreach (var jointType in interestedJointTypes) { var joint = body.Joints[jointType]; if (joint.TrackingState != TrackingState.NotTracked) { var cameraPosition = joint.Position; DepthSpacePoint depthPosition = mapper.MapCameraPointToDepthSpace(cameraPosition); if (!float.IsNegativeInfinity(depthPosition.X)) { var consolePosition = MapDepthPointToConsoleSpace(depthPosition, depthFrameSize); // Ajust index if (iTrack > iMax) { iTrack = 0; } // Populate holders iXY[iTrack] = consolePosition.Item1; iXY[iTrack + 1] = consolePosition.Item2; String strPrintout = (iTrack == 0 ? "LeftHand" : "RightHand"); int iCode = getCode(iXY); iTrack += 2; // draw lines - MAKES DISPLAY FLICKER TOO MUCH // DrawLines(); // swapped jointType.ToString().Substring(0,1) for strPrintout ConsoleEx.DrawAt( consolePosition.Item1, consolePosition.Item2, strPrintout, joint.TrackingState == TrackingState.Inferred ? ConsoleColor.Black : ConsoleColor.Black); // draw code ConsoleEx.DrawAt(1, 1, "Code Left = " + iXY[idxLX].ToString() + ", " + iXY[idxLY].ToString(), ConsoleColor.Black); ConsoleEx.DrawAt(1, 2, "Code Right = " + iXY[idxRX].ToString() + ", " + iXY[idxRY].ToString(), ConsoleColor.Black); ConsoleEx.DrawAt(1, 3, "CODE = " + iCode.ToString(), ConsoleColor.Black); PrintStack(iCode, iXY[idxLX], iXY[idxLY], iXY[idxRX], iXY[idxRY]); // draw border lines // Console.BackgroundColor = ConsoleColor.DarkRed; } } } } #if ZERO if (isMain) { DumpMainBodyInfo(body); } #endif }
private void color_MouseLeftButtonDown(object sender, MouseEventArgs e) { if (sender != null) { Point cPoint = e.GetPosition(colorImage); DepthSpacePoint dPoint = this.colorMappedToDepthPoints[(int)cPoint.X * 3 + (int)cPoint.Y * 3 * (this.colorFrameDescription.Width)]; if (!float.IsNegativeInfinity((float)dPoint.X)) { Double depth = this.depthData[(int)(dPoint.X) + (int)(dPoint.Y) * depthFrameDescription.Width]; depth = depth / 1000.00; DistanceText.Text = depth.ToString() + " meters"; } else { DistanceText.Text = "Unkown Depth"; } Coordinates.Text = "X: " + cPoint.X * 3 + " Y: " + cPoint.Y * 3; CoordinatesInColor.Text = "X: " + cPoint.X * 3 + " Y: " + cPoint.Y * 3; Ellipse ellipse = new Ellipse(); ellipse.Width = 5; ellipse.Height = 5; ellipse.Fill = System.Windows.Media.Brushes.Orange; Canvas.SetLeft(ellipse, cPoint.X); Canvas.SetTop(ellipse, cPoint.Y); cameraCanvas.Children.Add(ellipse); } }
private DepthSpacePoint CalculateJointPositions(Body body, JointType jointType) { Windows.Kinect.Joint joint = body.Joints [jointType]; CameraSpacePoint position = joint.Position; // sometimes the depth(Z) of an inferred joint may show as negative // clamp down to 0.1f to prevent coordinatemapper from returning (-Infinity, -Infinity) if (position.Z < 0) { position.Z = 0.1f; } if (joint.TrackingState == TrackingState.Tracked) { switch (jointType) { case JointType.HandRight: rightHandPos = _Mapper.MapCameraPointToDepthSpace(position); PerformeGesture(body.HandRightState, rightHandMover); break; case JointType.HandLeft: PerformeGesture(body.HandLeftState, leftHandMover); break; } } return(_Mapper.MapCameraPointToDepthSpace(position)); }
public BackArea(KinectSensor kinectSensor, IReadOnlyDictionary <JointType, Joint> joints) { var leftShoulder = joints[JointType.ShoulderLeft].Position; var rightShoulder = joints[JointType.ShoulderRight].Position; var spineBase = joints[JointType.SpineBase].Position; var spineShoulder = joints[JointType.SpineShoulder].Position; var neck = joints[JointType.Head].Position; var backPoints2D = new DepthSpacePoint[5]; kinectSensor.CoordinateMapper.MapCameraPointsToDepthSpace(new CameraSpacePoint[5] { leftShoulder, rightShoulder, spineBase, spineShoulder, neck }, backPoints2D); leftShoulder2D = depthSpacePointToPoint(backPoints2D[0]); rightShoulder2D = depthSpacePointToPoint(backPoints2D[1]); spineBase2D = depthSpacePointToPoint(backPoints2D[2]); spineShoulder2D = depthSpacePointToPoint(backPoints2D[3]); neck2D = depthSpacePointToPoint(backPoints2D[4]); var spineLine = Line2D.makeLine(spineBase2D, spineShoulder2D); leftShoulderLine = spineLine.makeParalelLine(leftShoulder2D); rightShoulderLine = spineLine.makeParalelLine(rightShoulder2D); hipLine = spineLine.makePerpendicularLine(new Point(spineBase2D.X, spineBase2D.Y - 0.15 * (spineShoulder2D.Y - spineBase2D.Y))); neckLine = spineLine.makePerpendicularLine(neck2D); spineHipPosition = hipLine.GetPointPosition(spineShoulder2D); spineNeckPosition = neckLine.GetPointPosition(spineShoulder2D); }
private Point ScaleTo(Joint joint) { //3D space point CameraSpacePoint jointPosition = joint.Position; //2D sapce point Point point = new Point(); if (mode == CameraMode.Color) { ColorSpacePoint colorPoint = sensor.CoordinateMapper.MapCameraPointToColorSpace(jointPosition); point.X = float.IsInfinity(colorPoint.X) ? 0 : colorPoint.X; point.Y = float.IsInfinity(colorPoint.Y) ? 0 : colorPoint.Y; } else if (mode == CameraMode.Depth || mode == CameraMode.Infrared) { DepthSpacePoint depthPoint = sensor.CoordinateMapper.MapCameraPointToDepthSpace(jointPosition); point.X = float.IsInfinity(depthPoint.X) ? 0 : depthPoint.X; point.Y = float.IsInfinity(depthPoint.Y) ? 0 : depthPoint.Y; } //Scale down the points to current canvas size point.X = point.X * canvas.ActualWidth / 1920; point.Y = point.Y * canvas.ActualHeight / 1080; return(point); }
/// <summary> /// Create a new <c>CustomJoint</c> object based on the supplied /// <c>JointType</c> value. /// </summary> public CustomJoint(JointType type) { _jointType = type; _position = new CameraSpacePoint(); _depthPosition = new DepthSpacePoint(); _colorPosition = new ColorSpacePoint(); _trackingState = TrackingState.NotTracked; }
/// <summary> /// 深度画像座標からカラー画像座標へ変換するやつ /// </summary> /// <param name="depthSpacePoint"></param> /// <param name="depth"></param> /// <returns></returns> public ColorSpacePoint MapDepthPointToColorSpace(DepthSpacePoint depthSpacePoint, ushort depth) { int index = (int)(this.depthWidth * depthSpacePoint.Y + depthSpacePoint.X); PointF lut = this.depthFrameToColorSpacfeTable[index]; ColorSpacePoint tempP = new ColorSpacePoint(); tempP.Y = lut.Y; tempP.X = lut.X + (float)(this.D / depth); return tempP; }
void depthFrameReader_FrameArrived(object sender, DepthFrameArrivedEventArgs e) { var depthFrame = e.FrameReference.AcquireFrame(); if (depthFrame != null) { using (depthFrame) { depthFrame.CopyFrameDataToIntPtr(depthImage.DataIntPtr, Kinect2Calibration.depthImageWidth*Kinect2Calibration.depthImageHeight*2); // convert depth image coords to color image coords int x = 100, y = 100; ushort depthImageValue = depthImage[x, y]; // depth image values are in mm if (depthImageValue == 0) { Console.WriteLine("Sorry, depth value input coordinates is zero"); return; } float depth = (float)depthImageValue / 1000f; // convert to m, to match our calibration and the rest of the Kinect SDK double colorX, colorY; calibration.DepthImageToColorImage(x, y, depth, out colorX, out colorY); //// when converting many points, it may be faster to precompute pass in the distortion table: //var depthFrameToCameraSpaceTable = calibration.ComputeDepthFrameToCameraSpaceTable(); //calibration.DepthImageToColorImage(x, y, depth, depthFrameToCameraSpaceTable, out colorX, out colorY); Console.WriteLine("our color coordinates: {0} {1}", colorX, colorY); // compare to Kinect SDK var depthSpacePoint = new DepthSpacePoint(); depthSpacePoint.X = x; depthSpacePoint.Y = y; var colorSpacePoint = kinectSensor.CoordinateMapper.MapDepthPointToColorSpace(depthSpacePoint, depthImageValue); Console.WriteLine("SDK's color coordinates: {0} {1}", colorSpacePoint.X, colorSpacePoint.Y); // convert back to depth image Matrix depthPoint; double depthX, depthY; calibration.ColorImageToDepthImage(colorX, colorY, depthImage, out depthPoint, out depthX, out depthY); //// when converting many points, it may be faster to precompute and pass in the distortion table: //var colorFrameToCameraSapceTable = calibration.ComputeColorFrameToCameraSpaceTable(); //calibration.ColorImageToDepthImage((int)colorX, (int)colorY, depthImage, colorFrameToCameraSapceTable, out depthPoint, out depthX, out depthY); Console.WriteLine("convert back to depth: {0} {1}", depthX, depthY); } } }
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); }
private static CalibrationRecord Calibrate(KinectSensor sensor) { int width = sensor.DepthFrameSource.FrameDescription.Width; int height = sensor.DepthFrameSource.FrameDescription.Height; ushort minDepth = sensor.DepthFrameSource.DepthMinReliableDistance; ushort maxDepth = sensor.DepthFrameSource.DepthMaxReliableDistance; var result = new CalibrationRecord(); int nextDepth = minDepth; int depthIncrement = 777; if (depthIncrement >= maxDepth - minDepth || (maxDepth - minDepth) % depthIncrement == 0) throw new ArgumentException("Pick an increment which is less than, and not divisible by, maxDepth - minDepth"); // 0 to 512 for (int depthX = 0; depthX < width; depthX += 3) { // 0 to 424 for (int depthY = 0; depthY < height; depthY += 3) { // 500 to 4500 DepthSpacePoint depthPoint = new DepthSpacePoint { X = depthX, Y = depthY }; ColorSpacePoint colorPoint = sensor.CoordinateMapper.MapDepthPointToColorSpace(depthPoint, (ushort)nextDepth); CameraSpacePoint bodyPoint = sensor.CoordinateMapper.MapDepthPointToCameraSpace(depthPoint, (ushort)nextDepth); CalibrationPoint cpoint = new CalibrationPoint() { DepthPoint = depthPoint, Depth = (ushort)nextDepth, CameraPoint = bodyPoint, ColorPoint = colorPoint }; result.AddCalibrationPoint(cpoint); nextDepth += depthIncrement; if (nextDepth >= maxDepth) nextDepth -= maxDepth - minDepth; } } return result; }
/// <summary> /// Updates the position of the cursor. /// </summary> /// <param name="point">The specified point in the depth space.</param> /// <param name="ratioX">The specified horizontal scale scale ratio.</param> /// <param name="ratioY">The specified vertical scale ratio.</param> public void Update(DepthSpacePoint point, double ratioX = 1.0, double ratioY = 1.0) { Update(point.X * ratioX, point.Y * ratioY); }
/// <summary> /// Update the joint position based on the referenced <c>IJoint</c>. /// </summary> public virtual void Update(IJoint joint) { if (this.JointType != joint.JointType) throw new Exception("Cannot Update with Joint of a different Type"); _position = joint.Position; _depthPosition = new DepthSpacePoint(); _colorPosition = new ColorSpacePoint(); _trackingState = joint.TrackingState; }
private void RecordData(RectI faceRegion, FaceFrame faceFrame) { //record the R, G, B, IR values from the Face Region pixels using (var irFrame = faceFrame.InfraredFrameReference.AcquireFrame()) { using (var depthFrame = faceFrame.DepthFrameReference.AcquireFrame()) { using (var colorFrame = faceFrame.ColorFrameReference.AcquireFrame()) { if ((null == irFrame) || (null == colorFrame) || (null == depthFrame)) return; DepthSpacePoint[] depthSpacePoints = new DepthSpacePoint[colorFrame.FrameDescription.Height * colorFrame.FrameDescription.Width]; FrameDescription depthFrameDescription = depthFrame.FrameDescription; // Access the depth frame data directly via LockImageBuffer to avoid making a copy using (KinectBuffer depthFrameData = depthFrame.LockImageBuffer()) { this.m_CoordMapper.MapColorFrameToDepthSpaceUsingIntPtr( depthFrameData.UnderlyingBuffer, depthFrameData.Size, depthSpacePoints); } //Get the pixels m_colorBitmap.Lock(); unsafe { byte* pixels = (byte*)m_colorBitmap.BackBuffer; var startPixel = faceRegion.Left * faceRegion.Top; var endPixel = faceRegion.Right * faceRegion.Bottom; float alpha = 0; float red = 0; float green = 0; float blue = 0; float ir = 0; ushort[] irFrameData = new ushort[irFrame.FrameDescription.Height * irFrame.FrameDescription.Width]; irFrame.CopyFrameDataToArray(irFrameData); //Now get the Red, Green, Blue Pixels for the region for (int i = startPixel; i < endPixel; i += 4) { //var pixel = pixels[i]; int irIndex = (int)depthSpacePoints[i].X * (int)depthSpacePoints[i].Y; blue += pixels[i]; // << 24; green += pixels[i + 1]; // << 16; red += pixels[i + 2];// << 8; alpha += pixels[i + 3]; if (irIndex < irFrameData.Length) ir += irFrameData[irIndex]; else ir += 0; } float size = Math.Abs(startPixel - endPixel); float avg_alpha = alpha / size; float avg_red = red / size; float avg_green = green / size; float avg_blue = blue / size; float avg_ir = ir / size; double std_alpha = 0; double std_red = 0; double std_green = 0; double std_blue = 0; double std_ir = 0; double var_alpha = 0; double var_red = 0; double var_green = 0; double var_blue = 0; double var_ir = 0; //Now calculate standard deviation for (int i = startPixel; i < endPixel; i += 4) { //var pixel = pixels[i]; var_blue = (double)(pixels[i] - avg_blue); std_blue += Math.Pow(var_blue, 2.0); var_green = (pixels[i + 1] - avg_green); std_green += Math.Pow(var_green, 2); var_red = (pixels[i + 2] - avg_red); std_red += Math.Pow(var_red, 2); var_alpha = (pixels[i + 3] - avg_alpha); std_alpha += Math.Pow(var_alpha, 2); int irIndex = (int)depthSpacePoints[i].X * (int)depthSpacePoints[i].Y; if (irIndex < irFrameData.Length) var_ir = irFrameData[irIndex] - avg_ir; else var_ir = avg_ir; std_ir += Math.Pow(var_ir, 2); } std_alpha = Math.Sqrt(std_alpha / size); std_red = Math.Sqrt(std_red / size); std_green = Math.Sqrt(std_green / size); std_blue = Math.Sqrt(std_blue / size); std_ir = Math.Sqrt(std_ir / size); double prime_alpha = 0; double prime_red = 0; double prime_green = 0; double prime_blue = 0; double prime_ir = 0; //Now calculate standard deviation for (int i = startPixel; i < endPixel; i += 4) { //var pixel = pixels[i]; var_blue = (double)(pixels[i] - avg_blue); prime_blue += var_blue / std_blue; var_green = (pixels[i + 1] - avg_green); prime_green += var_green / std_green; var_red = (pixels[i + 2] - avg_red); prime_red += var_red / std_red; var_alpha = (pixels[i + 3] - avg_alpha); prime_alpha += var_alpha / std_alpha; int irIndex = (int)depthSpacePoints[i].X * (int)depthSpacePoints[i].Y; if (irIndex < irFrameData.Length) var_ir = irFrameData[irIndex] - avg_ir; else var_ir = avg_ir; prime_ir += var_ir / std_ir; } double norm_alpha = prime_alpha / size; double norm_red = prime_red / size; double norm_blue = prime_blue / size; double norm_green = prime_green / size; double norm_ir = prime_ir / size; Plot(norm_red, norm_green, norm_blue, norm_ir); jadeCalculation.Write( m_secondsElapsed.ElapsedMilliseconds, norm_alpha, norm_red, norm_green, norm_blue, norm_ir); } m_colorBitmap.Unlock(); } } } }
void Reader_MultiSourceFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e) { var reference = e.FrameReference.AcquireFrame(); // Infrared using (var infraFrame = reference.InfraredFrameReference.AcquireFrame()) { if (infraFrame != null) { RenderInfraredPixels(infraFrame); } } // Color using (var colorFrame = reference.ColorFrameReference.AcquireFrame()) { // Depth using (var depthFrame = reference.DepthFrameReference.AcquireFrame()) { if (colorFrame != null && depthFrame != null) { var _colorWidth = colorFrame.ColorFrameSource.FrameDescription.Width; var _colorHeight = colorFrame.ColorFrameSource.FrameDescription.Height; var _depthWidth = depthFrame.DepthFrameSource.FrameDescription.Width; var _depthHeight = depthFrame.DepthFrameSource.FrameDescription.Height; using (Microsoft.Kinect.KinectBuffer depthBuffer = depthFrame.LockImageBuffer()) { // verify data and write the color data to the display bitmap if (((this.depthFrameDescription.Width * this.depthFrameDescription.Height) == (depthBuffer.Size / this.depthFrameDescription.BytesPerPixel)) && (this.depthFrameDescription.Width == this.depthBitmap.PixelWidth) && (this.depthFrameDescription.Height == this.depthBitmap.PixelHeight)) { // Note: In order to see the full range of depth (including the less reliable far field depth) // we are setting maxDepth to the extreme potential depth threshold ushort maxDepth = ushort.MaxValue; // If you wish to filter by reliable depth distance, uncomment the following line: //// maxDepth = depthFrame.DepthMaxReliableDistance this.ProcessDepthFrameData(depthBuffer.UnderlyingBuffer, depthBuffer.Size, depthFrame.DepthMinReliableDistance, maxDepth); this.RenderDepthPixels(); } } using (KinectBuffer colorBuffer = colorFrame.LockRawImageBuffer()) { this.colorBitmap.Lock(); // verify data and write the new color frame data to the display bitmap if ((_colorWidth == this.colorBitmap.PixelWidth) && (_colorHeight == this.colorBitmap.PixelHeight)) { colorFrame.CopyConvertedFrameDataToIntPtr( this.colorBitmap.BackBuffer, (uint)(_colorWidth * _colorHeight * 4), ColorImageFormat.Bgra); this.colorBitmap.AddDirtyRect(new Int32Rect(0, 0, this.colorBitmap.PixelWidth, this.colorBitmap.PixelHeight)); } this.colorBitmap.Unlock(); } if ((takeScreenshot || dumpPpms) && !robot.IsMoving()) { ushort[] depths = new ushort[_depthHeight * _depthWidth]; DepthSpacePoint[] mappedColor = new DepthSpacePoint[_colorHeight * _colorWidth]; depthFrame.CopyFrameDataToArray(depths); cm.MapColorFrameToDepthSpace(depths, mappedColor); byte[] colors = new byte[_colorHeight * _colorWidth * 4]; //this is the byte array that is converted into a ppm in the end, make it rgba form colorFrame.CopyConvertedFrameDataToArray(colors, ColorImageFormat.Rgba); this.mappedColor = mappedColor; this.depths = depths; this.colors = colors; if (takeScreenshot) { ScreenshotSaveFile(); takeScreenshot = capturePanorama || false; } else if (dumpPpms) { ScreenshotSaveFile(); //DumpPpms(); dumpPpms = false; } // Kick off another rotation if capturing a panorama if (capturePanorama) { if (numRotations < MAX_ROTATIONS) { numRotations++; RotateCW(); StopMoving(rotateTime); Thread.Sleep(STABILIZE_TIME); } else { this.capturePanorama = false; this.takeScreenshot = false; this.panoramaNum++; this.imageNum = 0; this.numRotations = 0; } } } depthCamera.Source = this.depthBitmap; colorCamera.Source = this.colorBitmap; infraCamera.Source = this.infraBitmap; } } } }
private void depthFrameReader_FrameArrived(object sender, DepthFrameArrivedEventArgs e) { using (DepthFrame depthFrame = e.FrameReference.AcquireFrame()) { if (depthFrame != null) { using (KinectBuffer depthBuffer = depthFrame.LockImageBuffer()) { if ((this.depthFrameDescription.Width * this.depthFrameDescription.Height) == (depthBuffer.Size / this.depthFrameDescription.BytesPerPixel)) { //Frame handler if (btn_record.Content.Equals("Stop")) { lbl_fpsDepth.Content = Convert.ToUInt64(lbl_fpsBody.Content) + 1; depthFrameSelector++; } bool record = depthFrameSelector == depthFrameThreshold; if (record) depthFrameSelector = 0; depthFrameSelector++; if (depthFrameSelector != depthFrameThreshold) return; //########### LÖSCHEN (nut für test) int frame = windowSize / 2; DepthSpacePoint pl = coordinateMapper.MapCameraPointToDepthSpace(leftHandPostition); DepthSpacePoint pr = coordinateMapper.MapCameraPointToDepthSpace(rightHandPostition); // === Links if (pl.X <= frame || pl.X >= depthFrameDescription.Width - frame || pl.Y <= frame || pl.Y >= depthFrameDescription.Height - frame) pl = pl_old; ProcessDepthFrameData(depthBuffer.UnderlyingBuffer, frame, 0, ushort.MaxValue, pl, record, true); this.depthBitmapLeft.WritePixels( new Int32Rect(0, 0, this.depthBitmapLeft.PixelWidth, this.depthBitmapLeft.PixelHeight), this.depthPixels, this.depthBitmapLeft.PixelWidth, 0); pl_old = pl; // === Rechts if (pr.X <= frame || pr.X >= depthFrameDescription.Width - frame || pr.Y <= frame || pr.Y >= depthFrameDescription.Height - frame) pr = pr_old; ProcessDepthFrameData(depthBuffer.UnderlyingBuffer, frame, 0, ushort.MaxValue, pr, record, false); this.depthBitmapRight.WritePixels( new Int32Rect(0, 0, this.depthBitmapRight.PixelWidth, this.depthBitmapRight.PixelHeight), this.depthPixels, this.depthBitmapRight.PixelWidth, 0); pr_old = pr; } } } } }
public Location transformToLocation(DepthSpacePoint point, ushort[] depthData, bool isRelative = false) { return transformToLocation(point.X, point.Y, depthData, isRelative); }
// Note: In order to see the full range of depth (including the less reliable far field depth) we are setting maxDepth (ushort.MaxValue) to the extreme potential depth threshold // If you wish to filter by reliable depth distance, use: depthFrame.DepthMaxReliableDistance private unsafe void ProcessDepthFrameData(IntPtr depthFrameData, int frameSize, ushort minDepth, ushort maxDepth, DepthSpacePoint p, bool rec, bool left) { string file = ""; if (depthFrameSelector == 15 && rec) { if (left) file = String.Format("c:/temp/SLRS/hands/depthDataLeft_{0}_{1}_{2}.txt", gestureWord[gestureNumber], sequenceID, depthFrameIndexL++); else file = String.Format("c:/temp/SLRS/hands/depthDataRight_{0}_{1}_{2}.txt", gestureWord[gestureNumber], sequenceID, depthFrameIndexR++); depthData = new StreamWriter(file, true); Helper.writePCDHeader(depthData); } ushort* frameData = (ushort*)depthFrameData; // depth frame data is a 16 bit value ushort initDepth = frameData[depthFrameDescription.Width * ((int)p.Y) + ((int)p.X)]; byte initPos = (byte)(initDepth / MapDepthToByte); int cloudPointCount = 0; int factor = 80; int index = 0; for (int y = -frameSize; y < frameSize; y++) { for (int x = -frameSize; x < frameSize; x++) { //Select index for smaller frame and get Depth value int i = (depthFrameDescription.Width * ((int)p.Y + y) + ((int)p.X + x)); ushort depth = frameData[i]; // if this depth is near to the initpoint (handpalm) --> record and adapt depth map for visualization if (depth < initDepth + factor && depth > initDepth - factor) { if (depthFrameSelector == 15 && rec) { var point = Helper.depthToPCD(frameSize, p.X + x, p.Y + y, depth); depthData.WriteLine(String.Format("{0} {1} {2}", point.X, point.Y, point.Z)); cloudPointCount++; } depth += (ushort)((depth - initDepth) * 10); } else depth = 0; this.depthPixels[index++] = (byte)(depth / MapDepthToByte); //byte greyValue = (byte)(depth >= minDepth && depth <= maxDepth ? (depth / MapDepthToByte) : 0); //this.depthPixels[index++] = (byte)greyValue;//(255 - greyValue); } } if (depthFrameSelector == 15 && rec) { depthData.Flush(); depthData.Close(); FileStream fs = new FileStream(file); string heigth = cloudPointCount.ToString(); while (!fs.EndOfStream) { var line = fs.ReadLine(); if (line.Contains("WIDTH")) fs.Write(Encoding.ASCII.GetBytes(height), fs.Position+1, height.Lenght); if (line.Contains("POINTS")) { fs.Write(Encoding.ASCII.GetBytes(height), fs.Position+1, height.Lenght); break; } } fs.Flush(); fs.Close(); depthFrameSelector = 0; } if (rec) depthFrameSelector++; }
public byte[] FilterGPU(byte[] bgra, ushort[] depth, DepthSpacePoint[] depthSpaceData, int nearThresh, int farThresh, int haloSize) { if (computeShader == null) { return new byte[0]; } // Initialize last frame with current color frame, if it was reset if (bLastFrameReset) { lastFramePixels = bgra; bLastFrameReset = false; } // -- Create halo array -- List<int2> halo = new List<int2>(); int s = haloSize; int xd = s; int yd = s / 2; int S = (xd + yd) / 2; int x0 = -xd; int x1 = +xd; int y0 = -yd; int y1 = +yd; int actualHaloSize = 0; for (int y = y0; y < y1; ++y) { for (int x = x0; x < x1; ++x) { if (Math.Abs(x) + Math.Abs(y) <= S) { halo.Add(new int2(x, y)); ++actualHaloSize; } } } // -- // -- Perform data transformations so the arrays can be passed to the GPU -- var bgraDataTransformed = new int4[1920 * 1080]; for (int i = 0, j = 0; i < bgra.Length; i += 4, ++j) { bgraDataTransformed[j] = new int4(bgra[i], bgra[i + 1], bgra[i + 2], bgra[i + 3]); } var lastFrameDataTransformed = new int4[1920 * 1080]; for (int i = 0, j = 0; i < bgra.Length; i += 4, ++j) { lastFrameDataTransformed[j] = new int4(lastFramePixels[i], lastFramePixels[i + 1], lastFramePixels[i + 2], lastFramePixels[i + 3]); } // -- //var sw = Stopwatch.StartNew(); // Create a constant buffer to pass the filter configuration var cbuffer = GPGPUHelper.CreateConstantBuffer(device, new int[] { nearThresh, farThresh, haloSize }); // -- Create GPULists using the immediate context and pass the data -- GPUList<int4> bgraData = new GPUList<int4>(device.ImmediateContext); bgraData.AddRange(bgraDataTransformed); GPUList<uint> depthData = new GPUList<uint>(device.ImmediateContext); depthData.AddRange(depth.Select(d => (uint)d)); GPUList<DepthSpacePoint> depthSpacePointData = new GPUList<DepthSpacePoint>(device.ImmediateContext, depthSpaceData); //depthSpacePointData.AddRange(depthSpaceData.Select(dsp => { // if (dsp.X == float.NegativeInfinity || dsp.Y == -float.NegativeInfinity) // { // return new DepthSpacePoint() { X = -1, Y = -1 }; // } // else // { // return dsp; // } //})); GPUList<int4> lastFrameData = new GPUList<int4>(device.ImmediateContext); lastFrameData.AddRange(lastFrameDataTransformed); var resultArray = new int4[1920 * 1080]; GPUList<int4> resultData = new GPUList<int4>(device.ImmediateContext, resultArray); GPUList<int2> haloData = new GPUList<int2>(device.ImmediateContext, halo); // -- var sw = Stopwatch.StartNew(); // Set the buffers and uavs device.ImmediateContext.ComputeShader.Set(computeShader); device.ImmediateContext.ComputeShader.SetConstantBuffer(cbuffer, 0); device.ImmediateContext.ComputeShader.SetUnorderedAccessView(bgraData.UnorderedAccess, 0); device.ImmediateContext.ComputeShader.SetUnorderedAccessView(depthData.UnorderedAccess, 1); device.ImmediateContext.ComputeShader.SetUnorderedAccessView(depthSpacePointData.UnorderedAccess, 2); device.ImmediateContext.ComputeShader.SetUnorderedAccessView(lastFrameData.UnorderedAccess, 3); device.ImmediateContext.ComputeShader.SetUnorderedAccessView(resultData.UnorderedAccess, 4); device.ImmediateContext.ComputeShader.SetUnorderedAccessView(haloData.UnorderedAccess, 5); // Run the compute shader device.ImmediateContext.Dispatch(1920 * 1080 / 256, 1, 1); // Get result. This call blocks, until the result was calculated // because the MapSubresource call waits. var result = resultData.ToArray(); sw.Stop(); // -- Clean up -- device.ImmediateContext.ComputeShader.SetConstantBuffer(null, 0); device.ImmediateContext.ComputeShader.SetUnorderedAccessView(null, 0); device.ImmediateContext.ComputeShader.SetUnorderedAccessView(null, 1); device.ImmediateContext.ComputeShader.SetUnorderedAccessView(null, 2); device.ImmediateContext.ComputeShader.SetUnorderedAccessView(null, 3); device.ImmediateContext.ComputeShader.SetUnorderedAccessView(null, 4); device.ImmediateContext.ComputeShader.SetUnorderedAccessView(null, 5); cbuffer.Dispose(); bgraData.Dispose(); depthData.Dispose(); depthSpacePointData.Dispose(); lastFrameData.Dispose(); resultData.Dispose(); haloData.Dispose(); // -- Debug.WriteLine($"Filtering took {sw.ElapsedMilliseconds} ms"); var resultBytes = new byte[1920 * 1080 * 4]; for (int i = 0, j = 0; i < resultBytes.Length; i += 4, ++j) { resultBytes[i] = (byte)result[j].x; resultBytes[i+1] = (byte)result[j].y; resultBytes[i+2] = (byte)result[j].z; resultBytes[i+3] = (byte)result[j].a; } lastFramePixels = resultBytes; return resultBytes; }
private void PostProc() { if (!TrackDepth || CameraPoints == null) return; if (Points==null) { Points = new KinectPoint[DepthWidth, DepthHeight]; for (int j=0; j<DepthHeight; ++j) { for(int i=0; i<DepthWidth; ++i) { Points[i, j] = new KinectPoint(); Points[i, j].i = i; Points[i, j].j = j; } } } double mind = 0.8; double maxd = 5.0; double dmul = 1.0 / (maxd - mind); float mulcol = 1.0f / 255.0f; int k = 0; DepthSpacePoint dp = new DepthSpacePoint(); for (int j = 0; j < DepthHeight; ++j) { for (int i = 0; i < DepthWidth; ++i) { if (!double.IsNaN(CameraPoints[k].Z) && !double.IsInfinity(CameraPoints[k].Z)) { Points[i, j].p = new OpenTK.Vector3d(CameraPoints[k].X, CameraPoints[k].Y, CameraPoints[k].Z); Points[i, j].depthN = (Points[i, j].p.Z - mind) * dmul; Points[i, j].isReliable = true; } else { dp.X = i; dp.Y = j; CameraSpacePoint cp= coordinateMapper.MapDepthPointToCameraSpace(dp, maxReliableDistance); Points[i, j].p = new OpenTK.Vector3d(cp.X, cp.Y, cp.Z); Points[i, j].depthN = (Points[i, j].p.Z - mind) * dmul; Points[i, j].isReliable = false; } if (TrackColor) { Points[i, j].color.B = KColors[ColorIndex[k]*4]*mulcol; Points[i, j].color.G = KColors[ColorIndex[k]*4+1] * mulcol; Points[i, j].color.R = KColors[ColorIndex[k]*4+2] * mulcol; Points[i, j].color.A = 1.0f; } else { Points[i, j].color.R = 1.0f; Points[i, j].color.G = 1.0f; Points[i, j].color.B = 1.0f; Points[i, j].color.A = 1.0f; } k++; } } }
public byte[] Filter(byte[] bgra, ushort[] depth, DepthSpacePoint[] depthSpaceData, int nearThresh, int farThresh, int haloSize) { switch (_FilterMode) { case FilterMode.CPU: return FilterCPU(bgra, depth, depthSpaceData, nearThresh, farThresh, haloSize); case FilterMode.GPU: return FilterGPU(bgra, depth, depthSpaceData, nearThresh, farThresh, haloSize); default: return new byte[0]; } }
/// <summary> /// 縮小されてた場合に対応する深度TOカラー変換 /// </summary> /// <param name="depthSpacePoint"></param> /// <param name="depth"></param> /// <param name="colorWidth"></param> /// <param name="colorHeight"></param> /// <returns></returns> public ColorSpacePoint MapDepthPointToColorSpace(DepthSpacePoint depthSpacePoint, ushort depth, int colorWidth, int colorHeight) { ColorSpacePoint csp = this.MapDepthPointToColorSpace(depthSpacePoint, depth); csp.X = csp.X * colorWidth / this.originalColorWidth; csp.Y = csp.Y * colorHeight / this.originalColorHeight; return csp; }
private void DrawEllipse(DepthSpacePoint point, Brush brush, double radius) { Ellipse ellipse = new Ellipse { Width = radius, Height = radius, Fill = brush }; canvas.Children.Add(ellipse); Canvas.SetLeft(ellipse, point.X - radius / 2.0); Canvas.SetTop(ellipse, point.Y - radius / 2.0); }
public KinectApp() { this.kinectSensor = KinectSensor.GetDefault(); this.depthFrameDescription = this.kinectSensor.DepthFrameSource.FrameDescription; this.depthFrameReader = this.kinectSensor.DepthFrameSource.OpenReader(); this.depthFrameReader.FrameArrived += depthFrameReader_FrameArrived; this.depthBitmapLeft = new WriteableBitmap(windowSize, windowSize, 96.0, 96.0, PixelFormats.Gray8, null); this.depthBitmapRight = new WriteableBitmap(windowSize, windowSize, 96.0, 96.0, PixelFormats.Gray8, null); this.flowBitmapLeft = new WriteableBitmap(windowSize, windowSize, 96.0, 96.0, PixelFormats.Gray8, null); this.flowBitmapRight = new WriteableBitmap(windowSize, windowSize, 96.0, 96.0, PixelFormats.Gray8, null); this.depthPixels = new byte[windowSize * windowSize]; this.colorFrameDescription = this.kinectSensor.ColorFrameSource.CreateFrameDescription(ColorImageFormat.Bgra); this.colorFrameReader = this.kinectSensor.ColorFrameSource.OpenReader(); this.colorFrameReader.FrameArrived += this.colorFrameReader_FrameArrived; this.colorBitmap = new WriteableBitmap(colorFrameDescription.Width, colorFrameDescription.Height, 96.0, 96.0, PixelFormats.Bgr32, null); this.colorFrameData = new byte[colorFrameDescription.Width * colorFrameDescription.Height * BytesPerPixel]; this.bodyFrameReader = this.kinectSensor.BodyFrameSource.OpenReader(); this.bodyFrameReader.FrameArrived += bodyFrameReader_FrameArrived; this.bodyDrawingGroup = new DrawingGroup(); this.imageSource = new DrawingImage(bodyDrawingGroup); leftHandPostition = new CameraSpacePoint(); rightHandPostition = new CameraSpacePoint(); pl_old = new DepthSpacePoint() { X = depthFrameDescription.Width / 2, Y = depthFrameDescription.Height / 2 }; pr_old = new DepthSpacePoint() { X = depthFrameDescription.Width / 2, Y = depthFrameDescription.Height / 2 }; this.coordinateMapper = this.kinectSensor.CoordinateMapper; this.kinectSensor.IsAvailableChanged += this.Sensor_IsAvailableChanged; this.kinectSensor.Open(); this.DataContext = this; this.InitializeComponent(); }
private unsafe void ProcessDepthFrameData(IntPtr depthFrameData, int frameSize, ushort minDepth, ushort maxDepth, DepthSpacePoint p, bool rec, bool left) { ushort* frameData = (ushort*)depthFrameData; // depth frame data is a 16 bit value ushort initDepth = frameData[depthFrameDescription.Width * ((int)p.Y) + ((int)p.X)]; if (rec && (bool)chk_recDepth.IsChecked) { string file = ""; //FileCode: [left/right]_[gestureNumber]_[sequence]_[sequneceIndex] if (left) file = String.Format("c:/temp/PCD/pcd/dd_left_{0:00}_{1:00}_{2:00}.pcd", gestureNumber, sequenceID, depthFrameIndexL++); else file = String.Format("c:/temp/PCD/pcd/dd_right_{0:00}_{1:00}_{2:00}.pcd", gestureNumber, sequenceID, depthFrameIndexR++); pcdData = new StreamWriter(file, false); } int distanceFactor = 80; int index = 0; currentFrame = new byte[windowSize * windowSize]; for (int y = -frameSize; y < frameSize; y++) { for (int x = -frameSize; x < frameSize; x++) { //Select index for smaller frame and get Depth value int offset = (depthFrameDescription.Width * ((int)p.Y + y) + ((int)p.X + x)); ushort depth = frameData[offset]; bool isNearPalm = depth < initDepth + distanceFactor && depth > initDepth - distanceFactor; depth = isNearPalm ? (ushort)(depth + (depth - initDepth) * 10) : (ushort)0; depthPixels[index] = currentFrame[index] = (byte)(depth / MapDepthToByte); index++; // ==== Record DepthData for nextStep (Segmentation) if ((bool)chk_recDepth.IsChecked && rec) { if (isNearPalm) { var point = Helper.depthToPCD(p.X + (float)x, p.Y + (float)y, depth); pcdData.WriteLine(String.Format("{0} {1} {2}", point.X.ToString().Replace(',', '.'), point.Y.ToString().Replace(',', '.'), point.Z.ToString().Replace(',', '.'))); pcdData.Flush(); } } } } if ((bool)chk_recDepth.IsChecked && rec) pcdData.Close(); //============== Opt Flow ======== var thisPreviousFrame = left ? previousFrameL : previousFrameR; Image<Gray, byte> prevImg = new Image<Gray, byte>(arrayToBitmap(thisPreviousFrame, frameSize * 2, frameSize * 2)); Image<Gray, byte> currentImg = new Image<Gray, byte>(arrayToBitmap(currentFrame, frameSize * 2, frameSize * 2)); Image<Gray, float> flowX = new Image<Gray, float>(new System.Drawing.Size(frameSize * 2, frameSize * 2)); Image<Gray, float> flowY = new Image<Gray, float>(new System.Drawing.Size(frameSize * 2, frameSize * 2)); var winSize = new System.Drawing.Size(5, 5); try { currentImg = currentImg.SmoothMedian(5); OpticalFlow.LK(prevImg, currentImg, winSize, flowX, flowY); var bytes = (flowX.Convert<Gray, byte>() + flowY.Convert<Gray, byte>()).Bytes; var flow = new Image<Gray,byte>(frameSize * 2, frameSize * 2, new Gray (bytes.Sum(e => e) / bytes.Length)); if (left) { previousFrameL = currentFrame; this.flowBitmapLeft.WritePixels(new Int32Rect(0, 0, flow.Bitmap.Width, flow.Bitmap.Height), flow.Bytes, flow.Bitmap.Width, 0); } else { previousFrameR = currentFrame; this.flowBitmapRight.WritePixels(new Int32Rect(0, 0, flow.Bitmap.Width, flow.Bitmap.Height), flow.Bytes, flow.Bitmap.Width, 0); } } catch { Console.WriteLine("Optical Flow Exception"); } //============== OF ======== }
private void showHands(DepthSpacePoint rightHand, DepthSpacePoint leftHand, HandState rightHandState, HandState leftHandState) { var rightBrush = decideHandBrush(rightHandState); drawCircle(50, rightHand.X, rightHand.Y, rightBrush); var leftBrush = decideHandBrush(leftHandState); drawCircle(50, leftHand.X, leftHand.Y, leftBrush); }
// --- color private void ProcessColorMapping(MultiFrame multiFrame) { // --- color mapping from kinect long ticks1 = DateTime.Now.Ticks; DepthSpacePoint[] colorMappedToDepthPoints = new DepthSpacePoint[colorWidth * colorHeight]; coordMapper.MapColorFrameToDepthSpace(multiFrame.DepthData, colorMappedToDepthPoints); Utils.UpdateTimer("(ColorMappingCoord)", ticks1); // --- mapped colorAsDepth -> depth long ticks2 = DateTime.Now.Ticks; byte[] colorMappedBytes = new byte[colorWidth * colorHeight * 4]; unsafe { fixed (byte* fixedColorMapped = colorMappedBytes) fixed (ushort* fixedDepthData = multiFrame.DepthData) fixed (byte* fixedBodyIndexData = multiFrame.BodyIndexData) { byte* ptrColorMapped = fixedColorMapped; ushort* ptrDepthData = fixedDepthData; byte* ptrBodyIndexData = fixedBodyIndexData; // 8 bit if (Context.Use8bitDepth) { for (int i = 0; i < colorPixelSize; i++) { // checking infinity before adding + 0.5f is about 5x faster (!!) float xTmp = colorMappedToDepthPoints[i].X; float yTmp = colorMappedToDepthPoints[i].Y; int x = float.IsInfinity(xTmp) ? -1 : (int)(xTmp + 0.5f); int y = float.IsInfinity(yTmp) ? -1 : (int)(yTmp + 0.5f); if (x >= 0 && x < depthWidth && y >= 0 && y < depthHeight) { int idx = x + y * depthWidth; byte val = (ptrBodyIndexData[idx] < 6 ? byte.MaxValue : (byte)(ptrDepthData[idx] / depthToByte)); *ptrColorMapped++ = val; *ptrColorMapped++ = val; *ptrColorMapped++ = val; *ptrColorMapped++ = 255; // alpha } else { ptrColorMapped += 4; // 0 is default } } } // full depth (13 bit) else { for (int i = 0; i < colorPixelSize; i++) { // checking infinity before adding + 0.5f is about 5x faster (!!) float xTmp = colorMappedToDepthPoints[i].X; float yTmp = colorMappedToDepthPoints[i].Y; int x = float.IsInfinity(xTmp) ? -1 : (int)(xTmp + 0.5f); int y = float.IsInfinity(yTmp) ? -1 : (int)(yTmp + 0.5f); if (x >= 0 && x < depthWidth && y >= 0 && y < depthHeight) { int idx = x + y * depthWidth; ushort val = ptrDepthData[idx]; *ptrColorMapped++ = (byte)(val % 256); *ptrColorMapped++ = (byte)(val / 256); *ptrColorMapped++ = ptrBodyIndexData[idx]; *ptrColorMapped++ = 255; // alpha } else { ptrColorMapped += 4; // 0 is default } } } } } multiFrame.ColorMappedBytes = colorMappedBytes; Utils.UpdateTimer("(ColorMappingBytes)", ticks2); }
public CameraSpacePoint[] GenerateFullPointCloud() { int width = DepthFrameDescription.Width; int height = DepthFrameDescription.Height; int frameSize = width * height; FullPointCloud = new CameraSpacePoint[frameSize]; DepthSpacePoint[] allDepthSpacePoints = new DepthSpacePoint[frameSize]; ushort[] depths = new ushort[frameSize]; for (int i = 0; i < height; ++i) { for (int j = 0; j < width; ++j) { int index = i * width + j; allDepthSpacePoints[index] = new DepthSpacePoint { X = j, Y = i }; FullPointCloud[index] = new CameraSpacePoint(); depths[index] = depthArray[index]; } } kinectSensor.CoordinateMapper.MapDepthPointsToCameraSpace(allDepthSpacePoints, depths, FullPointCloud); return FullPointCloud; }
private async Task DrawColorCoodinate() { // カラー画像の解像度でデータを作る var colorImageBuffer = new byte[colorFrameDesc.LengthInPixels * colorFrameDesc.BytesPerPixel]; // 変換処理が重いため、非同期に行う await Task.Factory.StartNew( () => { // カラー座標系に対応するDepth座標系の一覧を取得する var depthSpace = new DepthSpacePoint[colorFrameDesc.LengthInPixels]; kinect.CoordinateMapper.MapColorFrameToDepthSpace( depthBuffer, depthSpace ); // 並列で処理する Parallel.For( 0, colorFrameDesc.LengthInPixels, i => { int depthX = (int)depthSpace[i].X; int depthY = (int)depthSpace[i].Y; if ( (depthX < 0) || (depthFrameDesc.Width <= depthX) || (depthY < 0) || (depthFrameDesc.Height <= depthY) ) { return; } // Depth座標系のインデックス int depthIndex = (depthY * depthFrameDesc.Width) + depthX; int bodyIndex = bodyIndexBuffer[depthIndex]; // 人を検出した位置だけ色を付ける if ( bodyIndex == 255 ) { return; } // カラー画像を設定する int colorImageIndex = (int)(i * colorFrameDesc.BytesPerPixel); colorImageBuffer[colorImageIndex + 0] = colorBuffer[colorImageIndex + 0]; colorImageBuffer[colorImageIndex + 1] = colorBuffer[colorImageIndex + 1]; colorImageBuffer[colorImageIndex + 2] = colorBuffer[colorImageIndex + 2]; } ); } ); // ビットマップにする var colorBitmap = new WriteableBitmap( colorFrameDesc.Width, colorFrameDesc.Height ); var stream = colorBitmap.PixelBuffer.AsStream(); stream.Write( colorImageBuffer, 0, colorImageBuffer.Length ); colorBitmap.Invalidate(); ImageColor.Source = colorBitmap; }
unsafe private void ShowMappedColorBackgroundRemoved(DepthSpacePoint[] colorMappedToDepthPoints, ushort[] depthFrameData, FrameDescription frameDescription) { fixed (DepthSpacePoint* colorMappedToDepthPointsPointer = colorMappedToDepthPoints) { IBufferByteAccess bitmapBackBufferByteAccess = (IBufferByteAccess)this.bitmap.PixelBuffer; byte* bitmapBackBufferBytes = null; bitmapBackBufferByteAccess.Buffer(out bitmapBackBufferBytes); // Treat the color data as 4-byte pixels uint* bitmapPixelsPointer = (uint*)bitmapBackBufferBytes; int depthWidth = frameDescription.Width; int depthHeight = frameDescription.Height; // Loop over each row and column of the color image // Zero out any pixels that don't correspond to a body index for (int colorIndex = 0; colorIndex < this.colorMappedToDepthPoints.Length; ++colorIndex) { float colorMappedToDepthX = colorMappedToDepthPoints[colorIndex].X; float colorMappedToDepthY = colorMappedToDepthPoints[colorIndex].Y; // The sentinel value is -inf, -inf, meaning that no depth pixel corresponds to this color pixel. if (!float.IsNegativeInfinity(colorMappedToDepthX) && !float.IsNegativeInfinity(colorMappedToDepthY)) { // Make sure the depth pixel maps to a valid point in color space int depthX = (int)(colorMappedToDepthX + 0.5f); int depthY = (int)(colorMappedToDepthY + 0.5f); // If the point is not valid, there is no body index there. if ((depthX >= 0) && (depthX < depthWidth) && (depthY >= 0) && (depthY < depthHeight)) { int depthIndex = (depthY * depthWidth) + depthX; if (depthFrameData[depthIndex] < DepthMax) { continue; } } } // no matching depth. zero out the pixel. bitmapPixelsPointer[colorIndex] = 0; } } this.bitmap.Invalidate(); FrameDisplayImage.Source = this.bitmap; }
/// <summary> /// Calculates the angle between the specified points. /// </summary> /// <param name="center">The center of the angle.</param> /// <param name="start">The start of the angle.</param> /// <param name="end">The end of the angle.</param> /// <returns>The angle, in degrees.</returns> public static double Angle(this DepthSpacePoint center, DepthSpacePoint start, DepthSpacePoint end) { Vector3D first = start.ToVector3() - center.ToVector3(); Vector3D second = end.ToVector3() - center.ToVector3(); return Vector3D.AngleBetween(first, second); }
private void depthFrameReader_FrameArrived(object sender, DepthFrameArrivedEventArgs e) { using (DepthFrame depthFrame = e.FrameReference.AcquireFrame()) { if (depthFrame != null) { using (KinectBuffer depthBuffer = depthFrame.LockImageBuffer()) { if ((this.depthFrameDescription.Width * this.depthFrameDescription.Height) == (depthBuffer.Size / this.depthFrameDescription.BytesPerPixel)) { int frame = windowSize / 2; DepthSpacePoint pl = coordinateMapper.MapCameraPointToDepthSpace(leftHandPostition); DepthSpacePoint pr = coordinateMapper.MapCameraPointToDepthSpace(rightHandPostition); //Draw recordState bool record = btn_record.Content.Equals("Stop"); if (record) { lbl_fpsDepth.Content = Convert.ToUInt64(lbl_fpsBody.Content) + 1; } // === Links if (pl.X <= frame || pl.X >= depthFrameDescription.Width - frame || pl.Y <= frame || pl.Y >= depthFrameDescription.Height - frame) pl = pl_old; ProcessDepthFrameData(depthBuffer.UnderlyingBuffer, frame, 0, ushort.MaxValue, pl, record, true); this.depthBitmapLeft.WritePixels( new Int32Rect(0, 0, this.depthBitmapLeft.PixelWidth, this.depthBitmapLeft.PixelHeight), this.depthPixels, this.depthBitmapLeft.PixelWidth, 0); using (DrawingContext dc = leftHandDrawingGroup.Open()) { dc.DrawImage(LeftHandSource, new Rect(0.0, 0.0, depthFrameDescription.Width, depthFrameDescription.Height)); } pl_old = pl; // === Rechts if (pr.X <= frame || pr.X >= depthFrameDescription.Width - frame || pr.Y <= frame || pr.Y >= depthFrameDescription.Height - frame) pr = pr_old; ProcessDepthFrameData(depthBuffer.UnderlyingBuffer, frame, 0, ushort.MaxValue, pr, record, false); this.depthBitmapRight.WritePixels( new Int32Rect(0, 0, this.depthBitmapRight.PixelWidth, this.depthBitmapRight.PixelHeight), this.depthPixels, this.depthBitmapRight.PixelWidth, 0); using (DrawingContext dc = rightHandDrawingGroup.Open()) { dc.DrawImage(RightHandSource, new Rect(0.0, 0.0, depthFrameDescription.Width, depthFrameDescription.Height)); } pr_old = pr; } } } } }
public Point MapJointToDepth(KinectBase.Joint joint, bool undoTransform) { //TODO: Update this so it takes a joint array instead of a single joint (this is supposed to be more efficient for the Kinect 2) Point mappedPoint = new Point(0, 0); Point3D transformedPosition = joint.Position; if (undoTransform) { Matrix3D inverseTransform = skeletonTransformation; inverseTransform.Invert(); transformedPosition = inverseTransform.Transform(transformedPosition); } //Setup the Kinect v2 objects to do the transformation CameraSpacePoint[] skelPoints = new CameraSpacePoint[1]; skelPoints[0] = new CameraSpacePoint(); skelPoints[0].X = (float)transformedPosition.X; skelPoints[0].Y = (float)transformedPosition.Y; skelPoints[0].Z = (float)transformedPosition.Z; DepthSpacePoint[] points = new DepthSpacePoint[1]; kinect.CoordinateMapper.MapCameraPointsToDepthSpace(skelPoints, points); //Convert back to the base object type mappedPoint.X = points[0].X; mappedPoint.Y = points[0].Y; return mappedPoint; }
public Task<byte[]> FilterGPUAsync(byte[] bgra, ushort[] depth, DepthSpacePoint[] depthSpaceData, int nearThresh, int farThresh, int haloSize) { throw new NotImplementedException(); return Task.Run(() => FilterGPU(bgra, depth, depthSpaceData, nearThresh, farThresh, haloSize)); }
/// <summary> /// Calculates the angle and updates the arc according to the specifed points. /// </summary> /// <param name="start">The starting point.</param> /// <param name="middle">The middle point.</param> /// <param name="end">The end point.</param> /// <param name="desiredRadius">The desired arc radius.</param> public void Update(DepthSpacePoint start, DepthSpacePoint middle, DepthSpacePoint end, double desiredRadius = 0) { Update(start.ToVector3(), middle.ToVector3(), end.ToVector3(), desiredRadius); }