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);
                }
            }
        }
Ejemplo n.º 2
0
        /// <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);
        }
Ejemplo n.º 3
0
        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);
        }
Ejemplo n.º 4
0
        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);
                    }
                }
            }
        }
Ejemplo n.º 5
0
        /// <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;
                }
            }
        }
Ejemplo n.º 6
0
        /// <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);
        }
Ejemplo n.º 7
0
        /// <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);
        }
Ejemplo n.º 8
0
    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);
    }
Ejemplo n.º 9
0
        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);
                        }
                    }
                }
            }
        }
Ejemplo n.º 10
0
        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);
        }
Ejemplo n.º 11
0
        /// <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));
        }
Ejemplo n.º 12
0
 private void _drawSurface(DepthSpacePoint a, DepthSpacePoint b, DepthSpacePoint c, DepthSpacePoint d)
 {
     _drawLine(a, b);
     _drawLine(b, c);
     _drawLine(c, d);
     _drawLine(d, a);
 }
Ejemplo n.º 13
0
    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);
    }
Ejemplo n.º 14
0
        /// <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);
        }
Ejemplo n.º 15
0
        /// <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);
        }
Ejemplo n.º 16
0
        /// <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));
        }
Ejemplo n.º 17
0
    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;
    }
Ejemplo n.º 18
0
        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();
            }
        }
Ejemplo n.º 19
0
        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;
            }
        }
Ejemplo n.º 20
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);
        }
Ejemplo n.º 21
0
    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);
    }
Ejemplo n.º 22
0
        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);
            }
        }
Ejemplo n.º 23
0
        // --- 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;
        }
Ejemplo n.º 24
0
        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));
        }
Ejemplo n.º 25
0
        // 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 };
 }
Ejemplo n.º 27
0
        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
        }
Ejemplo n.º 28
0
        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);
            }
        }
Ejemplo n.º 29
0
    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));
    }
Ejemplo n.º 30
0
        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);
        }
Ejemplo n.º 31
0
        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);
        }
Ejemplo n.º 32
0
 /// <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);
                }
            }

        
        
        }
Ejemplo n.º 35
0
        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);
        }
Ejemplo n.º 36
0
        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;
        }
Ejemplo n.º 37
0
 /// <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);
 }
Ejemplo n.º 38
0
        /// <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();
                    }
                }
            }
        }
Ejemplo n.º 40
0
        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;
                    }
                }
            }      
        }
Ejemplo n.º 41
0
        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;
                        }                                           
                    }
                }
            }
        }
Ejemplo n.º 42
0
 public Location transformToLocation(DepthSpacePoint point, ushort[] depthData, bool isRelative = false)
 {
     return transformToLocation(point.X, point.Y, depthData, isRelative);
 }
Ejemplo n.º 43
0
        // 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++;
        }
Ejemplo n.º 44
0
        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++;
                }
            }
        }
Ejemplo n.º 46
0
 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);
        }
Ejemplo n.º 49
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();
        }
Ejemplo n.º 50
0
        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 ========
        }
Ejemplo n.º 51
0
        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);
        }
Ejemplo n.º 53
0
        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;
        }
Ejemplo n.º 54
0
        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;
        }
Ejemplo n.º 55
0
        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;
        }
Ejemplo n.º 56
0
        /// <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);
        }
Ejemplo n.º 57
0
        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;
                        }
                    }
                }
            }
        }
Ejemplo n.º 58
0
        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;
        }
Ejemplo n.º 59
0
        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));
        }
Ejemplo n.º 60
0
 /// <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);
 }