Exemple #1
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 AngleBetween(this ColorSpacePoint center, ColorSpacePoint start, ColorSpacePoint end)
        {
            Vector3 first = start.ToVector3() - center.ToVector3();
            Vector3 second = end.ToVector3() - center.ToVector3();

            return Vector3.AngleBetween(first, second);
        }
        public void write(Int32 width, Int32 height, Int32 shortRange, Int32 longRange, ColorSpacePoint[,] shortRangeMap, ColorSpacePoint[,] longRangeMap)
        {
            if (File.Exists(filename))
            {
                var result = System.Windows.Forms.MessageBox.Show("File " + filename + " exists. Do you want to replace it?", "File exists", System.Windows.Forms.MessageBoxButtons.YesNo);
                if (result == System.Windows.Forms.DialogResult.No)
                {
                    return;
                }
            }
            using (BinaryWriter coordinateMappingWriter = new BinaryWriter(File.Open(filename, FileMode.Create)))
            {
                coordinateMappingWriter.Write(width);
                coordinateMappingWriter.Write(height);
                coordinateMappingWriter.Write(shortRange);
                coordinateMappingWriter.Write(longRange);

                for (int i = 0; i < width; i++)
                    for (int j = 0; j < height; j++)
                    {
                        coordinateMappingWriter.Write(shortRangeMap[i, j].X);
                        coordinateMappingWriter.Write(shortRangeMap[i, j].Y);
                    }

                for (int i = 0; i < width; i++)
                    for (int j = 0; j < height; j++)
                    {
                        coordinateMappingWriter.Write(longRangeMap[i, j].X);
                        coordinateMappingWriter.Write(longRangeMap[i, j].Y);
                    }
            }
        }
 /// <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;
 }
        /// <summary>
        /// Copies face color points to the gpu
        /// </summary>
        /// <param name="context">Device context</param>
        /// <param name="points">Points array</param>
        /// <param name="elementCount">Number of elements to copy</param>
        public void Copy(DeviceContext context, ColorSpacePoint[] points, int elementCount)
        {
            if (points.Length == 0)
                return;

            fixed (ColorSpacePoint* cptr = &points[0])
            {
                this.buffer.Upload(context, new IntPtr(cptr), points.Length * 8);
            }
        }
        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);
        }
Exemple #7
0
    /// <summary>
    /// this function alignes the depth and color data
    /// tracks the head joint
    /// and cuts out a texture scaled by the distance of the person,
    /// so it will only contain the head
    /// than it saves the files to disk
    /// and sets the loading state for the monitor
    /// </summary>
    /// <param name="body">Body.</param>
    /// <param name="bodyObject">Body object.</param>
    public void SetFace(Kinect.Body body, GameObject bodyObject)
    {
        Kinect.Joint headJoint = body.Joints[Kinect.JointType.Head];
        if (headJoint.TrackingState == TrackingState.Tracked)
        {
            /// <summary>
            /// get the color image coordinates for the head joint
            /// </summary>
            ColorSpacePoint ColorSpaceHead =
                KinectSensor.GetDefault().CoordinateMapper.MapCameraPointToColorSpace(headJoint.Position);

            /// <summary>
            /// get the depth image coordinates for the head joint
            /// </summary>
            DepthSpacePoint depthPoint =
                KinectSensor.GetDefault().CoordinateMapper.MapCameraPointToDepthSpace(headJoint.Position);

            /// <summary>
            /// use the x & y coordinates to locate the depth data
            /// </summary>
            FrameDescription depthDesc = KinectSensor.GetDefault().DepthFrameSource.FrameDescription;

            int depthX     = (int)Mathf.Floor(depthPoint.X + 0.5F);
            int depthY     = (int)Mathf.Floor(depthPoint.Y + 0.5F);
            int depthIndex = (depthY * depthDesc.Width) + depthX;

            //ushort depth = _depthData[depthIndex];

            /// <remarks>
            /// SET FACE IS COUSING AN ERROR SOMETIMES
            ///
            /// IndexOutOfRangeException: Array index is out of range.
            /// FaceSourceView.SetFace (Windows.Kinect.Body body, UnityEngine.GameObject bodyObject) (at Assets/KinectView/Scripts/msaw/FaceSourceView.cs:201)
            /// FaceSourceView.Update () (at Assets/KinectView/Scripts/msaw/FaceSourceView.cs:143)
            ///
            /// this error occures when you move super close to the canvas so i guess the head gets out of frame on top
            /// or the kinect looses depth info when to close ...
            /// </remarks>
            ushort depth = _MultiManager.GetDepthData()[depthIndex];

            int imageSize = (int)(FaceFrameSize / CalculatePixelWidth(depthDesc, depth));
            int x         = (int)(Mathf.Floor(ColorSpaceHead.X + 0.5F) - (imageSize / 2));
            int y         = (int)(Mathf.Floor(ColorSpaceHead.Y + 0.5F) - (imageSize / 2));

//			_width = (float)imageSize;
//			_height = (float)imageSize;

            if (x > 0 && y > 0 && imageSize > 0 && x < _MultiManager.ColorWidth - imageSize && y < _MultiManager.ColorHeight - imageSize)
            {
                Texture2D colorSource = _MultiManager.GetColorTexture();
                /// <remarks>
                /// ERROR
                ///     Texture rectangle is out of bounds (867 + 263 > 1080)
                ///     UnityEngine.Texture2D:GetPixels(Int32, Int32, Int32, Int32)
                ///     FaceSourceView:SetFace(Body, GameObject) (at Assets/KinectView/Scripts/msaw/FaceSourceView.cs:224)
                ///     FaceSourceView:Update() (at Assets/KinectView/Scripts/msaw/FaceSourceView.cs:145)
                ///
                /// tried to fix it with better if condition but did not help
                /// && x <= _MultiManager.ColorWidth -(imageSize / 2) && y <= _MultiManager.ColorHeight -(imageSize / 2)
                ///
                /// Texture rectangle is out of bounds (1859 + 72 > 1920)
                /// tried to fix it now by removing the division on the imageSize
                /// from -(imageSize / 2) to -(imageSize)
                /// </remarks>
                /// 1920x1080 color frame
                /// 512x424 depth and infrared
                Color[]   pix     = colorSource.GetPixels(x, y, imageSize, imageSize);
                Texture2D destTex = new Texture2D(imageSize, imageSize);
                destTex.SetPixels(pix);
                destTex.Apply();
                //gameObject.GetComponent<Renderer>().material.mainTexture = destTex;

                /// <summary>
                /// saving the destTex texture single frame to disk
                /// </summary>
                if (FaceImageNumber <= MaximumFaceImages)
                {
                    if (_MonitorState._Display == MonitorState.Show.Nothing)
                    {
                        SaveTextureToFile(destTex, FaceImageNumber);
                        FaceImageNumber += 1;
                    }
                }
                else
                {
                    //after images are saved tell the server to set the loading state
                    _MonitorState._Display = MonitorState.Show.StartLoading;
                    FaceImageNumber        = 1;
                    //_MonitorState._Display = MonitorState.Show.FaceAnimation;
                }
            }
        }
    }
 /// <summary>
 /// Converts the specified ColorSpacePoint into a 3-D vector structure.
 /// </summary>
 /// <param name="point">The ColorSpacePoint to convert</param>
 /// <returns>The corresponding 3-D vector.</returns>
 public static Vector3D ToVector3(this ColorSpacePoint point)
 {
     return(new Vector3D(point.X, point.Y, 0.0));
 }
        /// <summary>
        /// Updates the bitmap with new frame data.
        /// </summary>
        /// <param name="depthFrame">The specified depth frame.</param>
        /// <param name="colorFrame">The specified color frame.</param>
        /// <param name="bodyIndexFrame">The specified body index frame.</param>
        public void Update(ColorFrame colorFrame, DepthFrame depthFrame, BodyIndexFrame bodyIndexFrame)
        {
            int colorWidth  = colorFrame.FrameDescription.Width;
            int colorHeight = colorFrame.FrameDescription.Height;

            int depthWidth  = depthFrame.FrameDescription.Width;
            int depthHeight = depthFrame.FrameDescription.Height;

            int bodyIndexWidth  = bodyIndexFrame.FrameDescription.Width;
            int bodyIndexHeight = bodyIndexFrame.FrameDescription.Height;

            if (_displayPixels == null)
            {
                _depthData     = new ushort[depthWidth * depthHeight];
                _bodyData      = new byte[depthWidth * depthHeight];
                _colorData     = new byte[colorWidth * colorHeight * Constants.BYTES_PER_PIXEL];
                _displayPixels = new byte[depthWidth * depthHeight * Constants.BYTES_PER_PIXEL];
                _colorPoints   = new ColorSpacePoint[depthWidth * depthHeight];
                Bitmap         = new WriteableBitmap(depthWidth, depthHeight);
                Stream         = Bitmap.PixelBuffer.AsStream();
            }

            if (((depthWidth * depthHeight) == _depthData.Length) && ((colorWidth * colorHeight * Constants.BYTES_PER_PIXEL) == _colorData.Length) && ((bodyIndexWidth * bodyIndexHeight) == _bodyData.Length))
            {
                depthFrame.CopyFrameDataToArray(_depthData);

                if (colorFrame.RawColorImageFormat == ColorImageFormat.Bgra)
                {
                    colorFrame.CopyRawFrameDataToArray(_colorData);
                }
                else
                {
                    colorFrame.CopyConvertedFrameDataToArray(_colorData, ColorImageFormat.Bgra);
                }

                bodyIndexFrame.CopyFrameDataToArray(_bodyData);

                CoordinateMapper.MapDepthFrameToColorSpace(_depthData, _colorPoints);

                Array.Clear(_displayPixels, 0, _displayPixels.Length);

                for (int y = 0; y < depthHeight; ++y)
                {
                    for (int x = 0; x < depthWidth; ++x)
                    {
                        int depthIndex = (y * depthWidth) + x;

                        byte player = _bodyData[depthIndex];

                        if (player != 0xff)
                        {
                            ColorSpacePoint colorPoint = _colorPoints[depthIndex];

                            int colorX = (int)Math.Floor(colorPoint.X + 0.5);
                            int colorY = (int)Math.Floor(colorPoint.Y + 0.5);

                            if ((colorX >= 0) && (colorX < colorWidth) && (colorY >= 0) && (colorY < colorHeight))
                            {
                                int colorIndex   = ((colorY * colorWidth) + colorX) * Constants.BYTES_PER_PIXEL;
                                int displayIndex = depthIndex * Constants.BYTES_PER_PIXEL;

                                _displayPixels[displayIndex + 0] = _colorData[colorIndex];
                                _displayPixels[displayIndex + 1] = _colorData[colorIndex + 1];
                                _displayPixels[displayIndex + 2] = _colorData[colorIndex + 2];
                                _displayPixels[displayIndex + 3] = 0xff;
                            }
                        }
                    }
                }

                Stream.Seek(0, SeekOrigin.Begin);
                Stream.Write(_displayPixels, 0, _displayPixels.Length);

                Bitmap.Invalidate();
            }
        }
        static void reader_FrameArrived(object sender, MultiSourceFrameArrivedEventArgs e)
        {
            var reference = e.FrameReference.AcquireFrame();
            using (var colorFrame = reference.ColorFrameReference.AcquireFrame())
            {
                if (colorFrame != null)
                {
                    ColorFrameReady(colorFrame);
                }
            }
            using (var bodyFrame = reference.BodyFrameReference.AcquireFrame())
            {
                if (bodyFrame != null)
                {
                    var _bodies = new Body[bodyFrame.BodyFrameSource.BodyCount];

                    bodyFrame.GetAndRefreshBodyData(_bodies);
                    _headFound = false;
                    foreach (var body in _bodies)
                    {
                        if (body.IsTracked)
                        {
                            Joint head = body.Joints[JointType.Head];

                            if (head.TrackingState == TrackingState.NotTracked)
                                continue;
                            _headFound = true;
                            _headPosition = Sensor.CoordinateMapper.MapCameraPointToColorSpace(head.Position);

                        }
                    }
                    UpdateZoomPosition();
                }
            }
        }
        /// <summary>
        /// Generic function for filling display buffer and transforming depth data to colorspace 
        /// </summary>
        /// <param name="sourceBuffer"></param>
        /// <param name="outputBuffer"></param>
        /// <param name="bodyData"></param>
        /// <param name="colorPoints"></param>
        public void Apply(ushort[] sourceBuffer, byte[] outputBuffer, byte[] bodyData, ColorSpacePoint [] colorPoints=null)
        {
            int displayIndex = 0;

            for (int sourceIndex = 0; sourceIndex < sourceBuffer.Length; ++sourceIndex)
            {
                if (colorPoints != null)
                {
                    ColorSpacePoint colorPoint = colorPoints[sourceIndex];

                    // make sure the depth pixel maps to a valid point in color space
                    if (float.IsInfinity(colorPoint.X) || float.IsInfinity(colorPoint.Y))
                        continue;

                    int colorX = (int)Math.Floor(colorPoint.X + 0.5);
                    int colorY = (int)Math.Floor(colorPoint.Y + 0.5);

                    if ((colorX < 0) || (colorX >= _displayWidth) || (colorY < 0) || (colorY >= _displayHeight))
                    {
                        continue;
                    }

                    displayIndex = ((colorY * _displayWidth) + colorX) * _bytesPerPixel;
                }

                ApplyColor(outputBuffer, displayIndex, sourceBuffer, sourceIndex, bodyData);

                if (colorPoints == null)
                    displayIndex += 4;
            }
        }
        /// <summary>
        /// action called by the anonymous event handler for every frame arriving
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        /// <param name="form"></param>
        private static void MultiSourceFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e, Display form)
        {
            // get the current acquired frames
            var multiFrames = e.FrameReference.AcquireFrame();

            // automatic resource management with acquired color, body, depth frames
            using (var color_frame = multiFrames.ColorFrameReference.AcquireFrame())
                using (var body_frame = multiFrames.BodyFrameReference.AcquireFrame())
                    using (var depth_frame = multiFrames.DepthFrameReference.AcquireFrame())
                    {
                        // all the frames have to be acquired
                        if (color_frame != null && body_frame != null && depth_frame != null)
                        {
                            var color_frame_width  = color_frame.FrameDescription.Width;
                            var color_frame_height = color_frame.FrameDescription.Height;
                            var color_frame_pixels = new byte[color_frame_width * color_frame_height * 4];
                            var bitmap             = new Bitmap(color_frame_width, color_frame_height, PixelFormat.Format32bppRgb);

                            color_frame.CopyConvertedFrameDataToArray(color_frame_pixels, ColorImageFormat.Bgra);

                            var bitmapData = bitmap.LockBits(new Rectangle(0, 0, bitmap.Width, bitmap.Height),
                                                             ImageLockMode.WriteOnly, bitmap.PixelFormat);
                            Marshal.Copy(color_frame_pixels, 0, bitmapData.Scan0, color_frame_pixels.Length);
                            bitmap.UnlockBits(bitmapData);

                            // wrap the bitmap data into an image object(1920 x 1080)
                            Image <Bgr, byte> bgr = new Image <Bgr, byte>(bitmap);
                            // resize the image into 960 x 540
                            CvInvoke.Resize(bgr, display_kinect_image, new Size(640, 360));

                            //var depth_width = depth_frame.FrameDescription.Width;
                            //var depth_height = depth_frame.FrameDescription.Height;
                            //var depthData = new ushort[depth_width * depth_height];
                            //depth_frame.CopyFrameDataToArray(depthData);

                            var bodies = new Body[kinect_sensor.BodyFrameSource.BodyCount];

                            body_frame.GetAndRefreshBodyData(bodies);

                            var body = bodies.Where(bd => bd.IsTracked).FirstOrDefault();

                            form.Program_status = "Detecting Body";
                            if (!played_detecting_body)
                            {
                                form.play_audio(Properties.Resources.detecting_body);
                                played_detecting_body = true;
                            }

                            if (body != null)
                            {
                                form.Program_status = "Body Tracked";
                                head            = body.Joints[JointType.Head].Position;
                                neck            = body.Joints[JointType.Neck].Position;
                                shoulder_center = body.Joints[JointType.SpineShoulder].Position;
                                hip_center      = body.Joints[JointType.SpineBase].Position;
                                wrist_left      = body.Joints[JointType.WristLeft].Position;
                                wrist_right     = body.Joints[JointType.WristRight].Position;
                                shoulder_left   = body.Joints[JointType.ShoulderLeft].Position;
                                shoulder_right  = body.Joints[JointType.ShoulderRight].Position;
                                elbow_left      = body.Joints[JointType.ElbowLeft].Position;
                                hip_left        = body.Joints[JointType.HipLeft].Position;
                                knee_left       = body.Joints[JointType.KneeLeft].Position;
                                foot_left       = body.Joints[JointType.FootLeft].Position;
                                elbow_right     = body.Joints[JointType.ElbowRight].Position;
                                hip_right       = body.Joints[JointType.HipRight].Position;
                                knee_right      = body.Joints[JointType.KneeRight].Position;
                                foot_right      = body.Joints[JointType.FootRight].Position;

                                //ankle_left = body.Joints[JointType.AnkleLeft].Position;
                                //ankle_right = body.Joints[JointType.AnkleRight].Position;


                                var body_joints = body.Joints.Values;

                                var body_joints_Trked = new Joint[body_joints.Count()];
                                var body_joints_Infrd = new Joint[body_joints.Count()];

                                var body_joints_CamSpacePts_Trked = new CameraSpacePoint[body_joints.Count()];
                                var body_joints_ClrSpacePts_Trked = new ColorSpacePoint[body_joints.Count()];

                                var body_joints_CamSpacePts_Infrd = new CameraSpacePoint[body_joints.Count()];
                                var body_joints_ClrSpacePts_Infrd = new ColorSpacePoint[body_joints.Count()];
                                var untracked_joints = new List <Joint>(0);

                                int i = 0, j = 0;
                                foreach (var joint in body_joints)
                                {
                                    if (joint.TrackingState == TrackingState.Tracked)
                                    {
                                        body_joints_Trked[i]             = joint;
                                        body_joints_CamSpacePts_Trked[i] = joint.Position;
                                        i++;
                                    }
                                    else if (joint.TrackingState == TrackingState.Inferred)
                                    {
                                        body_joints_Infrd[i]             = joint;
                                        body_joints_CamSpacePts_Infrd[j] = joint.Position;
                                        j++;
                                    }
                                    else
                                    {
                                        untracked_joints.Add(joint);
                                    }
                                }
                                kinect_sensor.CoordinateMapper.MapCameraPointsToColorSpace(body_joints_CamSpacePts_Trked, body_joints_ClrSpacePts_Trked);
                                kinect_sensor.CoordinateMapper.MapCameraPointsToColorSpace(body_joints_CamSpacePts_Infrd, body_joints_ClrSpacePts_Infrd);

                                DrawingHelper.draw_tracked_joints(body_joints_ClrSpacePts_Trked, display_kinect_image);
                                DrawingHelper.draw_inferred_joints(body_joints_ClrSpacePts_Infrd, display_kinect_image);


                                if (!HighKnees.ready_for_tutoring)
                                {
                                    if (HighKnees.check_for_tutoring(form))
                                    {
                                        HighKnees.ready_for_tutoring = true;
                                    }
                                }
                                else
                                {
                                    if (!HighKnees.finished_first_quarter)
                                    {
                                        form.Program_status = "First-Quarter Tutoring";
                                        HighKnees.first_quarter_tutoring(form);
                                    }
                                    else if (!HighKnees.finished_second_quarter)
                                    {
                                        form.Program_status = "Second-Quarter Tutoring";
                                        HighKnees.second_quarter_tutoring(form);
                                    }
                                    else if (!HighKnees.finished_third_quarter)
                                    {
                                        form.Program_status  = "Third-Quarter Tutoring";
                                        form.Text_box_status = "Now, see if you can curl your right arm, raise your left knee and slightly bend your left arm simultaneously then switch to another side, which is similar";
                                        if (!HighKnees.played_third_quarter_tutoring)
                                        {
                                            form.play_audio(Properties.Resources.third_quarter_tutoring);
                                            HighKnees.played_third_quarter_tutoring = true;
                                        }

                                        HighKnees.third_quarter_tutoring(form);
                                        if (HighKnees.finished_third_quarter)
                                        {
                                            System.Media.SystemSounds.Beep.Play();
                                        }
                                    }
                                    else if (!HighKnees.finished_fourth_quarter)
                                    {
                                        form.Program_status = "Fourth-Quarter Tutoring";
                                        HighKnees.fourth_quarter_tutoring(form);
                                        if (HighKnees.finished_fourth_quarter)
                                        {
                                            System.Media.SystemSounds.Beep.Play();
                                        }
                                    }
                                    else
                                    {
                                        form.Program_status  = "Free-mode";
                                        form.Text_box_status = "Now you are in exercising mode. Practice more to gain a better skill! Remember to alternate your arms back and forth.";
                                        if (!HighKnees.played_free_mode)
                                        {
                                            form.play_audio(Properties.Resources.free_mode);
                                            HighKnees.played_free_mode = true;
                                        }

                                        if (!HighKnees.finish_first_part_exercise)
                                        {
                                            if (HighKnees.third_quarter_tutoring(form))
                                            {
                                                HighKnees.finish_first_part_exercise = true;
                                                HighKnees.count++;
                                                System.Media.SystemSounds.Beep.Play();
                                                form.Set_counter = HighKnees.count;
                                                HighKnees.finish_second_part_exercise = false;
                                            }
                                        }
                                        else if (!HighKnees.finish_second_part_exercise)
                                        {
                                            if (HighKnees.fourth_quarter_tutoring(form))
                                            {
                                                HighKnees.finish_second_part_exercise = true;
                                                HighKnees.count++;
                                                System.Media.SystemSounds.Beep.Play();
                                                form.Set_counter = HighKnees.count;
                                                HighKnees.finish_first_part_exercise = false;
                                            }
                                        }
                                    }
                                }
                            }
                            else
                            {
                                HighKnees.ready_for_tutoring        = false;
                                HighKnees.played_check_for_tutoring = false;
                            }
                        }
                        form.setImage = display_kinect_image;
                    }
        }
 public static Point GetPoint(this ColorSpacePoint colorSpacePoint)
 {
     return(new Point(colorSpacePoint.X, colorSpacePoint.Y));
 }
Exemple #14
0
        //  TODO: don't sent alpha
        private void ProcessPointCloudColors()
        {
            byte[] pointCloudColors = new byte[depthArray.Length * 4];
            ColorSpacePoint[] colorSpacePoints = new ColorSpacePoint[depthArray.Length];
            var colorWidth = ColorFrameDescription.Width;
            var colorHeight = ColorFrameDescription.Height;

            CoordinateMapper.MapDepthFrameToColorSpace(depthArray, colorSpacePoints);
            float maxX = 0;
            float maxY = 0;

            for (int i = 0; i < depthArray.Length; i++)
            {
                var point = colorSpacePoints[i];
                if (GeometryHelper.IsValidPoint(point))
                {
                    maxX = point.X > maxX ? point.X : maxX;
                    maxY = point.Y > maxY ? point.Y : maxY;

                    int colorX = (int) Math.Floor(point.X + 0.5);
                    int colorY = (int)Math.Floor(point.Y + 0.5);

                    if ((colorX > 0 && colorX < colorWidth) && (colorY > 0 && colorY < colorHeight))
                    {

                        int index = (int)(((colorWidth*colorY)+colorX) * 4);
                        pointCloudColors[i * 4] = colorPixels[index];
                        pointCloudColors[i * 4 + 1] = colorPixels[index + 1];
                        pointCloudColors[i * 4 + 2] = colorPixels[index + 2];
                        pointCloudColors[i * 4 + 3] = colorPixels[index + 3];
                    }

                    //int index = ((int)point.X + DepthFrameDescription.Width / 2) +
                    //            ((int)point.Y + DepthFrameDescription.Height / 2) * DepthFrameDescription.Width;

                    //int index = (int)((point.X + DepthFrameDescription.Width / 2) * ColorFrameDescription.BytesPerPixel +
                    //    (point.Y+ DepthFrameDescription.Height/2) * stride);

                }
            }
            coloredPointCloudStreamMessage = new ColoredPointCloudStreamMessage(FullPointCloud, pointCloudColors);
        }
        private void ProcessDepthMapping(MultiFrame multiFrame)
        {
            // if depth data was not run
            if (multiFrame.DepthBytes == null) multiFrame.DepthBytes = new byte[depthWidth * depthHeight * 4 * 2];

            // --- depth mapping from kinect
            long ticks1 = DateTime.Now.Ticks;
            ColorSpacePoint[] depthMapping = new ColorSpacePoint[depthWidth * depthHeight];
            coordMapper.MapDepthFrameToColorSpace(multiFrame.DepthData, depthMapping);
            Utils.UpdateTimer("(DepthMappingCoord)", ticks1);

            // --- mapped depthAsColor -> color
            long ticks2 = DateTime.Now.Ticks;            
            unsafe
            {
                fixed (byte* fixedDepthMapped = multiFrame.DepthBytes)
                fixed (byte* fixedColorData = multiFrame.ColorData)
                {
                    byte* ptrDepthMapped = fixedDepthMapped;
                    ptrDepthMapped += depthByteSize * 4;                            // joined
                    byte* ptrColorData = fixedColorData;

                    int countMapped = 0;
                    int countNotMapped = 0;
                    for (int i = 0; i < depthByteSize; i++)
                    {
                        // checking infinity before adding + 0.5f is about 5x faster (!!)
                        float xTmp = depthMapping[i].X;
                        float yTmp = depthMapping[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 < colorWidth && y >= 0 && y < colorHeight)
                        {                            
                            int idxBase = x * 4 + y * colorWidth * 4;
                            *ptrDepthMapped++ = ptrColorData[idxBase];
                            *ptrDepthMapped++ = ptrColorData[idxBase + 1];
                            *ptrDepthMapped++ = ptrColorData[idxBase + 2];
                            *ptrDepthMapped++ = ptrColorData[idxBase + 3];
                            countMapped++;
                        }
                        else
                        {
                            *ptrDepthMapped++ = 0;
                            *ptrDepthMapped++ = 0;
                            *ptrDepthMapped++ = 0;
                            *ptrDepthMapped++ = 0;
                            countNotMapped++;
                        }
                    }
                }                
            }                                    
            Utils.UpdateTimer("(DepthMappingBytes)", ticks2);            
        }
Exemple #16
0
        private void MapColorToDepth(ushort[] depthFrameData)
        {
            if (rawColorBitmap != null)
            {
                var pixelBytes = colorFrameDescription.BytesPerPixel;
                byte[] rawColorPixels = new byte[rawColorBitmap.PixelHeight * rawColorBitmap.PixelWidth * pixelBytes];
                rawColorBitmap.CopyPixels(rawColorPixels, rawColorBitmap.BackBufferStride, 0);
                byte[] colorPixels = new byte[colorBitmap.PixelHeight * colorBitmap.PixelWidth * pixelBytes];
                ColorSpacePoint[] colorPoints = new ColorSpacePoint[depthFrameDescription.Height * depthFrameDescription.Width];
                kinectSensor.CoordinateMapper.MapDepthFrameToColorSpace(depthFrameData, colorPoints);
                for (var id = 0; id < colorPoints.Length; id++)
                {
                    if (colorPoints[id].X < 0 || colorPoints[id].X >= colorFrameDescription.Width ||
                        colorPoints[id].Y < 0 || colorPoints[id].Y >= colorFrameDescription.Height)
                        continue;

                    for (var i = 0; i < pixelBytes; i++)
                        colorPixels[id * pixelBytes + i] =
                            rawColorPixels[(Int32)colorPoints[id].Y * rawColorBitmap.PixelWidth * pixelBytes + (Int32)colorPoints[id].X * pixelBytes + i];
                }
                colorBitmap.WritePixels(new Int32Rect(0, 0, colorBitmap.PixelWidth, colorBitmap.PixelHeight),
                    colorPixels, colorBitmap.BackBufferStride, 0);
                colorBitmap.Lock();
                colorBitmap.AddDirtyRect(new Int32Rect(0, 0, colorBitmap.PixelWidth, colorBitmap.PixelHeight));
                colorBitmap.Unlock();
            }
        }
        /// <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;
        }
        /// <summary>
        /// Initializes a new instance of the MainWindow class.
        /// </summary>
        public MainWindow()
        {
            //Depth pixe> FILE
            string winDir = System.Environment.GetEnvironmentVariable("windir");

            writer.WriteLine("START");

            // get the kinectSensor object
            this.kinectSensor = KinectSensor.GetDefault();

            // get the coordinate mapper
            coordinateMapper = this.kinectSensor.CoordinateMapper;

            // open the reader for the depth frames
            this.depthFrameReader = this.kinectSensor.DepthFrameSource.OpenReader();

            this.bodyFrameReader = this.kinectSensor.BodyFrameSource.OpenReader();

            // wire handler for frame arrival
            this.depthFrameReader.FrameArrived += this.Depth_Reader_FrameArrived;

            if (this.bodyFrameReader != null)
            {
                Console.Write("yO");
                this.bodyFrameReader.FrameArrived += this.Body_Reader_FrameArrived;
            }

            // get FrameDescription from DepthFrameSource
            this.depthFrameDescription = this.kinectSensor.DepthFrameSource.FrameDescription;
            // get size of joint space
            this.displayWidth = depthFrameDescription.Width;
            this.displayHeight = depthFrameDescription.Height;

            // open the reader for the body frames
            this.bodyFrameReader = this.kinectSensor.BodyFrameSource.OpenReader();

            // allocate space to put the pixels being received and converted
            this.depthPixels = new byte[this.depthFrameDescription.Width * this.depthFrameDescription.Height];
            this.hist = new int[this.depthPixels.Length];
            // create the bitmap to display
            this.depthBitmap = new WriteableBitmap(this.depthFrameDescription.Width, this.depthFrameDescription.Height, 96.0, 96.0, PixelFormats.Gray8, null);

            // set IsAvailableChanged event notifier
            this.kinectSensor.IsAvailableChanged += this.Sensor_IsAvailableChanged;

            // open the sensor
            this.kinectSensor.Open();

            // set the status text
            this.StatusText = this.kinectSensor.IsAvailable ? Properties.Resources.RunningStatusText
            : Properties.Resources.NoSensorStatusText;

            // use the window object as the view model in this simple example
            this.DataContext = this;

            this.ctr = 0;
            this.handpoint = coordinateMapper.MapCameraPointToColorSpace(lefthandpoint);
            this.lefthandpoint = this.lefthand.Position;

            // initialize the components (controls) of the window
            this.InitializeComponent();
        }
Exemple #19
0
 public static bool IsValidPoint(ColorSpacePoint point)
 {
     return IsNumber(point.X) && IsNumber(point.Y);
 }
Exemple #20
0
 private void DrawPoint(WriteableBitmap bitmap, ColorSpacePoint p)
 {
     bitmap.DrawLineAa(Math.Max((int)p.X - 5, 0), Math.Max((int)p.Y - 5, 0), Math.Min((int)p.X + 5, colorWidth - 1), Math.Min((int)p.Y + 5, colorHeight - 1), Colors.Cyan, 20);
 }
Exemple #21
0
        private async Task DrawDepthCoodinate()
        {
            // Depth画像の解像度でデータを作る
            var colorImageBuffer = new byte[depthFrameDesc.LengthInPixels *
                                            colorFrameDesc.BytesPerPixel];

            // 変換処理が重いため、非同期に行う
            await Task.Factory.StartNew( () =>
            {
                // Depth座標系に対応するカラー座標系の一覧を取得する
                var colorSpace = new ColorSpacePoint[depthFrameDesc.LengthInPixels];
                kinect.CoordinateMapper.MapDepthFrameToColorSpace( depthBuffer, colorSpace );

                // 並列で処理する
                Parallel.For( 0, depthFrameDesc.LengthInPixels, i =>
                {
                    int colorX = (int)colorSpace[i].X;
                    int colorY = (int)colorSpace[i].Y;
                    if ( (colorX < 0) || (colorFrameDesc.Width <= colorX) ||
                     (colorY < 0) || (colorFrameDesc.Height <= colorY) ) {
                        return;
                    }

                    // カラー座標系のインデックス
                    int colorIndex = (colorY * colorFrameDesc.Width) + colorX;
                    int bodyIndex = bodyIndexBuffer[i];

                    // 人を検出した位置だけ色を付ける
                    if ( bodyIndex == 255 ) {
                        return;
                    }

                    // カラー画像を設定する
                    int colorImageIndex = (int)(i * colorFrameDesc.BytesPerPixel);
                    int colorBufferIndex = (int)(colorIndex * colorFrameDesc.BytesPerPixel);
                    colorImageBuffer[colorImageIndex + 0] = colorBuffer[colorBufferIndex + 0];
                    colorImageBuffer[colorImageIndex + 1] = colorBuffer[colorBufferIndex + 1];
                    colorImageBuffer[colorImageIndex + 2] = colorBuffer[colorBufferIndex + 2];
                } );
            } );

            // ビットマップにする
            var colorBitmap = new WriteableBitmap( depthFrameDesc.Width,
                                                   depthFrameDesc.Height );
            var stream = colorBitmap.PixelBuffer.AsStream();
            stream.Write( colorImageBuffer, 0, colorImageBuffer.Length );
            colorBitmap.Invalidate();
            ImageColor.Source = colorBitmap;
        }
Exemple #22
0
        static void Main()
        {
            Application.EnableVisualStyles();
            Application.SetCompatibleTextRenderingDefault(false);

            RenderForm form = new RenderForm("Kinect hd face sample with color map");

            RenderDevice device = new RenderDevice(SharpDX.Direct3D11.DeviceCreationFlags.BgraSupport | DeviceCreationFlags.Debug);
            RenderContext context = new RenderContext(device);
            DX11SwapChain swapChain = DX11SwapChain.FromHandle(device, form.Handle);

            VertexShader vertexShader = ShaderCompiler.CompileFromFile<VertexShader>(device, "ColorHdFaceView.fx", "VS");
            PixelShader pixelShader = ShaderCompiler.CompileFromFile<PixelShader>(device, "ColorHdFaceView.fx", "PS");

            HdFaceIndexBuffer faceIndexBuffer = new HdFaceIndexBuffer(device, 1);
            DynamicHdFaceStructuredBuffer faceVertexBuffer = new DynamicHdFaceStructuredBuffer(device, 1);
            DynamicRgbSpaceFaceStructuredBuffer faceRgbBuffer = new DynamicRgbSpaceFaceStructuredBuffer(device, 1);

            KinectSensor sensor = KinectSensor.GetDefault();
            sensor.Open();

            cbCamera camera = new cbCamera();
            camera.Projection = Matrix.PerspectiveFovLH(1.57f * 0.5f, 1.3f, 0.01f, 100.0f);
            camera.View = Matrix.Translation(0.0f, 0.0f, 0.5f);

            camera.Projection.Transpose();
            camera.View.Transpose();

            ConstantBuffer<cbCamera> cameraBuffer = new ConstantBuffer<cbCamera>(device);
            cameraBuffer.Update(context, ref camera);

            bool doQuit = false;
            bool doUpload = false;

            KinectBody[] bodyFrame = null;
            KinectSensorBodyFrameProvider provider = new KinectSensorBodyFrameProvider(sensor);

            form.KeyDown += (sender, args) => { if (args.KeyCode == Keys.Escape) { doQuit = true; } };

            FaceModel currentFaceModel = new FaceModel();
            FaceAlignment currentFaceAlignment = new FaceAlignment();

            SingleHdFaceProcessor hdFaceProcessor = new SingleHdFaceProcessor(sensor);
            hdFaceProcessor.HdFrameReceived += (sender, args) => { currentFaceModel = args.FaceModel; currentFaceAlignment = args.FaceAlignment; doUpload = true; };

            bool uploadColor = false;
            ColorRGBAFrameData currentData = null;
            DynamicColorRGBATexture colorTexture = new DynamicColorRGBATexture(device);
            KinectSensorColorRGBAFrameProvider colorProvider = new KinectSensorColorRGBAFrameProvider(sensor);
            colorProvider.FrameReceived += (sender, args) => { currentData = args.FrameData; uploadColor = true; };

            provider.FrameReceived += (sender, args) =>
            {
                bodyFrame = args.FrameData;
                var body = bodyFrame.TrackedOnly().ClosestBodies().FirstOrDefault();
                if (body != null)
                {
                    hdFaceProcessor.AssignBody(body);
                }
                else
                {
                    hdFaceProcessor.Suspend();
                }
            };

            context.Context.Rasterizer.State = device.RasterizerStates.WireFrame;

            RenderLoop.Run(form, () =>
            {
                if (doQuit)
                {
                    form.Dispose();
                    return;
                }

                if (doUpload)
                {
                    var vertices = currentFaceModel.CalculateVerticesForAlignment(currentFaceAlignment).ToArray();
                    var vertRgb = new ColorSpacePoint[vertices.Length];
                    sensor.CoordinateMapper.MapCameraPointsToColorSpace(vertices, vertRgb);

                    faceVertexBuffer.Copy(context, vertices);
                    faceRgbBuffer.Copy(context, vertRgb);
                    doUpload = false;
                }

                if (uploadColor)
                {
                    colorTexture.Copy(context, currentData);
                    uploadColor = false;
                }

                context.Context.ClearRenderTargetView(swapChain.RenderView, SharpDX.Color.Black);

                if (hdFaceProcessor.IsValid)
                {
                    context.RenderTargetStack.Push(swapChain);
                    context.Context.VertexShader.SetShaderResource(0, faceVertexBuffer.ShaderView);
                    context.Context.VertexShader.SetShaderResource(1, faceRgbBuffer.ShaderView);
                    context.Context.VertexShader.SetConstantBuffer(0, cameraBuffer.Buffer);

                    context.Context.PixelShader.SetShaderResource(0, colorTexture.ShaderView);
                    context.Context.PixelShader.SetSampler(0, device.SamplerStates.LinearClamp);

                    //Draw lines
                    context.Context.PixelShader.Set(pixelShader);
                    context.Context.VertexShader.Set(vertexShader);

                    //Attach index buffer, null topology since we fetch
                    faceIndexBuffer.AttachWithLayout(context);
                    faceIndexBuffer.Draw(context, 1);
                    context.RenderTargetStack.Pop();
                }

                swapChain.Present(0, SharpDX.DXGI.PresentFlags.None);
            });

            swapChain.Dispose();
            context.Dispose();
            device.Dispose();

            colorProvider.Dispose();
            colorTexture.Dispose();

            cameraBuffer.Dispose();
            faceIndexBuffer.Dispose();
            faceVertexBuffer.Dispose();
            faceRgbBuffer.Dispose();

            provider.Dispose();
            pixelShader.Dispose();
            vertexShader.Dispose();

            sensor.Close();
        }
Exemple #23
0
    private void RefreshData(ushort[] depthData, int colorWidth, int colorHeight)
    {
        var frameDesc = _Sensor.DepthFrameSource.FrameDescription;

        ColorSpacePoint[]  colorSpace  = new ColorSpacePoint[depthData.Length];
        CameraSpacePoint[] cameraSpace = new CameraSpacePoint[depthData.Length];
        _Mapper.MapDepthFrameToColorSpace(depthData, colorSpace);
        _Mapper.MapDepthFrameToCameraSpace(depthData, cameraSpace);

        for (int y = 0; y < frameDesc.Height; y += _DownsampleSize)
        {
            for (int x = 0; x < frameDesc.Width; x += _DownsampleSize)
            {
                int indexX     = x / _DownsampleSize;
                int indexY     = y / _DownsampleSize;
                int smallIndex = (indexY * (frameDesc.Width / _DownsampleSize)) + indexX;
                int bigIndex   = y * frameDesc.Width + x;

                _Vertices[smallIndex].x = -cameraSpace[bigIndex].X;
                _Vertices[smallIndex].y = cameraSpace[bigIndex].Y;
                _Vertices[smallIndex].z = cameraSpace[bigIndex].Z;

                // Update UV mapping with CDRP
                var colorSpacePoint = colorSpace[bigIndex];
                _UV[smallIndex] = new Vector2(colorSpacePoint.X / colorWidth, colorSpacePoint.Y / colorHeight);
            }
        }

        _Mesh.vertices  = _Vertices;
        _Mesh.uv        = _UV;
        _Mesh.triangles = _Triangles;
        _Mesh.RecalculateNormals();

        bool sendMesh = false;

        lock (readyToSendLock)
        {
            sendMesh = readyToSendMesh;
        }

        if (sendMesh)
        {
            var depthWidth  = frameDesc.Width / _DownsampleSize;
            var depthHeight = frameDesc.Height / _DownsampleSize;
            var depthCount  = depthWidth * depthHeight;

            // Breite und Höhe Tiefendaten schicken
            SendDataToClient(BitConverter.GetBytes(depthWidth));
            SendDataToClient(BitConverter.GetBytes(depthHeight));

            // Punktdaten und UV-Daten sammeln und als einzelne Nachricht schicken
            // 5 Floats (X, Y, Z, U, V)
            int    byteCount  = 5 * sizeof(float) * depthCount;
            byte[] dataToSend = new byte[byteCount];
            for (int i = 0; i < depthCount; i++)
            {
                Buffer.BlockCopy(BitConverter.GetBytes(_Vertices[i].x), 0, dataToSend, i * 5 * sizeof(float), sizeof(float));
                Buffer.BlockCopy(BitConverter.GetBytes(_Vertices[i].y), 0, dataToSend, (i * 5 + 1) * sizeof(float), sizeof(float));
                Buffer.BlockCopy(BitConverter.GetBytes(_Vertices[i].z), 0, dataToSend, (i * 5 + 2) * sizeof(float), sizeof(float));
                Buffer.BlockCopy(BitConverter.GetBytes(_UV[i].x), 0, dataToSend, (i * 5 + 3) * sizeof(float), sizeof(float));
                Buffer.BlockCopy(BitConverter.GetBytes(_UV[i].y), 0, dataToSend, (i * 5 + 4) * sizeof(float), sizeof(float));
            }

            SendDataToClient(dataToSend.ToArray());

            // Farbdaten mittels JPG komprimieren und als einzelne Nachricht schicken
            byte[] colorDataCopy = new byte[colorData.Length];
            byte[] jpgData;
            Buffer.BlockCopy(colorData, 0, colorDataCopy, 0, colorData.Length);
            // Der nachfolgende Code zur JPG-Kompression vertauscht Rot- und Blaukanal, deswegen tauschen wir ihn hier auch schonmal
            for (int y = 0; y < colorHeight; y++)
            {
                for (int x = 0; x < colorWidth; x++)
                {
                    int index = y * colorWidth * 4 + x * 4;
                    // Vertausche erstes Byte (Rot) und drittes Byte (Blau)
                    byte temp = colorDataCopy[index];
                    colorDataCopy[index]     = colorDataCopy[index + 2];
                    colorDataCopy[index + 2] = temp;
                }
            }

            try
            {
                // Quelle: https://stackoverflow.com/questions/11730373/byte-array-to-bitmap-image
                Bitmap     bitmap = new Bitmap(colorWidth, colorHeight, System.Drawing.Imaging.PixelFormat.Format32bppRgb);
                BitmapData bmData = bitmap.LockBits(new Rectangle(0, 0, colorWidth, colorHeight), ImageLockMode.ReadWrite, bitmap.PixelFormat);
                Marshal.Copy(colorDataCopy, 0, bmData.Scan0, colorWidth * colorHeight * 4);
                bitmap.UnlockBits(bmData);

                using (var ms = new MemoryStream())
                {
                    bitmap.Save(ms, ImageFormat.Jpeg);
                    jpgData = ms.ToArray();

                    textInGui.text = "JPG length: " + jpgData.Length;

                    // Länge der Daten schicken
                    SendDataToClient(BitConverter.GetBytes(jpgData.Length));
                    SendDataToClient(jpgData);
                }

                lock (readyToSendLock)
                {
                    readyToSendMesh = false;
                }
            } catch (Exception e)
            {
                textInGui.text = e.Message;
            }
        }
    }
Exemple #24
0
        unsafe void UpdateFusionFrame()
        {
            //KinectのDepthImageデータからFusionのDepthImageFrameを作成
            reconstruction.DepthToDepthFloatFrame( depthImagePixels, depthImageFrame,
                FusionDepthProcessor.DefaultMinimumDepth, FusionDepthProcessor.DefaultMaximumDepth, true );

            //depthImageFrameに平滑化をかけてsmoothDepthFrameに格納
            reconstruction.SmoothDepthFloatFrame( depthImageFrame, smoothDepthImageFrame, SmoothingKernelWidth, SmoothingDistanceThreshold );

            //色情報を取得するためにDepthImage各点のColorImageでの位置を計算
            ColorSpacePoint[] points = new ColorSpacePoint[depthWidth * depthHeight];
            coordinateMapper.MapDepthFrameToColorSpace( depthImagePixels, points );

            //colorImageFrameBufferに色情報を格納
            int[] colorImageFrameBuffer = new int[depthWidth * depthHeight];
            fixed ( byte* ptrColorImagePixels = colorImagePixels ) {
                int* rawColorPixels = (int*)ptrColorImagePixels;
                Parallel.For( 0, depthHeight,
                    y =>
                    {
                        for ( int x = 0; x < depthWidth; x++ ) {
                            int index = y * depthWidth + x;
                            ColorSpacePoint point = points[index];
                            int colorX = (int)(Math.Ceiling( point.X ));
                            int colorY = (int)(Math.Ceiling( point.Y ));
                            if ( (colorX >= 0) && (colorX < colorWidth) && (colorY >= 0) && (colorY < colorHeight) ) {
                                colorImageFrameBuffer[index] = rawColorPixels[colorY * colorWidth + colorX];
                            }
                        }

                    } );
            }

            //colorImageFrameBufferからFusionのcolorImageFrameを作成
            colorImageFrame.CopyPixelDataFrom( colorImageFrameBuffer );

            //現在のカメラ位置や角度の情報を計算に反映させるため、worldToCameraTransformを最新に更新
            worldToCameraTransform = reconstruction.GetCurrentWorldToCameraTransform();

            //最新のsmoothDepthImageFrame,colorImageFrameをFusionのReconstructionオブジェクトに反映
            float alignmentEnergy = 0.0f;
            bool ret = reconstruction.ProcessFrame( smoothDepthImageFrame, colorImageFrame, FusionDepthProcessor.DefaultAlignIterationCount,
                FusionDepthProcessor.DefaultIntegrationWeight, FusionDepthProcessor.DefaultColorIntegrationOfAllAngles,
                out alignmentEnergy, worldToCameraTransform );
            //反映結果がエラーならカウントし、カウンタが100を越えたら初めからやり直す
            if ( !ret ) {
                if ( ++errorCount>=100 ) {
                    errorCount = 0;
                    reset();
                }
            }
            //結果を格納
            result();
        }
Exemple #25
0
        /// <summary>
        /// Handles the arrival of each skeleton frame
        /// </summary?
        private void Reader_FrameArrivedSkel(object sender, BodyFrameArrivedEventArgs e)
        {
            BodyFrameReference frameReference = e.FrameReference;

            try
            {
                BodyFrame frame = frameReference.AcquireFrame();

                if (frame != null)
                {
                    // BodyFrame is IDisposable
                    using (frame)
                    {
                        this.framesSinceUpdate++;


                        using (DrawingContext dc = this.drawingGroup.Open())
                        {
                            // Draw a transparent background to set the render size
                            dc.DrawRectangle(Brushes.Transparent, null, new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));

                            // The first time GetAndRefreshBodyData is called, Kinect will allocate each Body in the array.
                            // As long as those body objects are not disposed and not set to null in the array,
                            // those body objects will be re-used.
                            frame.GetAndRefreshBodyData(this.bodies);

                            foreach (Body body in this.bodies)
                            {
                                if (body.IsTracked)
                                {
                                    // 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)
                                    {
                                        ColorSpacePoint depthSpacePoint = this.coordinateMapper.MapCameraPointToColorSpace(joints[jointType].Position);
                                        jointPoints[jointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y);
                                    }

                                    this.DrawBody(joints, jointPoints, dc);

                                    this.DrawHand(body.HandLeftState, jointPoints[JointType.HandLeft], dc);
                                    this.DrawHand(body.HandRightState, jointPoints[JointType.HandRight], dc);

                                    //save the skel coords if the option is selected
                                    if (skelRecO == true)
                                    {
                                        long milliseconds = DateTime.Now.Ticks / TimeSpan.TicksPerMillisecond;

                                        saveSkelInformation(milliseconds, body);
                                        saveTimeStamp("time_skel", milliseconds);
                                        //Save current frame time stamp
                                    }
                                }
                            }

                            // prevent drawing outside of our render area
                            this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));
                        }
                    }
                }
            }
            catch (Exception ex)
            {
                Console.WriteLine(ex.Message);
                // ignore if the frame is no longer available
            }
        }
 /// <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(ColorSpacePoint start, ColorSpacePoint middle, ColorSpacePoint end, double desiredRadius = 0)
 {
     Update(start.ToVector3(), middle.ToVector3(), end.ToVector3(), desiredRadius);
 }
Exemple #27
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(ColorSpacePoint start, ColorSpacePoint middle, ColorSpacePoint end, double desiredRadius = 0)
 {
     Update(start.ToVector3(), middle.ToVector3(), end.ToVector3(), desiredRadius);
 }
Exemple #28
0
        private unsafe void getSkinColor(ColorSpacePoint leftHand, ColorSpacePoint rightHand)
        {
            byte bl, gl, rl, al, br, gr, rr, ar;
            int handLeftOffset = ((colorFrameDescription.Width * ((int)leftHand.Y) + ((int)leftHand.X)) * BytesPerPixel);
            int handRightOffset = ((colorFrameDescription.Width * ((int)rightHand.Y) + ((int)rightHand.X)) * BytesPerPixel);

            if (handLeftOffset > 0 && handRightOffset > 0 && handLeftOffset < 8294400 && handRightOffset < 8294400)
            {
                bl = colorFrameData[handLeftOffset];
                gl = colorFrameData[handLeftOffset + 1];
                rl = colorFrameData[handLeftOffset + 2];
                al = colorFrameData[handLeftOffset + 3];
                br = colorFrameData[handRightOffset];
                gr = colorFrameData[handRightOffset + 1];
                rr = colorFrameData[handRightOffset + 2];
                ar = colorFrameData[handRightOffset + 3];

                var color = new System.Windows.Media.Color();
                color.B = (byte)((bl + br) / 2);
                color.G = (byte)((gl + gr) / 2);
                color.R = (byte)((rl + rr) / 2);
                color.A = (byte)((al + ar) / 2);

                skinColorHue = System.Drawing.Color.FromArgb(color.A, color.R, color.G, color.B).GetHue();
                this.ellipseSkinColor.Fill = new System.Windows.Media.SolidColorBrush(color);
            }
        }
Exemple #29
0
        private void DrawWorksapces(DrawingContext dc)
        {
            CoordinateMapper coordinateMapper = kinectStreamer.CoordinateMapper;

            foreach (Workspace workspace in WorkspaceList)
            {
                Pen pen = WorkspaceActiveMap[workspace] ? activeWorkspacePen : passiveWorkspacePen;

                CameraSpacePoint p0 = Point3DtoCameraSpacePoint(workspace.FittedVertices[0]);
                CameraSpacePoint p1 = Point3DtoCameraSpacePoint(workspace.FittedVertices[1]);
                CameraSpacePoint p2 = Point3DtoCameraSpacePoint(workspace.FittedVertices[2]);
                CameraSpacePoint p3 = Point3DtoCameraSpacePoint(workspace.FittedVertices[3]);

                ColorSpacePoint[] colorPoints = new ColorSpacePoint[4];
                coordinateMapper.MapCameraPointsToColorSpace(new[] { p0, p1, p2, p3 }, colorPoints);

                for (int i = 0; i < colorPoints.Length; i++)
                {
                    dc.DrawLine(pen,
                        new Point(colorPoints[i % colorPoints.Length].X, colorPoints[i % colorPoints.Length].Y),
                        new Point(colorPoints[(i + 1) % colorPoints.Length].X, colorPoints[(i + 1) % colorPoints.Length].Y));
                }
            }
        }
Exemple #30
0
        private unsafe bool isSkinColor(ColorSpacePoint point)
        {
            int offset = ((colorFrameDescription.Width * ((int)point.Y) + ((int)point.X)) * BytesPerPixel);

            if (offset > 0 && offset < 8294400)
            {
                byte b = colorFrameData[offset];
                byte g = colorFrameData[offset + 1];
                byte r = colorFrameData[offset + 2];
                byte a = colorFrameData[offset + 3];

                var color = System.Drawing.Color.FromArgb(r, g, b);
                var hue = color.GetHue(); //290 - 340
                //var sat = color.GetSaturation(); //0-100
                //var bri = color.GetBrightness(); //0-100

                //if (hue > skinColorHue - colorThreshold && hue < skinColorHue + colorThreshold)
                if (hue > 280 && hue < 350)
                    return true;

                return false;
            }

            return false;
        }
 /// <summary>
 /// Converts the specified ColorSpacePoint into a 2-D point.
 /// </summary>
 /// <param name="position">The ColorSpacePoint to convert.</param>
 /// <returns>The corresponding 2-D point.</returns>
 public static Point ToPoint(this ColorSpacePoint position)
 {
     return(new Point(position.X, position.Y));
 }
        /// <summary>
        /// Draws a body joint
        /// </summary>
        /// <param name="joint">Joint of the body</param>
        /// <param name="radius">Circle radius</param>
        /// <param name="fill">Fill color</param>
        /// <param name="borderWidth">Thickness of the border</param>
        /// <param name="border">Color of the boder</param>
        private void DrawJoint(Joint joint, double radius, SolidColorBrush fill, double borderWidth, SolidColorBrush border)
        {
            if (joint.TrackingState != TrackingState.Tracked)
            {
                return;
            }

            // Map the CameraPoint to ColorSpace so they match
            ColorSpacePoint colorPoint = _kinect.CoordinateMapper.MapCameraPointToColorSpace(joint.Position);

            // Create the UI element based on the parameters
            Ellipse el = new Ellipse();

            el.Fill            = fill;
            el.Stroke          = border;
            el.StrokeThickness = borderWidth;
            el.Width           = el.Height = radius;


            Ellipse cb = new Ellipse();

            cb.Fill            = fill;
            cb.Stroke          = border;
            el.StrokeThickness = borderWidth;
            cb.Height          = radius;
            cb.Width           = radius;

            //hand
            Ellipse te = new Ellipse();

            te.Fill    = fill;
            te.Stroke  = border;
            te.Height  = radius;
            te.Width   = radius;
            te.Opacity = 0.25f;

            TextBlock tb = new TextBlock();

            tb.Text     = joint.Position.Y.ToString();
            tb.FontSize = 34;
            tb.Width    = cb.Width;
            tb.Height   = cb.Height;

            //bar path
            if (trackingBarPath)
            {
                Canvas.SetLeft(te, colorPoint.X / 2 - radius / 2);
                Canvas.SetTop(te, colorPoint.Y / 2 - radius / 2);

                if (recording)
                {
                    myCanvas.Children.Add(te);
                }

                if (statMap["feetDistance"])
                {
                    distanceText.Text = "INIT: " + Math.Round(feetData[0], 2).ToString() + " " +
                                        Math.Round(feetData[1], 2).ToString() + " " +
                                        Math.Round(feetData[2], 2).ToString() + " CURRENT: " +
                                        Math.Round(feetDistance, 2).ToString() + " " +
                                        Math.Round(distanceInFront, 2).ToString() + " " +
                                        Math.Round(distanceInBack, 2).ToString() + " r: " +
                                        Math.Round(Math.Abs(feetData[0] - feetDistance) / feetData[0], 2) * 100 + "%, " +
                                        Math.Round(Math.Abs(feetData[1] - distanceInFront) / feetData[1], 2) * 100 + "%, " +
                                        Math.Round(Math.Abs(feetData[2] - distanceInBack) / feetData[2], 2) * 100 + "%";
                }

                testWindow.Content = myCanvas;
                testWindow.Show();
            }

            // Add the Ellipse to the canvas
            //SkeletonCanvas.Children.Add(el);
            SkeletonCanvas.Children.Add(cb);
            //SkeletonCanvas.Children.Add(tb);

            // Avoid exceptions based on bad tracking
            if (float.IsInfinity(colorPoint.X) || float.IsInfinity(colorPoint.X))
            {
                return;
            }

            // Allign ellipse on canvas (Divide by 2 because image is only 50% of original size)
            //Canvas.SetLeft(el, colorPoint.X / 2);
            //Canvas.SetTop(el, colorPoint.Y / 2);



            //if (joint.JointType == JointType.FootLeft)
            //{
            //    //TextBlock tb = new TextBlock();
            //    //tb.Text = joint.Position.Y.ToString();
            //    //tb.FontSize = 34;
            //    //tb.Width = cb.Width;
            //    //tb.Height = cb.Height;
            //    Line line = new Line();

            //    line.X1 = colorPoint.X / 2 - radius / 2;
            //    line.Y1 = colorPoint.Y / 2 - radius / 2;

            //    line.X2 = colorPoint.X / 2 - radius / 2;
            //    line.Y2 = colorPoint.Y / 2 - radius / 2;

            //    line.StrokeThickness = 50;
            //    SolidColorBrush color = new SolidColorBrush();
            //    color.Color = Colors.Yellow;
            //    line.Stroke = color;
            //    SkeletonCanvas.Children.Add(line);
            //}

            Canvas.SetLeft(cb, colorPoint.X / 2 - radius / 2);
            Canvas.SetTop(cb, colorPoint.Y / 2 - radius / 2);
        }
        /// <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 AngleBetween(this ColorSpacePoint center, ColorSpacePoint start, ColorSpacePoint end)
        {
            Vector3D first  = start.ToVector3() - center.ToVector3();
            Vector3D second = end.ToVector3() - center.ToVector3();

            return(Vector3D.AngleBetween(first, second));
        }
Exemple #34
0
        public (Texture2D, Texture2D, DepthSpacePoint[], bool) CreateMapperTextures(Matrix projMatrix, Viewport viewport)
        {
            var w           = viewport.Width;
            var h           = viewport.Height;
            var colorPoints = new ColorSpacePoint[w * h];
            var depthPoints = new DepthSpacePoint[w * h];
            var colorMapper = new Texture2D(gdevice, w, h, false, SurfaceFormat.Vector2); // two channel float32
            var depthMapper = new Texture2D(gdevice, w, h, false, SurfaceFormat.Vector2);

            var targetProjPlane = viewport.Project(new Vector3(0, 0, TARGET_PLANE_M),
                                                   projMatrix, Matrix.Identity, Matrix.Identity).Z;
            var cameraPoints = new CameraSpacePoint[w * h];

            for (int y = 0; y < h; y++)
            {
                for (int x = 0; x < w; x++)
                {
                    var point = viewport.Unproject(new Vector3(x, y, targetProjPlane),
                                                   projMatrix, Matrix.Identity, Matrix.Identity);
                    cameraPoints[y * w + x] = new CameraSpacePoint
                    {
                        X = point.X,
                        Y = point.Y,
                        Z = -point.Z
                    };
                }
            }

            //sometimes the coordinate mapper returns invalid buffers, retry until a valid buffer is returned
            int colorTimeoutCounter = 0;

            do
            {
                kinect.CoordinateMapper.MapCameraPointsToColorSpace(cameraPoints, colorPoints);
                colorTimeoutCounter++;
            } while (float.IsInfinity(colorPoints[w * h / 2 + w / 2].X) && colorTimeoutCounter < 20);
            int depthTimeoutCounter = 0;

            do
            {
                kinect.CoordinateMapper.MapCameraPointsToDepthSpace(cameraPoints, depthPoints);
                depthTimeoutCounter++;
            } while (float.IsInfinity(depthPoints[w * h / 2 + w / 2].X) && depthTimeoutCounter < 20);

            var colorStartX = (colorFrameSize.Width - w) / 2;
            var colorStartY = (colorFrameSize.Height - h) / 2;
            var depthStartX = (depthFrameSize.Width - w) / 2;
            var depthStartY = (depthFrameSize.Height - h) / 2;

            for (int i = 0; i < colorPoints.Length; i++)
            {
                colorPoints[i].X /= colorFrameSize.Width;
                colorPoints[i].Y /= colorFrameSize.Height;
                depthPoints[i].X /= depthFrameSize.Width;
                depthPoints[i].Y /= depthFrameSize.Height;
            }
            colorMapper.SetData(colorPoints);
            depthMapper.SetData(depthPoints);
            return(colorMapper, depthMapper, depthPoints,
                   colorTimeoutCounter < 20 && depthTimeoutCounter < 20);
        }
Exemple #35
0
        private void HandleBodyFrame(BodyFrame bodyFrame)
        {
            if (bodyFrame == null)
            {
                return;
            }
            if (bodies == null)
            {
                bodies = new Body[bodyFrame.BodyCount];
            }
            bodyFrame.GetAndRefreshBodyData(bodies);

            var jmjBody = bodies.FirstOrDefault(b => b.IsTracked); // we get the first tracker body

            if (jmjBody == null)
            {
                HideLaser();
                this.currentViewModel.Status = "NO BODY";
                return;
            }
            ShowLaser();

            posMappedLeft = this.kinectSensor.CoordinateMapper.MapCameraPointToColorSpace(jmjBody.Joints[JointType.HandLeft].Position);
            this.currentViewModel.LeftHandX = posMappedLeft.X;
            this.currentViewModel.LeftHandY = posMappedLeft.Y;

            posMappedRight = this.kinectSensor.CoordinateMapper.MapCameraPointToColorSpace(jmjBody.Joints[JointType.HandRight].Position);
            this.currentViewModel.RightHandX = posMappedRight.X;
            this.currentViewModel.RightHandY = posMappedRight.Y;


            switch (jmjBody.HandLeftState)
            {
            case HandState.Open:
                this.currentViewModel.LeftHandColor = openedHandColor;
                break;

            case HandState.Closed:
                this.currentViewModel.LeftHandColor = closedHandColor;
                break;

            default:
                this.currentViewModel.LeftHandColor = unusableHandColor;
                break;
            }
            switch (jmjBody.HandRightState)
            {
            case HandState.Open:
                this.currentViewModel.RightHandColor = openedHandColor;
                break;

            case HandState.Closed:
                this.currentViewModel.RightHandColor = closedHandColor;
                break;

            default:
                this.currentViewModel.RightHandColor = unusableHandColor;
                break;
            }

            this.currentViewModel.LeftHandStatus  = jmjBody.HandLeftState.ToString();
            this.currentViewModel.RightHandStatus = jmjBody.HandRightState.ToString();

            if (jmjBody.Joints[JointType.ShoulderLeft].Position.Z - jmjBody.Joints[JointType.HandLeft].Position.Z > 0.40)
            {
                PlayLaserUnderLeftHand(posMappedLeft.X, posMappedLeft.Y, jmjBody.HandLeftState);
                this.currentViewModel.LeftHandTorsoDistance = "PLAY";
            }
            else
            {
                leftHandAdapter.ReleaseLaser();
                this.currentViewModel.LeftHandTorsoDistance = "";
            }
            if (jmjBody.Joints[JointType.ShoulderRight].Position.Z - jmjBody.Joints[JointType.HandRight].Position.Z > 0.40)
            {
                PlayLaserUnderRightHand(posMappedRight.X, posMappedRight.Y, jmjBody.HandRightState);
                this.currentViewModel.RightHandTorsoDistance = "PLAY";
            }
            else
            {
                rightHandAdapter.ReleaseLaser();
                this.currentViewModel.RightHandTorsoDistance = "";
            }
        }
Exemple #36
0
        void Reader_MultiSourceFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e)
        {
            var reference = e.FrameReference.AcquireFrame();

            // Color
            using (var frame = reference.ColorFrameReference.AcquireFrame())
            {
                if (frame != null)
                {
                    //将图像转入writeablebitmmap
                    FrameDescription colorFrameDescription = frame.FrameDescription;
                    using (KinectBuffer colorBuffer = frame.LockRawImageBuffer())
                    {
                        this.colorBitmap.Lock();

                        // verify data and write the new color frame data to the display bitmap
                        if ((colorFrameDescription.Width == this.colorBitmap.PixelWidth) && (colorFrameDescription.Height == this.colorBitmap.PixelHeight))
                        {
                            frame.CopyConvertedFrameDataToIntPtr(
                                this.colorBitmap.BackBuffer,
                                (uint)(colorFrameDescription.Width * colorFrameDescription.Height * 4),
                                ColorImageFormat.Bgra);

                            this.colorBitmap.AddDirtyRect(new Int32Rect(0, 0, this.colorBitmap.PixelWidth, this.colorBitmap.PixelHeight));
                        }

                        this.colorBitmap.Unlock();
                    }
                    if (_mode == CameraMode.Color)
                    {
                        camera.Source = frame.ToBitmap();
                    }
                }
            }

            // Depth
            using (var frame = reference.DepthFrameReference.AcquireFrame())
            {
                if (frame != null)
                {
                    if (_mode == CameraMode.Depth)
                    {
                        camera.Source = frame.ToBitmap();
                    }
                }
            }

            // Infrared
            using (var frame = reference.InfraredFrameReference.AcquireFrame())
            {
                if (frame != null)
                {
                    if (_mode == CameraMode.Infrared)
                    {
                        camera.Source = frame.ToBitmap();
                    }
                }
            }

            // Body
            using (var frame = reference.BodyFrameReference.AcquireFrame())
            {
                if (frame != null)
                {
                    canvas.Children.Clear();

                    _bodies = new Body[frame.BodyFrameSource.BodyCount];

                    frame.GetAndRefreshBodyData(_bodies);

                    foreach (var body in _bodies)
                    {
                        if (body.IsTracked)
                        {
                            // COORDINATE MAPPING
                            foreach (Joint joint in body.Joints.Values)
                            {
                                if (joint.TrackingState == TrackingState.Tracked)
                                {
                                    // 3D space point
                                    CameraSpacePoint jointPosition = joint.Position;

                                    // 2D space point
                                    System.Windows.Point point = new System.Windows.Point();


                                    if (_mode == CameraMode.Color)
                                    {
                                        ColorSpacePoint colorPoint = _sensor.CoordinateMapper.MapCameraPointToColorSpace(jointPosition);

                                        point.X = float.IsInfinity(colorPoint.X) ? 0 : colorPoint.X / 2;
                                        point.Y = float.IsInfinity(colorPoint.Y) ? 0 : colorPoint.Y / 2;
                                        //获取肩肘腕,脊椎中间坐标
                                        switch (joint.JointType)
                                        {
                                        case JointType.WristLeft:
                                            WristLeft.X = point.X;
                                            WristLeft.Y = point.Y;
                                            break;

                                        case JointType.ElbowLeft:
                                            ElbowLeft.X = point.X;
                                            ElbowLeft.Y = point.Y;
                                            break;

                                        case JointType.ShoulderLeft:
                                            ShoulderLeft.X = point.X;
                                            ShoulderLeft.Y = point.Y;
                                            break;

                                        case JointType.SpineShoulder:
                                            SpineShoulder.X = point.X;
                                            SpineShoulder.Y = point.Y;
                                            break;

                                        case JointType.ShoulderRight:
                                            ShoulderRight.X = point.X;
                                            ShoulderRight.Y = point.Y;
                                            break;

                                        case JointType.ElbowRight:
                                            ElbowRight.X = point.X;
                                            ElbowRight.Y = point.Y;
                                            break;

                                        case JointType.WristRight:
                                            WristRight.X = point.X;
                                            WristRight.Y = point.Y;
                                            break;
                                        }
                                    }
                                    else if (_mode == CameraMode.Depth || _mode == CameraMode.Infrared) // Change the Image and Canvas dimensions to 512x424
                                    {
                                        DepthSpacePoint depthPoint = _sensor.CoordinateMapper.MapCameraPointToDepthSpace(jointPosition);

                                        point.X = float.IsInfinity(depthPoint.X) ? 0 : depthPoint.X / 2;
                                        point.Y = float.IsInfinity(depthPoint.Y) ? 0 : depthPoint.Y / 2;
                                    }

                                    // Draw 红色圆,直径10
                                    Ellipse ellipse = new Ellipse
                                    {
                                        Fill   = System.Windows.Media.Brushes.Red,
                                        Width  = 10,
                                        Height = 10
                                    };

                                    Canvas.SetLeft(ellipse, point.X - ellipse.Width / 2);
                                    Canvas.SetTop(ellipse, point.Y - ellipse.Height / 2);
                                    canvas.Children.Add(ellipse);
                                }
                            }


                            if (elbowrightflag)
                            {
                                DrawingLine(ShoulderRight, ElbowRight); //连接肩肘
                                DrawingLine(ElbowRight, WristRight);    //连接肘腕

                                //计算肘角度并添加到canvas
                                rightelbowangle = GetAngle(ShoulderRight, ElbowRight, WristRight);
                                Addangle(rightelbowangle, ElbowRight);
                            }

                            if (shoulderrightflag)
                            {
                                DrawingLine(ShoulderRight, ElbowRight);    //连接肩肘
                                DrawingLine(SpineShoulder, ShoulderRight); //连接肩和肩中
                                //计算肩角度并添加到canvas
                                rightshoulderangle = GetAngle(SpineShoulder, ShoulderRight, ElbowRight);
                                Addangle(rightshoulderangle, ShoulderRight);
                            }
                        }
                    }
                }
            }
        }
        public BitmapSource GreenScreenCustom(ColorFrame colorFrame, DepthFrame depthFrame, BodyIndexFrame bodyIndexFrame)
        {
            //

            ushort[]          _depthData        = null;
            byte[]            _bodyData         = null;
            byte[]            _colorData        = null;
            byte[]            _displayPixels    = null;
            ColorSpacePoint[] _colorPoints      = null;
            CoordinateMapper  _coordinateMapper = _sensor.CoordinateMapper;

            int         BYTES_PER_PIXEL = (PixelFormats.Bgr32.BitsPerPixel + 7) / 8;
            PixelFormat FORMAT          = PixelFormats.Bgra32;
            double      DPI             = 96.0;

            WriteableBitmap _bitmap = null;



            //

            int colorWidth  = colorFrame.FrameDescription.Width;
            int colorHeight = colorFrame.FrameDescription.Height;

            int depthWidth  = depthFrame.FrameDescription.Width;
            int depthHeight = depthFrame.FrameDescription.Height;

            int bodyIndexWidth  = bodyIndexFrame.FrameDescription.Width;
            int bodyIndexHeight = bodyIndexFrame.FrameDescription.Height;

            if (_displayPixels == null)
            {
                _depthData     = new ushort[depthWidth * depthHeight];
                _bodyData      = new byte[depthWidth * depthHeight];
                _colorData     = new byte[colorWidth * colorHeight * BYTES_PER_PIXEL];
                _displayPixels = new byte[depthWidth * depthHeight * BYTES_PER_PIXEL];
                _colorPoints   = new ColorSpacePoint[depthWidth * depthHeight];
                _bitmap        = new WriteableBitmap(depthWidth, depthHeight, DPI, DPI, FORMAT, null);
            }

            if (((depthWidth * depthHeight) == _depthData.Length) && ((colorWidth * colorHeight * BYTES_PER_PIXEL) == _colorData.Length) && ((bodyIndexWidth * bodyIndexHeight) == _bodyData.Length))
            {
                depthFrame.CopyFrameDataToArray(_depthData);

                if (colorFrame.RawColorImageFormat == ColorImageFormat.Bgra)
                {
                    colorFrame.CopyRawFrameDataToArray(_colorData);
                }
                else
                {
                    colorFrame.CopyConvertedFrameDataToArray(_colorData, ColorImageFormat.Bgra);
                }

                bodyIndexFrame.CopyFrameDataToArray(_bodyData);

                _coordinateMapper.MapDepthFrameToColorSpace(_depthData, _colorPoints);

                Array.Clear(_displayPixels, 0, _displayPixels.Length);
                Array.Clear(rarray_final, 0, rarray_final.Length);
                Array.Clear(garray_final, 0, rarray_final.Length);
                Array.Clear(barray_final, 0, rarray_final.Length);


                for (int y = 0; y < depthHeight; ++y)
                {
                    for (int x = 0; x < depthWidth; ++x)
                    {
                        int depthIndex = (y * depthWidth) + x;

                        byte player = _bodyData[depthIndex];

                        if (player != 0xff)
                        {
                            ColorSpacePoint colorPoint = _colorPoints[depthIndex];

                            int colorX = (int)Math.Floor(colorPoint.X + 0.5);
                            int colorY = (int)Math.Floor(colorPoint.Y + 0.5);

                            if ((colorX >= 0) && (colorX < colorWidth) && (colorY >= 0) && (colorY < colorHeight))
                            {
                                int colorIndex   = ((colorY * colorWidth) + colorX) * BYTES_PER_PIXEL;
                                int displayIndex = depthIndex * BYTES_PER_PIXEL;

                                _displayPixels[displayIndex + 0] = _colorData[colorIndex];
                                _displayPixels[displayIndex + 1] = _colorData[colorIndex + 1];
                                _displayPixels[displayIndex + 2] = _colorData[colorIndex + 2];
                                _displayPixels[displayIndex + 3] = 0xff;

                                barray_final[y, x] = _colorData[colorIndex];
                                garray_final[y, x] = _colorData[colorIndex + 1];
                                rarray_final[y, x] = _colorData[colorIndex + 2];

                                // For ROS


                                _finaldisplayPixels[displayIndex + 0] = _colorData[colorIndex + 2];
                                _finaldisplayPixels[displayIndex + 1] = _colorData[colorIndex + 1];
                                _finaldisplayPixels[displayIndex + 2] = _colorData[colorIndex];
                                _finaldisplayPixels[displayIndex + 3] = 0xff;
                            }
                        }
                    }
                }

                _bitmap.Lock();

                Marshal.Copy(_displayPixels, 0, _bitmap.BackBuffer, _displayPixels.Length);
                _bitmap.AddDirtyRect(new Int32Rect(0, 0, depthWidth, depthHeight));

                _bitmap.Unlock();
            }

            return(_bitmap);
        }
Exemple #38
0
        /// <summary>
        /// Updates the bitmap with new frame data.
        /// </summary>
        /// <param name="depthFrame">The specified depth frame.</param>
        /// <param name="colorFrame">The specified color frame.</param>
        /// <param name="bodyIndexFrame">The specified body index frame.</param>
        override public void Update(ColorFrame colorFrame, DepthFrame depthFrame, BodyIndexFrame bodyIndexFrame)
        {
            int colorWidth  = colorFrame.FrameDescription.Width;
            int colorHeight = colorFrame.FrameDescription.Height;

            int depthWidth  = depthFrame.FrameDescription.Width;
            int depthHeight = depthFrame.FrameDescription.Height;

            int bodyIndexWidth  = bodyIndexFrame.FrameDescription.Width;
            int bodyIndexHeight = bodyIndexFrame.FrameDescription.Height;

            if (Bitmap == null)
            {
                InitBuffers(colorFrame.FrameDescription, depthFrame.FrameDescription, bodyIndexFrame.FrameDescription);
            }

            if (((depthWidth * depthHeight) == _depthData.Length) && ((colorWidth * colorHeight * Constants.BYTES_PER_PIXEL) == _colorData.Length) && ((bodyIndexWidth * bodyIndexHeight) == _bodyData.Length))
            {
                depthFrame.CopyFrameDataToArray(_depthData);

                if (colorFrame.RawColorImageFormat == ColorImageFormat.Bgra)
                {
                    colorFrame.CopyRawFrameDataToArray(_colorData);
                }
                else
                {
                    colorFrame.CopyConvertedFrameDataToArray(_colorData, ColorImageFormat.Bgra);
                }

                bodyIndexFrame.CopyFrameDataToArray(_bodyData);

                CoordinateMapper.MapDepthFrameToColorSpace(_depthData, _colorPoints);

                Array.Clear(Pixels, 0, Pixels.Length);

                for (int y = 0; y < depthHeight; ++y)
                {
                    for (int x = 0; x < depthWidth; ++x)
                    {
                        int depthIndex = (y * depthWidth) + x;

                        if (_bodyData[depthIndex] != 0xff)
                        {
                            ColorSpacePoint colorPoint = _colorPoints[depthIndex];

                            int colorX = (int)(colorPoint.X + 0.5);
                            int colorY = (int)(colorPoint.Y + 0.5);

                            if ((colorX >= 0) && (colorX < colorWidth) && (colorY >= 0) && (colorY < colorHeight))
                            {
                                int colorIndex   = ((colorY * colorWidth) + colorX) * Constants.BYTES_PER_PIXEL;
                                int displayIndex = depthIndex * Constants.BYTES_PER_PIXEL;

                                for (int b = 0; b < Constants.BYTES_PER_PIXEL; ++b)
                                {
                                    Pixels[displayIndex + b] = _colorData[colorIndex + b];
                                }
                            }
                        }
                    }
                }

                UpdateBitmap();
            }
        }
        void Reader_MultiSourceFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e)
        {
            var reference = e.FrameReference.AcquireFrame();

            // Color
            using (var frame = reference.ColorFrameReference.AcquireFrame())
            {
                if (frame != null)
                {
                    if (_mode == CameraMode.Color)
                    {
                        camera.Source = frame.ToBitmap();
                    }
                }
            }

            // Depth
            using (var frame = reference.DepthFrameReference.AcquireFrame())
            {
                if (frame != null)
                {
                    if (_mode == CameraMode.Depth)
                    {
                        camera.Source = frame.ToBitmap();
                    }
                }
            }

            // Infrared
            using (var frame = reference.InfraredFrameReference.AcquireFrame())
            {
                if (frame != null)
                {
                    if (_mode == CameraMode.Infrared)
                    {
                        camera.Source = frame.ToBitmap();
                    }
                }
            }

            // Body
            using (var frame = reference.BodyFrameReference.AcquireFrame())
            {
                if (frame != null)
                {
                    canvas.Children.Clear();

                    _bodies = new Body[frame.BodyFrameSource.BodyCount];

                    frame.GetAndRefreshBodyData(_bodies);

                    foreach (var body in _bodies)
                    {
                        if (body.IsTracked)
                        {
                            // COORDINATE MAPPING
                            foreach (Joint joint in body.Joints.Values)
                            {
                                if (joint.TrackingState == TrackingState.Tracked)
                                {
                                    // 3D space point
                                    CameraSpacePoint jointPosition = joint.Position;

                                    // 2D space point
                                    Point point = new Point();

                                    if (_mode == CameraMode.Color)
                                    {
                                        ColorSpacePoint colorPoint = _sensor.CoordinateMapper.MapCameraPointToColorSpace(jointPosition);

                                        point.X = float.IsInfinity(colorPoint.X) ? 0 : (1366 * colorPoint.X) / 1920;
                                        point.Y = float.IsInfinity(colorPoint.Y) ? 0 : (768 * colorPoint.Y) / 1080;
                                    }
                                    else if (_mode == CameraMode.Depth || _mode == CameraMode.Infrared) // Change the Image and Canvas dimensions to 512x424
                                    {
                                        DepthSpacePoint depthPoint = _sensor.CoordinateMapper.MapCameraPointToDepthSpace(jointPosition);

                                        point.X = float.IsInfinity(depthPoint.X) ? 0 : depthPoint.X;
                                        point.Y = float.IsInfinity(depthPoint.Y) ? 0 : depthPoint.Y;
                                    }

                                    // Draw
                                    Ellipse ellipse = new Ellipse
                                    {
                                        Fill   = Brushes.Red,
                                        Width  = 15,
                                        Height = 15
                                    };


                                    Canvas.SetLeft(ellipse, point.X - ellipse.Width / 2);
                                    Canvas.SetTop(ellipse, point.Y - ellipse.Height / 2);

                                    canvas.Children.Add(ellipse);

                                    // CHECK 3____________________________________
                                    FLX = camera_intrinsics.FocalLengthX;
                                    FLY = camera_intrinsics.FocalLengthY;

                                    string   FLX2 = Convert.ToString(FLX);
                                    string   FLY2 = Convert.ToString(FLY);
                                    string[] CI   = { "FLX", FLX2, "FLY", FLY2 };
                                    System.IO.File.WriteAllLines(@"D:\ALL.txt", CI);

                                    //int abcdefgh = 8;
                                    //mapper0.GetDepthCameraIntrinsics.FocalLengthX;
                                }
                            }
                        }
                    }
                }
            }
        }
        public void Reader_MultiSourceFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e)
        {
            var reference = e.FrameReference.AcquireFrame();

            // Color
            colorFrame = reference.ColorFrameReference.AcquireFrame();
            {
                if (colorFrame != null)
                {

                    if (_mode == CameraMode.Color)
                    {
                        camera.Source = colorFrame.ToBitmap();
                    }
                }
            }

            // Depth
            depthFrame = reference.DepthFrameReference.AcquireFrame();
            {
                if (depthFrame != null)
                {
                    if (_mode == CameraMode.Depth)
                    {
                        cameraDepth.Source = depthFrame.ToBitmap();
                    }
                }
            }

            Ellipse ellipse1 = new Ellipse
            {
                Fill = Brushes.Green,
                Width = 30,
                Height = 30
            };
            Ellipse ellipse2 = new Ellipse
            {
                Fill = Brushes.Red,
                Width = 30,
                Height = 30
            };

            Ellipse ellipse3 = new Ellipse
            {
                Fill = Brushes.Green,
                Width = 30,
                Height = 30
            };
            Ellipse ellipse4 = new Ellipse
            {
                Fill = Brushes.Red,
                Width = 30,
                Height = 30
            };

            Ellipse ellipse5 = new Ellipse
            {
                Fill = Brushes.Green,
                Width = 30,
                Height = 30
            };
            Ellipse ellipse6 = new Ellipse
            {
                Fill = Brushes.Red,
                Width = 30,
                Height = 30
            };

            Ellipse ellipse7 = new Ellipse
            {
                Fill = Brushes.Green,
                Width = 30,
                Height = 30
            };
            Ellipse ellipse8 = new Ellipse
            {
                Fill = Brushes.Red,
                Width = 30,
                Height = 30
            };

            Ellipse ellipse9 = new Ellipse
            {
                Fill = Brushes.Green,
                Width = 30,
                Height = 30
            };
            Ellipse ellipse10 = new Ellipse
            {
                Fill = Brushes.Red,
                Width = 30,
                Height = 30
            };

            Ellipse ellipse11 = new Ellipse
            {
                Fill = Brushes.Green,
                Width = 30,
                Height = 30
            };
            Ellipse ellipse12 = new Ellipse
            {
                Fill = Brushes.Red,
                Width = 30,
                Height = 30
            };

            if (finishedCountdown)
            {
                if (_mode == CameraMode.Color)
                {
                    camera.Visibility = Visibility.Visible;
                    canvas.Visibility = Visibility.Visible;
                    cameraDepth.Visibility = Visibility.Hidden;
                    canvasDepth.Visibility = Visibility.Hidden;
                    finishedCountdown = false;
                }

                if (_mode == CameraMode.Depth)
                {
                    camera.Visibility = Visibility.Hidden;
                    canvas.Visibility = Visibility.Hidden;
                    cameraDepth.Visibility = Visibility.Visible;
                    canvasDepth.Visibility = Visibility.Visible;
                    finishedCountdown = false;
                }
            }

            // Body
            using (var frame = reference.BodyFrameReference.AcquireFrame())
            {
                if (frame != null)
                {

                    this.canvas.Children.Clear();
                    this.canvasDepth.Children.Clear();

                    _bodies = new Body[frame.BodyFrameSource.BodyCount];

                    frame.GetAndRefreshBodyData(_bodies);

                    //int bodyIndex = 0;
                    foreach (var body in _bodies)
                    {
                        //if (bodyIndex != 0) continue;
                        //bodyIndex++;
                        var cameraSpacePoint = new CameraSpacePoint();

                        DepthSpacePoint depthPointWristLeft = new DepthSpacePoint();
                        DepthSpacePoint depthPointWristRight = new DepthSpacePoint();
                        DepthSpacePoint depthPointHead = new DepthSpacePoint();
                        DepthSpacePoint depthPointSpineMid = new DepthSpacePoint();
                        DepthSpacePoint depthPointKneeLeft = new DepthSpacePoint();
                        DepthSpacePoint depthPointKneeRight = new DepthSpacePoint();

                        ColorSpacePoint colorPointWristLeft = new ColorSpacePoint();
                        ColorSpacePoint colorPointWristRight = new ColorSpacePoint();
                        ColorSpacePoint colorPointHead = new ColorSpacePoint();
                        ColorSpacePoint colorPointSpineMid = new ColorSpacePoint();
                        ColorSpacePoint colorPointKneeLeft = new ColorSpacePoint();
                        ColorSpacePoint colorPointKneeRight = new ColorSpacePoint();

                        if (body.IsTracked)
                        {
                            BodyStructure currentFrameBody = new BodyStructure();

                            var lines = new List<string>();

                            foreach (var joint in body.Joints.Values)
                            {
                                cameraSpacePoint.X = joint.Position.X;//(float)currentFrameBody.currentFrameBody[joint.JointType].X;
                                cameraSpacePoint.Y = joint.Position.Y; //(float)currentFrameBody.currentFrameBody[joint.JointType].Y;
                                cameraSpacePoint.Z = joint.Position.Z; //(float)currentFrameBody.currentFrameBody[joint.JointType].Z;

                                if (PressSpaced && !escape && jointsOfInterest.Contains(joint.JointType))
                                {
                                    curNumOfFrames++;
                                    distanceSum[joint.JointType] += cameraSpacePoint.Z;
                                    meanDistanceToSensor[joint.JointType] = distanceSum[joint.JointType] / ( (curNumOfFrames/6) + 1);
                                }
                                else if(escape && movementVector != null && jointsOfInterest.Contains(joint.JointType))
                                {
                                    distanceScale[joint.JointType] = cameraSpacePoint.Z / meanDistanceToSensor[joint.JointType];
                                }

                                var colorPoint = _sensor.CoordinateMapper.MapCameraPointToColorSpace(cameraSpacePoint);
                                var depthPoint = _sensor.CoordinateMapper.MapCameraPointToDepthSpace(cameraSpacePoint);

                                if (_mode == CameraMode.Depth)
                                {
                                    /*  Ellipse ellipseBlue = new Ellipse
                                      {
                                          Fill = Brushes.Blue,
                                          Width = 10,
                                          Height = 10
                                      };
                                      var xPos = depthPoint.X - ellipseBlue.Width / 2;
                                      var yPos = depthPoint.Y - ellipseBlue.Height / 2;

                                      if (xPos >= 0 && xPos < this.canvasDepth.ActualWidth &&
                                          yPos >= 0 && yPos < this.canvasDepth.ActualHeight)
                                      {
                                          Canvas.SetLeft(ellipseBlue, xPos);
                                          Canvas.SetTop(ellipseBlue, yPos);

                                          canvasDepth.Children.Add(ellipseBlue);
                                      } */

                                    Extensions.DrawSkeleton(canvas, body, jointsOfInterest, _sensor);

                                    //currentFrameBody.currentFrameBody.Add(joint.JointType, new Point3D());
                                    if (jointsOfInterest.Contains(joint.JointType))
                                    {
                                        if (joint.JointType == JointType.HandLeft)
                                            depthPointWristLeft = depthPoint;

                                        else if (joint.JointType == JointType.HandRight)
                                            depthPointWristRight = depthPoint;

                                        else if (joint.JointType == JointType.Head)
                                            depthPointHead = depthPoint;

                                        else if (joint.JointType == JointType.SpineMid)
                                            depthPointSpineMid = depthPoint;

                                        else if (joint.JointType == JointType.KneeRight)
                                            depthPointKneeRight = depthPoint;

                                        else if (joint.JointType == JointType.KneeLeft)
                                            depthPointKneeLeft = depthPoint;

                                        currentFrameBody.currentFrameBody.Add(joint.JointType, new Point3D(depthPoint.X, depthPoint.Y, 0));
                                    }
                                }
                                else
                                {
                                      Ellipse ellipseBlue = new Ellipse
                                      {
                                          Fill = Brushes.Blue,
                                          Width = 10,
                                          Height = 10
                                      };
                                      var xPos = colorPoint.X - ellipseBlue.Width / 2;
                                      var yPos = colorPoint.Y - ellipseBlue.Height / 2;

                                      if (xPos >= 0 && xPos < this.canvas.ActualWidth &&
                                          yPos >= 0 && yPos < this.canvas.ActualHeight)
                                      {
                                          Canvas.SetLeft(ellipseBlue, xPos);
                                          Canvas.SetTop(ellipseBlue, yPos);

                                          canvas.Children.Add(ellipseBlue);
                                      }

                                    Extensions.DrawSkeleton(canvas, body, jointsOfInterest, _sensor);

                                    //currentFrameBody.currentFrameBody.Add(joint.JointType, new Point3D());
                                    if (jointsOfInterest.Contains(joint.JointType))
                                    {
                                        if (joint.JointType == JointType.HandLeft)
                                            colorPointWristLeft = colorPoint;

                                        else if (joint.JointType == JointType.HandRight)
                                            colorPointWristRight = colorPoint;

                                        else if (joint.JointType == JointType.Head)
                                            colorPointHead = colorPoint;

                                        else if (joint.JointType == JointType.SpineMid)
                                            colorPointSpineMid = colorPoint;

                                        else if (joint.JointType == JointType.KneeRight)
                                            colorPointKneeRight = colorPoint;

                                        else if (joint.JointType == JointType.KneeLeft)
                                            colorPointKneeLeft = colorPoint;

                                        var pointCurJoint = new Point3D(colorPoint.X, colorPoint.Y, 0);
                                        this.dictAllMovementPositions[joint.JointType].Add(pointCurJoint);
                                        currentFrameBody.currentFrameBody.Add(joint.JointType, pointCurJoint);
                                    }
                                }
                            }

                            /*TODO - null or empty Dictionary  (.Count == 0)*/
                            //  if (movementVector == null)
                            if (PressSpaced && !escape)
                            {
                                if (movementVector != null && jointsChange)
                                {
                                    movementVector.Clear();
                                }

                                jointsChange = false;
                                movementVector = record.GetMovementVectors(currentFrameBody);

                            }
                            //else if(movementVector != null)
                            else if (escape && movementVector != null)
                            {
                                var curFrameResult = benchPress.ValidateCurrentSkeletonFrame(movementVector, currentFrameBody, distanceScale);
                                var resultWristLeft = curFrameResult.BoneStates[JointType.HandLeft];
                                var resultWristRight = curFrameResult.BoneStates[JointType.HandRight];

                                Extensions.DrawSkeleton(canvas, body, jointsOfInterest, _sensor);

                                if (recordVisualization)
                                {
                                    recordVisualization = false;

                                    Color colorHandLeft = Color.FromArgb(255, 35, 63, 147);
                                    Color colorHandRight = Color.FromArgb(255, 0, 136, 170);
                                    Color colorHead = Color.FromArgb(255, 193, 84, 151);
                                    Color colorSpineMid = Color.FromArgb(255, 238, 198, 20);
                                    Color colorKneeLeft = Color.FromArgb(255, 116, 81, 67);
                                    Color colorKneeRight = Color.FromArgb(255, 94, 58, 106);

                                    foreach (var joy in this.dictAllMovementPositions)
                                    {
                                        Color fillColor = Color.FromArgb(255, 255, 255, 255);
                                        switch (joy.Key)
                                        {
                                            case JointType.HandLeft:
                                                fillColor = colorHandLeft;
                                                break;

                                            case JointType.HandRight:
                                                fillColor = colorHandRight;
                                                break;

                                            case JointType.Head:
                                                fillColor = colorHead;
                                                break;

                                            case JointType.SpineMid:
                                                fillColor = colorSpineMid;
                                                break;

                                            case JointType.KneeLeft:
                                                fillColor = colorKneeLeft;
                                                break;

                                            case JointType.KneeRight:
                                                fillColor = colorKneeRight;
                                                break;
                                        }

                                        foreach (var point in joy.Value)
                                        {

                                           float opacityFactor =  joy.Value.Count / 255  ;
                                            if (opacityFactor == 0.0f)
                                                opacityFactor = 1.0f;

                                            int alpha = (int)opacityFactor * (joy.Value.IndexOf(point) + 1);

                                            fillColor = SetTransparency(alpha, fillColor);

                                            Brush fill = new SolidColorBrush(fillColor);

                                            Ellipse newEllipse = new Ellipse
                                            {
                                                Fill = fill,
                                                Width = 20,
                                                Height = 20
                                            };

                                            var xPosRecord = point.X - newEllipse.Width / 2;
                                            var yPosRecord = point.Y - newEllipse.Height / 2;

                                            if (xPosRecord >= 0 && xPosRecord < this.Record.ActualWidth &&
                                                yPosRecord >= 0 && yPosRecord < this.Record.ActualHeight)
                                            {
                                                Canvas.SetLeft(newEllipse, xPosRecord);
                                                Canvas.SetTop(newEllipse, yPosRecord);

                                                Record.Children.Add(newEllipse);
                                            }
                                        }
                                    }
                                }

                                if (sixJoints.IsChecked == true)
                                {
                                    var resultHead = curFrameResult.BoneStates[JointType.Head];
                                    var resultSpineMid = curFrameResult.BoneStates[JointType.SpineMid];
                                    var resultKneeLeft = curFrameResult.BoneStates[JointType.KneeLeft];
                                    var resultKneeRight = curFrameResult.BoneStates[JointType.KneeRight];

                                    if (_mode == CameraMode.Depth)
                                    {
                                        if (resultSpineMid == SkeletonBoneState.Matched)
                                        {
                                            var xPosMid = depthPointSpineMid.X - ellipse7.Width / 2;
                                            var yPosMid = depthPointSpineMid.Y - ellipse7.Height / 2;

                                            if (xPosMid >= 0 && xPosMid < this.canvasDepth.ActualWidth &&
                                                yPosMid >= 0 && yPosMid < this.canvasDepth.ActualHeight)
                                            {
                                                Canvas.SetLeft(ellipse7, xPosMid);
                                                Canvas.SetTop(ellipse7, yPosMid);

                                                canvasDepth.Children.Add(ellipse7);
                                            }
                                        }
                                        else
                                        {
                                            var xPosMid = depthPointSpineMid.X - ellipse8.Width / 2;
                                            var yPosMid = depthPointSpineMid.Y - ellipse8.Height / 2;

                                            if (xPosMid >= 0 && xPosMid < this.canvasDepth.ActualWidth &&
                                                yPosMid >= 0 && yPosMid < this.canvasDepth.ActualHeight)
                                            {
                                                Canvas.SetLeft(ellipse8, xPosMid);
                                                Canvas.SetTop(ellipse8, yPosMid);

                                                canvasDepth.Children.Add(ellipse8);
                                            }
                                        }

                                        if (resultKneeLeft == SkeletonBoneState.Matched)
                                        {
                                            var xPosKneeLeft = depthPointKneeLeft.X - ellipse9.Width / 2;
                                            var yPosKneeLeft = depthPointKneeLeft.Y - ellipse9.Height / 2;

                                            if (xPosKneeLeft >= 0 && xPosKneeLeft < this.canvasDepth.ActualWidth &&
                                                yPosKneeLeft >= 0 && yPosKneeLeft < this.canvasDepth.ActualHeight)
                                            {
                                                Canvas.SetLeft(ellipse9, xPosKneeLeft);
                                                Canvas.SetTop(ellipse9, yPosKneeLeft);

                                                canvasDepth.Children.Add(ellipse9);
                                            }
                                        }
                                        else
                                        {
                                            var xPosKneeLeft = depthPointKneeLeft.X - ellipse10.Width / 2;
                                            var yPosKneeLeft = depthPointKneeLeft.Y - ellipse10.Height / 2;

                                            if (xPosKneeLeft >= 0 && xPosKneeLeft < this.canvasDepth.ActualWidth &&
                                                yPosKneeLeft >= 0 && yPosKneeLeft < this.canvasDepth.ActualHeight)
                                            {
                                                Canvas.SetLeft(ellipse10, xPosKneeLeft);
                                                Canvas.SetTop(ellipse10, yPosKneeLeft);

                                                canvasDepth.Children.Add(ellipse10);
                                            }
                                        }

                                        if (resultKneeRight == SkeletonBoneState.Matched)
                                        {
                                            var xPosKneeRight = depthPointKneeRight.X - ellipse11.Width / 2;
                                            var yPosKneeRight = depthPointKneeRight.Y - ellipse11.Height / 2;

                                            if (xPosKneeRight >= 0 && xPosKneeRight < this.canvasDepth.ActualWidth &&
                                                yPosKneeRight >= 0 && yPosKneeRight < this.canvasDepth.ActualHeight)
                                            {
                                                Canvas.SetLeft(ellipse11, xPosKneeRight);
                                                Canvas.SetTop(ellipse11, yPosKneeRight);

                                                canvasDepth.Children.Add(ellipse11);
                                            }
                                        }
                                        else
                                        {
                                            var xPosKneeRight = depthPointKneeRight.X - ellipse12.Width / 2;
                                            var yPosKneeRight = depthPointKneeRight.Y - ellipse12.Height / 2;

                                            if (xPosKneeRight >= 0 && xPosKneeRight < this.canvasDepth.ActualWidth &&
                                                yPosKneeRight >= 0 && yPosKneeRight < this.canvasDepth.ActualHeight)
                                            {
                                                Canvas.SetLeft(ellipse12, xPosKneeRight);
                                                Canvas.SetTop(ellipse12, yPosKneeRight);

                                                canvasDepth.Children.Add(ellipse12);
                                            }
                                        }

                                        if (resultHead == SkeletonBoneState.Matched)
                                        {
                                            var xPosHead = depthPointHead.X - ellipse5.Width / 2;
                                            var yPosHead = depthPointHead.Y - ellipse5.Height / 2;

                                            if (xPosHead >= 0 && xPosHead < this.canvasDepth.ActualWidth &&
                                                yPosHead >= 0 && yPosHead < this.canvasDepth.ActualHeight)
                                            {
                                                Canvas.SetLeft(ellipse5, xPosHead);
                                                Canvas.SetTop(ellipse5, yPosHead);

                                                canvasDepth.Children.Add(ellipse5);
                                            }

                                        }
                                        else
                                        {
                                            var xPosHead = depthPointHead.X - ellipse6.Width / 2;
                                            var yPosHead = depthPointHead.Y - ellipse6.Height / 2;

                                            if (xPosHead >= 0 && xPosHead < this.canvasDepth.ActualWidth &&
                                                yPosHead >= 0 && yPosHead < this.canvasDepth.ActualHeight)
                                            {
                                                Canvas.SetLeft(ellipse6, xPosHead);
                                                Canvas.SetTop(ellipse6, yPosHead);

                                                canvasDepth.Children.Add(ellipse6);
                                            }
                                        }

                                        if (resultWristLeft == SkeletonBoneState.Matched)
                                        {
                                            var xPosLeft = depthPointWristLeft.X - ellipse1.Width / 2;
                                            var yPosLeft = depthPointWristLeft.Y - ellipse1.Height / 2;

                                            if (xPosLeft >= 0 && xPosLeft < this.canvasDepth.ActualWidth &&
                                                yPosLeft >= 0 && yPosLeft < this.canvasDepth.ActualHeight)
                                            {
                                                Canvas.SetLeft(ellipse1, xPosLeft);
                                                Canvas.SetTop(ellipse1, yPosLeft);

                                                canvasDepth.Children.Add(ellipse1);
                                            }

                                        }
                                        else
                                        {
                                            var xPosLeft = depthPointWristLeft.X - ellipse2.Width / 2;
                                            var yPosLeft = depthPointWristLeft.Y - ellipse2.Height / 2;

                                            if (xPosLeft >= 0 && xPosLeft < this.canvasDepth.ActualWidth &&
                                                yPosLeft >= 0 && yPosLeft < this.canvasDepth.ActualHeight)
                                            {
                                                Canvas.SetLeft(ellipse2, xPosLeft);
                                                Canvas.SetTop(ellipse2, yPosLeft);

                                                canvasDepth.Children.Add(ellipse2);
                                            }
                                        }

                                        if (resultWristRight == SkeletonBoneState.Matched)
                                        {
                                            var xPosRight = depthPointWristRight.X - ellipse3.Width / 2;
                                            var yPosRight = depthPointWristRight.Y - ellipse3.Height / 2;

                                            if (xPosRight >= 0 && xPosRight < this.canvasDepth.ActualWidth &&
                                                yPosRight >= 0 && yPosRight < this.canvasDepth.ActualHeight)
                                            {
                                                Canvas.SetLeft(ellipse3, xPosRight);
                                                Canvas.SetTop(ellipse3, yPosRight);

                                                canvasDepth.Children.Add(ellipse3);
                                            }

                                        }
                                        else
                                        {
                                            var xPosRight = depthPointWristRight.X - ellipse4.Width / 2;
                                            var yPosRight = depthPointWristRight.Y - ellipse4.Height / 2;

                                            if (xPosRight >= 0 && xPosRight < this.canvasDepth.ActualWidth &&
                                                yPosRight >= 0 && yPosRight < this.canvasDepth.ActualHeight)
                                            {
                                                Canvas.SetLeft(ellipse4, xPosRight);
                                                Canvas.SetTop(ellipse4, yPosRight);

                                                canvasDepth.Children.Add(ellipse4);
                                            }
                                        }
                                    }
                                    else
                                    {
                                        if (resultSpineMid == SkeletonBoneState.Matched)
                                        {
                                            var xPosMid = colorPointSpineMid.X - ellipse7.Width / 2;
                                            var yPosMid = colorPointSpineMid.Y - ellipse7.Height / 2;

                                            if (xPosMid >= 0 && xPosMid < this.canvas.ActualWidth &&
                                                yPosMid >= 0 && yPosMid < this.canvas.ActualHeight)
                                            {
                                                Canvas.SetLeft(ellipse7, xPosMid);
                                                Canvas.SetTop(ellipse7, yPosMid);

                                                canvas.Children.Add(ellipse7);
                                            }
                                        }
                                        else
                                        {
                                            var xPosMid = colorPointSpineMid.X - ellipse8.Width / 2;
                                            var yPosMid = colorPointSpineMid.Y - ellipse8.Height / 2;

                                            if (xPosMid >= 0 && xPosMid < this.canvas.ActualWidth &&
                                                yPosMid >= 0 && yPosMid < this.canvas.ActualHeight)
                                            {
                                                Canvas.SetLeft(ellipse8, xPosMid);
                                                Canvas.SetTop(ellipse8, yPosMid);

                                                canvas.Children.Add(ellipse8);
                                            }
                                        }

                                        if (resultKneeLeft == SkeletonBoneState.Matched)
                                        {
                                            var xPosKneeLeft = colorPointKneeLeft.X - ellipse9.Width / 2;
                                            var yPosKneeLeft = colorPointKneeLeft.Y - ellipse9.Height / 2;

                                            if (xPosKneeLeft >= 0 && xPosKneeLeft < this.canvas.ActualWidth &&
                                                yPosKneeLeft >= 0 && yPosKneeLeft < this.canvas.ActualHeight)
                                            {
                                                Canvas.SetLeft(ellipse9, xPosKneeLeft);
                                                Canvas.SetTop(ellipse9, yPosKneeLeft);

                                                canvas.Children.Add(ellipse9);
                                            }
                                        }
                                        else
                                        {
                                            var xPosKneeLeft = colorPointKneeLeft.X - ellipse10.Width / 2;
                                            var yPosKneeLeft = colorPointKneeLeft.Y - ellipse10.Height / 2;

                                            if (xPosKneeLeft >= 0 && xPosKneeLeft < this.canvas.ActualWidth &&
                                                yPosKneeLeft >= 0 && yPosKneeLeft < this.canvas.ActualHeight)
                                            {
                                                Canvas.SetLeft(ellipse10, xPosKneeLeft);
                                                Canvas.SetTop(ellipse10, yPosKneeLeft);

                                                canvas.Children.Add(ellipse10);
                                            }
                                        }

                                        if (resultKneeRight == SkeletonBoneState.Matched)
                                        {
                                            var xPosKneeRight = colorPointKneeRight.X - ellipse11.Width / 2;
                                            var yPosKneeRight = colorPointKneeRight.Y - ellipse11.Height / 2;

                                            if (xPosKneeRight >= 0 && xPosKneeRight < this.canvas.ActualWidth &&
                                                yPosKneeRight >= 0 && yPosKneeRight < this.canvas.ActualHeight)
                                            {
                                                Canvas.SetLeft(ellipse11, xPosKneeRight);
                                                Canvas.SetTop(ellipse11, yPosKneeRight);

                                                canvas.Children.Add(ellipse11);
                                            }
                                        }
                                        else
                                        {
                                            var xPosKneeRight = colorPointKneeRight.X - ellipse12.Width / 2;
                                            var yPosKneeRight = colorPointKneeRight.Y - ellipse12.Height / 2;

                                            if (xPosKneeRight >= 0 && xPosKneeRight < this.canvas.ActualWidth &&
                                                yPosKneeRight >= 0 && yPosKneeRight < this.canvas.ActualHeight)
                                            {
                                                Canvas.SetLeft(ellipse12, xPosKneeRight);
                                                Canvas.SetTop(ellipse12, yPosKneeRight);

                                                canvas.Children.Add(ellipse12);
                                            }
                                        }

                                        if (resultHead == SkeletonBoneState.Matched)
                                        {
                                            var xPosHead = colorPointHead.X - ellipse5.Width / 2;
                                            var yPosHead = colorPointHead.Y - ellipse5.Height / 2;

                                            if (xPosHead >= 0 && xPosHead < this.canvas.ActualWidth &&
                                                yPosHead >= 0 && yPosHead < this.canvas.ActualHeight)
                                            {
                                                Canvas.SetLeft(ellipse5, xPosHead);
                                                Canvas.SetTop(ellipse5, yPosHead);

                                                canvas.Children.Add(ellipse5);
                                            }

                                        }
                                        else
                                        {
                                            var xPosHead = colorPointHead.X - ellipse6.Width / 2;
                                            var yPosHead = colorPointHead.Y - ellipse6.Height / 2;

                                            if (xPosHead >= 0 && xPosHead < this.canvas.ActualWidth &&
                                                yPosHead >= 0 && yPosHead < this.canvas.ActualHeight)
                                            {
                                                Canvas.SetLeft(ellipse6, xPosHead);
                                                Canvas.SetTop(ellipse6, yPosHead);

                                                canvas.Children.Add(ellipse6);
                                            }
                                        }

                                        if (resultWristLeft == SkeletonBoneState.Matched)
                                        {
                                            var xPosLeft = colorPointWristLeft.X - ellipse1.Width / 2;
                                            var yPosLeft = colorPointWristLeft.Y - ellipse1.Height / 2;

                                            if (xPosLeft >= 0 && xPosLeft < this.canvas.ActualWidth &&
                                                yPosLeft >= 0 && yPosLeft < this.canvas.ActualHeight)
                                            {
                                                Canvas.SetLeft(ellipse1, xPosLeft);
                                                Canvas.SetTop(ellipse1, yPosLeft);

                                                canvas.Children.Add(ellipse1);
                                            }

                                        }
                                        else
                                        {
                                            var xPosLeft = colorPointWristLeft.X - ellipse2.Width / 2;
                                            var yPosLeft = colorPointWristLeft.Y - ellipse2.Height / 2;

                                            if (xPosLeft >= 0 && xPosLeft < this.canvas.ActualWidth &&
                                                yPosLeft >= 0 && yPosLeft < this.canvas.ActualHeight)
                                            {
                                                Canvas.SetLeft(ellipse2, xPosLeft);
                                                Canvas.SetTop(ellipse2, yPosLeft);

                                                canvas.Children.Add(ellipse2);
                                            }
                                        }

                                        if (resultWristRight == SkeletonBoneState.Matched)
                                        {
                                            var xPosRight = colorPointWristRight.X - ellipse3.Width / 2;
                                            var yPosRight = colorPointWristRight.Y - ellipse3.Height / 2;

                                            if (xPosRight >= 0 && xPosRight < this.canvas.ActualWidth &&
                                                yPosRight >= 0 && yPosRight < this.canvas.ActualHeight)
                                            {
                                                Canvas.SetLeft(ellipse3, xPosRight);
                                                Canvas.SetTop(ellipse3, yPosRight);

                                                canvas.Children.Add(ellipse3);
                                            }

                                        }
                                        else
                                        {
                                            var xPosRight = colorPointWristRight.X - ellipse4.Width / 2;
                                            var yPosRight = colorPointWristRight.Y - ellipse4.Height / 2;

                                            if (xPosRight >= 0 && xPosRight < this.canvas.ActualWidth &&
                                                yPosRight >= 0 && yPosRight < this.canvas.ActualHeight)
                                            {
                                                Canvas.SetLeft(ellipse4, xPosRight);
                                                Canvas.SetTop(ellipse4, yPosRight);

                                                canvas.Children.Add(ellipse4);
                                            }
                                        }
                                    }
                                }

                                else
                                {
                                    if (_mode == CameraMode.Depth)
                                    {
                                        if (resultWristLeft == SkeletonBoneState.Matched)
                                        {
                                            var xPosLeft = depthPointWristLeft.X - ellipse1.Width / 2;
                                            var yPosLeft = depthPointWristLeft.Y - ellipse1.Height / 2;

                                            if (xPosLeft >= 0 && xPosLeft < this.canvasDepth.ActualWidth &&
                                                yPosLeft >= 0 && yPosLeft < this.canvasDepth.ActualHeight)
                                            {
                                                Canvas.SetLeft(ellipse1, xPosLeft);
                                                Canvas.SetTop(ellipse1, yPosLeft);

                                                canvasDepth.Children.Add(ellipse1);
                                            }

                                        }
                                        else
                                        {
                                            var xPosLeft = depthPointWristLeft.X - ellipse2.Width / 2;
                                            var yPosLeft = depthPointWristLeft.Y - ellipse2.Height / 2;

                                            if (xPosLeft >= 0 && xPosLeft < this.canvasDepth.ActualWidth &&
                                                yPosLeft >= 0 && yPosLeft < this.canvasDepth.ActualHeight)
                                            {
                                                Canvas.SetLeft(ellipse2, xPosLeft);
                                                Canvas.SetTop(ellipse2, yPosLeft);

                                                canvasDepth.Children.Add(ellipse2);
                                            }
                                        }

                                        if (resultWristRight == SkeletonBoneState.Matched)
                                        {
                                            var xPosRight = depthPointWristRight.X - ellipse3.Width / 2;
                                            var yPosRight = depthPointWristRight.Y - ellipse3.Height / 2;

                                            if (xPosRight >= 0 && xPosRight < this.canvasDepth.ActualWidth &&
                                                yPosRight >= 0 && yPosRight < this.canvasDepth.ActualHeight)
                                            {
                                                Canvas.SetLeft(ellipse3, xPosRight);
                                                Canvas.SetTop(ellipse3, yPosRight);

                                                canvasDepth.Children.Add(ellipse3);
                                            }

                                        }
                                        else
                                        {
                                            var xPosRight = depthPointWristRight.X - ellipse4.Width / 2;
                                            var yPosRight = depthPointWristRight.Y - ellipse4.Height / 2;

                                            if (xPosRight >= 0 && xPosRight < this.canvasDepth.ActualWidth &&
                                                yPosRight >= 0 && yPosRight < this.canvasDepth.ActualHeight)
                                            {
                                                Canvas.SetLeft(ellipse4, xPosRight);
                                                Canvas.SetTop(ellipse4, yPosRight);

                                                canvasDepth.Children.Add(ellipse4);
                                            }
                                        }
                                    }
                                    else
                                    {
                                        if (resultWristLeft == SkeletonBoneState.Matched)
                                        {
                                            var xPosLeft = colorPointWristLeft.X - ellipse1.Width / 2;
                                            var yPosLeft = colorPointWristLeft.Y - ellipse1.Height / 2;

                                            if (xPosLeft >= 0 && xPosLeft < this.canvas.ActualWidth &&
                                                yPosLeft >= 0 && yPosLeft < this.canvas.ActualHeight)
                                            {
                                                Canvas.SetLeft(ellipse1, xPosLeft);
                                                Canvas.SetTop(ellipse1, yPosLeft);

                                                canvas.Children.Add(ellipse1);
                                            }

                                        }
                                        else
                                        {
                                            var xPosLeft = colorPointWristLeft.X - ellipse2.Width / 2;
                                            var yPosLeft = colorPointWristLeft.Y - ellipse2.Height / 2;

                                            if (xPosLeft >= 0 && xPosLeft < this.canvas.ActualWidth &&
                                                yPosLeft >= 0 && yPosLeft < this.canvas.ActualHeight)
                                            {
                                                Canvas.SetLeft(ellipse2, xPosLeft);
                                                Canvas.SetTop(ellipse2, yPosLeft);

                                                canvas.Children.Add(ellipse2);
                                            }
                                        }

                                        if (resultWristRight == SkeletonBoneState.Matched)
                                        {
                                            var xPosRight = colorPointWristRight.X - ellipse3.Width / 2;
                                            var yPosRight = colorPointWristRight.Y - ellipse3.Height / 2;

                                            if (xPosRight >= 0 && xPosRight < this.canvas.ActualWidth &&
                                                yPosRight >= 0 && yPosRight < this.canvas.ActualHeight)
                                            {
                                                Canvas.SetLeft(ellipse3, xPosRight);
                                                Canvas.SetTop(ellipse3, yPosRight);

                                                canvas.Children.Add(ellipse3);
                                            }

                                        }
                                        else
                                        {
                                            var xPosRight = colorPointWristRight.X - ellipse4.Width / 2;
                                            var yPosRight = colorPointWristRight.Y - ellipse4.Height / 2;

                                            if (xPosRight >= 0 && xPosRight < this.canvas.ActualWidth &&
                                                yPosRight >= 0 && yPosRight < this.canvas.ActualHeight)
                                            {
                                                Canvas.SetLeft(ellipse4, xPosRight);
                                                Canvas.SetTop(ellipse4, yPosRight);

                                                canvas.Children.Add(ellipse4);
                                            }
                                        }
                                    }
                                }

                            }
                            else
                            {
                                escape = false;
                            }
                        }
                    }
                }
            }

            if (depthFrame != null)
                depthFrame.Dispose();
            depthFrame = null;
            if (colorFrame != null)
                colorFrame.Dispose();
            colorFrame = null;
        }
Exemple #41
0
        private void Reader_BodyFrameArrived(object sender, BodyFrameArrivedEventArgs e)
        {
            bool dataReceived = false;

            if (e.FrameReference != null)
            {
                using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame())
                {
                    if (bodyFrame != null)
                    {
                        if (this.bodies == null)
                        {
                            this.bodies = new Body[bodyFrame.BodyCount];
                        }

                        bodyFrame.GetAndRefreshBodyData(this.bodies);
                        dataReceived = true;
                    }
                }
            }
            if (!dataReceived)
            {
                return;
            }


            for (int i = 0; i < this.bodyCount; i++)
            {
                Body body = this.bodies[i];


                if (body.IsTracked && engagement.engagedBodies.Contains(body.TrackingId))
                {
                    CameraSpacePoint handLeftPoint  = body.Joints[JointType.HandLeft].Position;
                    CameraSpacePoint handRightPoint = body.Joints[JointType.HandRight].Position;
                    CameraSpacePoint spineBase      = body.Joints[JointType.SpineBase].Position;

                    // Console.WriteLine(body.HandLeftConfidence);

                    ColorSpacePoint handLeft  = coordinateMapper.MapCameraPointToColorSpace(handLeftPoint);
                    ColorSpacePoint handRight = coordinateMapper.MapCameraPointToColorSpace(handRightPoint);
                    double          angle     = ColourGestures.DetectColourGestures(handLeftPoint, handRightPoint);
                    Point           last      = new Point(handLeft.X, handLeft.Y);
                    if (previousPoints.ContainsKey(body.TrackingId) && previousPoints[body.TrackingId] != null && previousPoints[body.TrackingId].Points.Count > 0)
                    {
                        last = previousPoints[body.TrackingId].Points[previousPoints[body.TrackingId].Points.Count - 1];
                    }

                    double notJoinedLeftX = (last.X + ((handLeftPoint.X - spineBase.X) * drawArea.ActualWidth - last.X));
                    double notJoinedLeftY = (last.Y + ((spineBase.Y - handLeftPoint.Y) * drawArea.ActualHeight - last.Y));

                    Point notJoinedLeft = new Point(notJoinedLeftX + drawArea.ActualWidth / 2, notJoinedLeftY + drawArea.ActualHeight);
                    notJoinedPrevPos = notJoinedLeft;
                    Console.WriteLine("my calcualtaion " + notJoinedLeft.X + " " + notJoinedLeft.Y);

                    Ellipse colourSwatch = new Ellipse()
                    {
                        HorizontalAlignment = HorizontalAlignment.Left,
                        Height          = 35,
                        Width           = 35,
                        Margin          = new Thickness(5),
                        Stroke          = Brushes.Black,
                        StrokeThickness = 3
                    };
                    if (!colourSwatches.ContainsKey(body.TrackingId))
                    {
                        colourSwatches.Add(body.TrackingId, colourSwatch);
                    }
                    drawArea.Children.Remove(colourSwatches[body.TrackingId]);
                    if (angle > -1)
                    {
                        Color newColour = ColourGestures.HsvToRgb(angle, 1, 1);
                        colours[body.TrackingId]             = new SolidColorBrush(newColour);
                        colourSwatches[body.TrackingId].Fill = colours[body.TrackingId];
                        drawArea.Children.Add(colourSwatches[body.TrackingId]);
                        startStopGestures.penUp[body.TrackingId] = true;
                    }


                    if (engagement.Draw(body.TrackingId) == HandType.LEFT)
                    {
                        startStopGestures.DetectLeftGestures(body, handLeftPoint);
                        startStopGestures.DetectStopGesture(body, handLeftPoint, handRightPoint);
                        Point currentPoint = new Point(handLeft.X, handLeft.Y);

                        float closenessFactor = handLeftPoint.Z / 4;
                        if (closenessFactor > 1)
                        {
                            closenessFactor = 1;
                        }


                        if (currentPoint.X < Double.PositiveInfinity && currentPoint.X > Double.NegativeInfinity &&
                            currentPoint.Y < Double.PositiveInfinity && currentPoint.Y > Double.NegativeInfinity)
                        {
                            currentPoint.X = (1 - closenessFactor) * currentPoint.X + (notJoinedLeft.X * (closenessFactor));
                            currentPoint.Y = (1 - closenessFactor) * currentPoint.Y + (notJoinedLeft.Y * (closenessFactor));
                            Draw(currentPoint, body.TrackingId, handLeftPoint.Z);

                            Canvas.SetTop(colourSwatches[body.TrackingId], handLeft.Y - 70);
                            Canvas.SetLeft(colourSwatches[body.TrackingId], handLeft.X + 10);
                        }
                    }
                    if (engagement.Draw(body.TrackingId) == HandType.RIGHT)
                    {
                        startStopGestures.DetectRightGestures(body, handRightPoint);
                        startStopGestures.DetectStopGesture(body, handRightPoint, handLeftPoint);

                        Point currentPoint = new Point(handRight.X, handRight.Y);
                        if (currentPoint.X < Double.PositiveInfinity && currentPoint.X > Double.NegativeInfinity &&
                            currentPoint.Y < Double.PositiveInfinity && currentPoint.Y > Double.NegativeInfinity)
                        {
                            Draw(currentPoint, body.TrackingId, handRightPoint.Z);


                            Canvas.SetTop(colourSwatches[body.TrackingId], handRight.Y - 70);
                            Canvas.SetLeft(colourSwatches[body.TrackingId], handRight.X - 10);
                        }
                    }
                }
                else if (body.IsTracked)
                {
                    if (cursors.ContainsKey(body.TrackingId))
                    {
                        drawArea.Children.Remove(cursors[body.TrackingId]);

                        cursors.Remove(body.TrackingId);
                        rotateTransforms.Remove(body.TrackingId);
                    }
                    if (colourSwatches.ContainsKey(body.TrackingId))
                    {
                        drawArea.Children.Remove(colourSwatches[body.TrackingId]);

                        colourSwatches.Remove(body.TrackingId);
                    }
                    if (previousPoints.ContainsKey(body.TrackingId))
                    {
                        if (previousPoints[body.TrackingId] != null)
                        {
                            FinishSegment(body.TrackingId);
                        }
                        previousPoints.Remove(body.TrackingId);
                    }
                }

                if (this.faceFrameSources[i].IsTrackingIdValid)
                {
                    if (this.faceFrameResults[i] != null)
                    {
                        this.DrawFaceFrameResults(i, this.faceFrameResults[i]);
                    }
                }
                else
                {
                    if (this.bodies[i].IsTracked)
                    {
                        this.faceFrameSources[i].TrackingId = this.bodies[i].TrackingId;
                    }
                }
            }
        }
Exemple #42
0
        private void BodyReader_FrameArrived(object sender, BodyFrameArrivedEventArgs e)
        {
            bool dataReceived = false;

            using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame())
            {
                if (bodyFrame != null)
                {
                    this.bodies = new Body[bodyFrame.BodyCount];
                    bodyFrame.GetAndRefreshBodyData(bodies);
                    //  Console.WriteLine(canvas.Width + " " + canvas.Height);
                    Body body = bodies.Where(b => b.IsTracked).FirstOrDefault();

                    if (body != null)
                    {
                        using (DrawingContext dc = this.drawingGroup.Open())
                        {
                            Joint            handRight         = body.Joints[JointType.HandTipRight];
                            Joint            handLeft          = body.Joints[JointType.HandLeft];
                            CameraSpacePoint handRightPosition = handRight.Position;
                            ColorSpacePoint  handRightPoint    = this.kinectSensor.CoordinateMapper.MapCameraPointToColorSpace(handRightPosition);
                            CameraSpacePoint handLeftPosition  = handLeft.Position;
                            ColorSpacePoint  handLeftPoint     = this.kinectSensor.CoordinateMapper.MapCameraPointToColorSpace(handLeftPosition);

                            /*Microsoft.Research.Kinect.Nui.Vector vector = new Microsoft.Research.Kinect.Nui.Vector();
                             * vector.X = ScaleVector(1024, handright.Position.X);
                             * vector.Y = ScaleVector(800, -handright.Position.Y);
                             * vector.Z = handright.Position.Z;
                             *
                             * handright.Position = vector;
                             *
                             * int topofscreen;
                             * int leftofscreen;
                             *
                             * leftofscreen = Convert.ToInt32(handRightPoint.X);
                             * topofscreen = Convert.ToInt32(handRightPoint.Y);
                             *
                             * DepthSpacePoint handPt = kinectSensor.CoordinateMapper.MapCameraPointToDepthSpace(handRightPosition);
                             *
                             * Point relativePoint = new Point (handRightPoint.X-350, handRightPoint.Y-120 );
                             *
                             * SetCursorPos((int)relativePoint.X, (int)relativePoint.Y);
                             *
                             */
                            //SetCursorPos(leftofscreen, topofscreen);
                            if (fpaint == true)
                            {
                                double x = handRightPoint.X;
                                double y = handRightPoint.Y;

                                if ((body.HandRightState == HandState.Closed /*|| body.HandRightState==HandState.Lasso */ || body.HandRightState == HandState.Unknown))
                                {
                                    fgpoly = true;
                                    Console.Write(x + " " + y + "\n");
                                    if (x <= (Canvas.GetLeft(Black) + Black.Width) && x >= Canvas.GetLeft(Black) && y <= (Canvas.GetTop(Black) + Black.Height) && y >= Canvas.GetTop(Black))
                                    {
                                        Console.Write(fcolor + "\n");
                                        fcolor = 0;
                                        newpoly();
                                    }
                                    else if (x <= (Canvas.GetLeft(Red) + Red.Width) && x >= Canvas.GetLeft(Red) && y <= (Canvas.GetTop(Red) + Red.Height) && y >= Canvas.GetTop(Red))
                                    {
                                        fcolor = 1;
                                        newpoly();
                                    }
                                    else if (x <= (Canvas.GetLeft(Green) + Green.Width) && x >= Canvas.GetLeft(Green) && y <= (Canvas.GetTop(Green) + Green.Height) && y >= Canvas.GetTop(Green))
                                    {
                                        fcolor = 2;
                                        newpoly();
                                    }
                                    else if (x <= (Canvas.GetLeft(Purple) + Purple.Width) && x >= Canvas.GetLeft(Purple) && y <= (Canvas.GetTop(Purple) + Purple.Height) && y >= Canvas.GetTop(Purple))
                                    {
                                        fcolor = 3;
                                        newpoly();
                                    }
                                    else if (x <= (Canvas.GetLeft(Yellow) + Yellow.Width) && x >= Canvas.GetLeft(Yellow) && y <= (Canvas.GetTop(Yellow) + Yellow.Height) && y >= Canvas.GetTop(Yellow))
                                    {
                                        fcolor = 4;
                                        newpoly();
                                    }
                                    else if (x <= (Canvas.GetLeft(Blue) + Blue.Width) && x >= Canvas.GetLeft(Blue) && y <= (Canvas.GetTop(Blue) + Blue.Height) && y >= Canvas.GetTop(Blue))
                                    {
                                        fcolor = 5;
                                        newpoly();
                                    }
                                    else if (x <= (Canvas.GetLeft(eraser) + eraser.Width) && x >= Canvas.GetLeft(eraser) && y <= (Canvas.GetTop(eraser) + eraser.Height) && y >= Canvas.GetTop(eraser))
                                    {
                                        fcolor = 6;
                                        newpoly();
                                    }
                                    else if (x <= 1779)
                                    {
                                        if (dppaint != null)
                                        {
                                            dppaint.Points.Add(new Point {
                                                X = x, Y = y
                                            });
                                        }
                                    }
                                }
                                else if (handRight.TrackingState != TrackingState.NotTracked && body.HandRightState != HandState.Closed)
                                {
                                    if (fgpoly == true)
                                    {
                                        newpoly();

                                        fgpoly = false;
                                    }
                                }


                                if (handRight.TrackingState != TrackingState.NotTracked)
                                {
                                    if (!double.IsInfinity(x - (fpoint.Width / 2.0)) && !double.IsInfinity(y - (fpoint.Height / 2.0)))
                                    {
                                        fpoint.Visibility = Visibility.Visible;
                                        Canvas.SetLeft(fpoint, x - (fpoint.Width / 2.0));
                                        Canvas.SetTop(fpoint, y - (fpoint.Height / 2.0));
                                        //dc.DrawEllipse(paint.Stroke, null, new Point(handRightPoint.X, handRightPoint.Y), 10, 10);
                                        //img.PointToScreen(new Point(handRightPoint.X, handRightPoint.Y));
                                    }
                                }

                                this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, this.colorFrameDescription.Width, this.colorFrameDescription.Height));
                            }
                            else
                            {
                                Point  right    = new Point(handRightPoint.X, handRightPoint.Y);
                                Point  left     = new Point(handLeftPoint.X, handLeftPoint.Y);
                                double distance = Point.Subtract(left, right).Length;

                                // if(handRight.TrackingState != TrackingState.NotTracked && Button.)
                                if (distance <= 200 && handLeft.TrackingState != TrackingState.NotTracked && handRight.TrackingState != TrackingState.NotTracked && body.HandLeftState == HandState.Closed && body.HandRightState != HandState.Closed)
                                {
                                    // Console.Write(x + " " + Canvas.GetLeft(circle) + "\n");
                                    // if (!float.IsInfinity(x) && !float.IsInfinity(y))
                                    //{

                                    // DRAW!
                                    //trail.Points.Add(new Point { X = x, Y = y });

                                    // right = new Point(handRightPoint.X, handRightPoint.Y);
                                    // left = new Point(handLeftPoint.X, handLeftPoint.Y);
                                    //distance = Point.Subtract(left, right).Length;


                                    if (((Canvas.GetLeft(circle) + circle.Width) - left.X) <= circle.Width + 100 && (right.X - (Canvas.GetLeft(circle))) <= circle.Width + 100 && Math.Abs((Canvas.GetTop(circle) + circle.Height / 2.0) - left.Y) <= circle.Height / 2.0 && Math.Abs(right.Y - (Canvas.GetTop(circle) + circle.Height / 2.0)) <= circle.Height / 2.0 || cirf == true)
                                    {
                                        cirf = true;
                                        if (temp != 0)
                                        {
                                            if (circle.Width + (temp - distance) >= 10)
                                            {
                                                circle.Width += (temp - distance);
                                            }
                                            if (circle.Height + (temp - distance) >= 10)
                                            {
                                                circle.Height += (temp - distance);
                                            }
                                        }
                                        temp = distance;
                                    }
                                    else if (((Canvas.GetLeft(ellipse) + ellipse.Width) - left.X) <= ellipse.Width + 100 && (right.X - (Canvas.GetLeft(ellipse) + ellipse.Width)) <= ellipse.Width + 100 && Math.Abs((Canvas.GetTop(ellipse) + ellipse.Height / 2.0) - left.Y) <= ellipse.Height / 2.0 && Math.Abs(right.Y - (Canvas.GetTop(ellipse) + ellipse.Height / 2.0)) <= circle.Height / 2.0 || ellf == true)
                                    {
                                        ellf = true;
                                        if (temp != 0)
                                        {
                                            if (ellipse.Width + (temp - distance) >= 10)
                                            {
                                                ellipse.Width += (temp - distance);
                                            }
                                            if (ellipse.Height + (temp - distance) >= 10)
                                            {
                                                ellipse.Height += (temp - distance);
                                            }
                                        }
                                        temp = distance;
                                    }
                                    else if (((Canvas.GetLeft(rec) + rec.Width) - left.X) <= rec.Width + 100 && (right.X - (Canvas.GetLeft(rec) + rec.Width)) <= rec.Width + 100 && Math.Abs((Canvas.GetTop(rec) + rec.Height / 2.0) - left.Y) <= rec.Height / 2.0 && Math.Abs(right.Y - (Canvas.GetTop(rec) + rec.Height / 2.0)) <= rec.Height / 2.0 || recf == true)
                                    {
                                        recf = true;
                                        if (temp != 0)
                                        {
                                            if (rec.Width + (temp - distance) >= 10)
                                            {
                                                rec.Width += (temp - distance);
                                            }
                                            if (rec.Height + (temp - distance) >= 10)
                                            {
                                                rec.Height += (temp - distance);
                                            }
                                        }
                                        temp = distance;
                                    }
                                    else if (((Canvas.GetLeft(square) + square.Width) - left.X) <= square.Width + 100 && (right.X - (Canvas.GetLeft(square) + square.Width)) <= square.Width + 100 && Math.Abs((Canvas.GetTop(square) + square.Height / 2.0) - left.Y) <= square.Height / 2.0 && Math.Abs(right.Y - (Canvas.GetTop(square) + square.Height / 2.0)) <= square.Height / 2.0 || squrf == true)
                                    {
                                        squrf = true;
                                        if (temp != 0)
                                        {
                                            if (square.Width + (temp - distance) >= 10)
                                            {
                                                square.Width += (temp - distance);
                                            }
                                            if (square.Height + (temp - distance) >= 10)
                                            {
                                                square.Height += (temp - distance);
                                            }
                                        }
                                        temp = distance;
                                    }
                                }

                                else if (handRight.TrackingState != TrackingState.NotTracked && body.HandRightState == HandState.Closed)
                                {
                                    float x = handRightPoint.X;
                                    float y = handRightPoint.Y;
                                    Console.Write(x + " " + Canvas.GetLeft(circle) + "\n");
                                    // if (!float.IsInfinity(x) && !float.IsInfinity(y))
                                    //{

                                    // DRAW!
                                    //trail.Points.Add(new Point { X = x, Y = y });

                                    if ((x < (Canvas.GetLeft(circle) + circle.Width) && x > Canvas.GetLeft(circle) && y < (Canvas.GetTop(circle) + circle.Height) && y > Canvas.GetTop(circle)) || cirf == true)
                                    {
                                        cirf = true;
                                        Canvas.SetLeft(circle, x - (circle.Width / 2.0));
                                        Canvas.SetTop(circle, y - circle.Height / 2.0);
                                    }
                                    else if ((x < (Canvas.GetLeft(ellipse) + ellipse.Width) && x > Canvas.GetLeft(ellipse) && y < (Canvas.GetTop(ellipse) + ellipse.Height) && y > Canvas.GetTop(ellipse)) || ellf == true)
                                    {
                                        ellf = true;
                                        Canvas.SetLeft(ellipse, x - ellipse.Width / 2.0);
                                        Canvas.SetTop(ellipse, y - ellipse.Height);
                                    }
                                    else if ((x < (Canvas.GetLeft(rec) + rec.Width) && x > Canvas.GetLeft(rec) && y < (Canvas.GetTop(rec) + rec.Height) && y > Canvas.GetTop(rec)) || recf == true)
                                    {
                                        recf = true;
                                        Canvas.SetLeft(rec, x - rec.Width / 2.0);
                                        Canvas.SetTop(rec, y - rec.Height / 2.0);
                                    }
                                    else if ((x < (Canvas.GetLeft(square) + square.Width) && x > Canvas.GetLeft(square) && y < (Canvas.GetTop(square) + square.Height) && y > Canvas.GetTop(square)) || squrf == true)
                                    {
                                        squrf = true;
                                        Canvas.SetLeft(square, x - square.Width / 2.0);
                                        Canvas.SetTop(square, y - square.Height);
                                    }
                                    //}
                                }
                                else
                                {
                                    cirf = squrf = recf = ellf = false;
                                    temp = 0;
                                }
                            }
                        }
                    }
                }
            }
        }
        public unsafe void Update(ushort[] depthData, byte[] bodyIndexData, Body body)
        {
            double handLength = 0.0;
            double barLength  = 0.0;
            double barHeight  = 0.0;
            double angle      = 0.0;

            Joint shoulderLeft  = body.Joints[JointType.ShoulderLeft];
            Joint shoulderRight = body.Joints[JointType.ShoulderRight];
            Joint chest         = body.Joints[JointType.SpineShoulder];
            Joint waist         = body.Joints[JointType.SpineBase];
            Joint elbowLeft     = body.Joints[JointType.ElbowLeft];
            Joint elbowRight    = body.Joints[JointType.ElbowRight];
            Joint handLeft      = body.Joints[JointType.HandLeft];
            Joint handRight     = body.Joints[JointType.HandRight];
            Joint footLeft      = body.Joints[JointType.FootLeft];
            Joint footRight     = body.Joints[JointType.FootRight];

            if (waist.TrackingState == TrackingState.NotTracked)
            {
                return;
            }

            if (waist.Position.Z < 2.0f || waist.Position.Z > 4.5f)
            {
                return;
            }

            if (shoulderLeft.TrackingState != TrackingState.NotTracked && shoulderRight.TrackingState != TrackingState.NotTracked &&
                elbowLeft.TrackingState != TrackingState.NotTracked && elbowRight.TrackingState != TrackingState.NotTracked &&
                handLeft.TrackingState != TrackingState.NotTracked && handRight.TrackingState != TrackingState.NotTracked)
            {
                handLength =
                    shoulderLeft.Position.Length(shoulderRight.Position) +
                    shoulderLeft.Position.Length(elbowLeft.Position) +
                    shoulderRight.Position.Length(elbowRight.Position) +
                    elbowLeft.Position.Length(handLeft.Position) +
                    elbowRight.Position.Length(handRight.Position);
            }

            CoordinateMapper.MapColorFrameToDepthSpace(depthData, _depthPoints);

            fixed(DepthSpacePoint *colorMappedToDepthPointsPointer = _depthPoints)
            {
                int    minimumX        = int.MaxValue;
                int    maximumX        = int.MinValue;
                int    minimumY        = int.MaxValue;
                int    maximumY        = int.MinValue;
                ushort minimumDistance = 0;
                ushort maximumdistance = 0;

                for (int colorIndex = 0; colorIndex < _depthPoints.Length; ++colorIndex)
                {
                    float colorMappedToDepthX = colorMappedToDepthPointsPointer[colorIndex].X;
                    float colorMappedToDepthY = colorMappedToDepthPointsPointer[colorIndex].Y;

                    if (!float.IsNegativeInfinity(colorMappedToDepthX) &&
                        !float.IsNegativeInfinity(colorMappedToDepthY))
                    {
                        int depthX = (int)(colorMappedToDepthX + 0.5f);
                        int depthY = (int)(colorMappedToDepthY + 0.5f);

                        if ((depthX >= 0) && (depthX < DepthWidth) && (depthY >= 0) && (depthY < DepthHeight))
                        {
                            int    depthIndex = (depthY * DepthWidth) + depthX;
                            ushort depth      = depthData[depthIndex];

                            if (bodyIndexData[depthIndex] != 0xff)
                            {
                                if (depthX < minimumX)
                                {
                                    minimumX        = depthX;
                                    minimumY        = depthY;
                                    minimumDistance = depth;
                                }
                                if (depthX > maximumX)
                                {
                                    maximumX        = depthX;
                                    maximumY        = depthY;
                                    maximumdistance = depth;
                                }
                                continue;
                            }
                        }
                    }
                }

                DepthSpacePoint depthMinimum = new DepthSpacePoint
                {
                    X = minimumX,
                    Y = minimumY
                };

                DepthSpacePoint depthMaximum = new DepthSpacePoint
                {
                    X = maximumX,
                    Y = maximumY
                };

                CameraSpacePoint cameraMinimum = CoordinateMapper.MapDepthPointToCameraSpace(depthMinimum, minimumDistance);
                CameraSpacePoint cameraMaximum = CoordinateMapper.MapDepthPointToCameraSpace(depthMaximum, maximumdistance);

                ColorSpacePoint colorMinimum = CoordinateMapper.MapDepthPointToColorSpace(depthMinimum, minimumDistance);
                ColorSpacePoint colorMaximum = CoordinateMapper.MapDepthPointToColorSpace(depthMaximum, maximumdistance);

                CameraSpacePoint cameraTrail = new CameraSpacePoint
                {
                    X = (cameraMinimum.X + cameraMaximum.X) / 2f,
                    Y = (cameraMinimum.Y + cameraMaximum.Y) / 2f,
                    Z = (cameraMinimum.Z + cameraMaximum.Z) / 2f
                };

                ColorSpacePoint colorTrail = new ColorSpacePoint
                {
                    X = (colorMinimum.X + colorMaximum.X) / 2f,
                    Y = (colorMinimum.Y + colorMaximum.Y) / 2f
                };

                DepthSpacePoint depthTrail = new DepthSpacePoint
                {
                    X = (depthMinimum.X + depthMaximum.X) / 2f,
                    Y = (depthMinimum.Y + depthMaximum.Y) / 2f
                };

                CameraSpacePoint feet = new CameraSpacePoint
                {
                    X = (footLeft.Position.X + footRight.Position.X) / 2f,
                    Y = (footLeft.Position.Y + footRight.Position.Y) / 2f,
                    Z = (footLeft.Position.Z + footRight.Position.Z) / 2f
                };

                CameraSpacePoint projection = new CameraSpacePoint
                {
                    X = cameraTrail.X,
                    Y = feet.Y,
                    Z = cameraTrail.Z
                };

                barLength = cameraMinimum.Length(cameraMaximum);
                barHeight = cameraTrail.Length(projection);

                angle = cameraMinimum.Angle(cameraMaximum, new CameraSpacePoint {
                    X = cameraMaximum.X, Y = cameraMinimum.Y, Z = (cameraMaximum.Z + cameraMinimum.Z) / 2f
                });

                if (angle > 180.0)
                {
                    angle = 360.0 - angle;
                }

                if (cameraMinimum.Y < cameraMaximum.Y)
                {
                    angle = -angle;
                }

                if (barLength > handLength && Math.Abs(angle) < 25.0)
                {
                    BarDetectionResult result = new BarDetectionResult
                    {
                        Minimum = new MultiPoint
                        {
                            CameraPoint = cameraMinimum,
                            ColorPoint  = colorMinimum,
                            DepthPoint  = depthMinimum
                        },
                        Maximum = new MultiPoint
                        {
                            CameraPoint = cameraMaximum,
                            ColorPoint  = colorMaximum,
                            DepthPoint  = depthMaximum
                        },
                        Trail = new MultiPoint
                        {
                            CameraPoint = cameraTrail,
                            ColorPoint  = colorTrail,
                            DepthPoint  = depthTrail
                        },
                        BarHeight = barHeight + HEIGHT_DIFFERENCE,
                        BarLength = barLength,
                        Angle     = angle
                    };

                    BarDetected?.Invoke(this, result);
                }
            }
        }
 private bool checkStart(ColorSpacePoint joint_head, ColorSpacePoint joint_handLeft, ColorSpacePoint joint_handRight, int i)
 {
     //通过判断左手或右手关节点与头结点的距离是否小于7厘米来触发变脸;单位:毫米
     if (startChangeFace[i] == false)
     {
         startChangeFace[i] = ((Math.Abs(joint_handLeft.X - joint_head.X) < 70 && Math.Abs(joint_handLeft.Y - joint_head.Y) < 70) ||
                               (Math.Abs(joint_handRight.X - joint_head.X) < 70 && Math.Abs(joint_handRight.Y - joint_head.Y) < 70));
     }
     return(false);
 }
Exemple #45
0
        public Point MapJointToColor(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;
            ColorSpacePoint[] points = new ColorSpacePoint[1];
            kinect.CoordinateMapper.MapCameraPointsToColorSpace(skelPoints, points);

            //Convert back to the base object type
            mappedPoint.X = points[0].X;
            mappedPoint.Y = points[0].Y;

            return mappedPoint;
        }
        /// <summary>
        /// 人が映った画素領域だけを映した BitmapSource を取得します。
        /// </summary>
        /// <param name="colorFrame">
        /// カラー画像フレーム。
        /// </param>
        /// <param name="depthFrame">
        /// 深度フレーム。
        /// </param>
        /// <param name="bodyIndexFrame">
        /// BodyIndex フレーム。
        /// </param>
        /// <returns>
        /// 人が映った画素領域だけを映した BitmapSource。
        /// </returns>
        BitmapSource GetUserMaskImage(ColorFrame colorFrame,
                                      DepthFrame depthFrame,
                                      BodyIndexFrame bodyIndexFrame)
        {
            byte[] colors = new byte[this.colorFrameDescription.Width
                                     * this.colorFrameDescription.Height
                                     * this.colorFrameDescription.BytesPerPixel];

            colorFrame.CopyConvertedFrameDataToArray(colors, this.colorImageFormat);

            ushort[] depths = new ushort[this.depthFrameDescription.Width
                                         * this.depthFrameDescription.Height];
            depthFrame.CopyFrameDataToArray(depths);

            byte[] bodyIndexes = new byte[this.bodyIndexFrameDescription.Width
                                          * this.bodyIndexFrameDescription.Height];
            bodyIndexFrame.CopyFrameDataToArray(bodyIndexes);


            //人が映っているだけの画像を表す byte 配列を用意して 0 で初期化する。
            byte[] bodyColors = new byte[this.bodyIndexFrameDescription.Width
                                         * this.bodyIndexFrameDescription.Height
                                         * this.colorFrameDescription.BytesPerPixel];
            Array.Clear(bodyColors, 0, bodyColors.Length);

            //カラー画像と深度画像の座標を対応させる(= マッピングする)必要がある。
            //ColorSpacePoint はカラー画像の座標を示す構造体。
            ColorSpacePoint[] colorSpacePoints
                = new ColorSpacePoint[this.bodyIndexFrameDescription.Width
                                      * this.bodyIndexFrameDescription.Height];
            this.kinect.CoordinateMapper.MapDepthFrameToColorSpace(depths, colorSpacePoints);

            for (int y = 0; y < this.depthFrameDescription.Height; y++)
            {
                for (int x = 0; x < this.depthFrameDescription.Width; x++)
                {
                    int depthsIndex = y * this.depthFrameDescription.Width + x;

                    if (bodyIndexes[depthsIndex] != 255)
                    {
                        //対応するカラー画像の座標を取得し、
                        //カラー画像座標系の範囲内に収まるかを判定する。
                        ColorSpacePoint colorSpacePoint = colorSpacePoints[depthsIndex];
                        int             colorX          = (int)colorSpacePoint.X;
                        int             colorY          = (int)colorSpacePoint.Y;

                        if ((colorSpacePoint.X >= 0) &&
                            (colorSpacePoint.X < this.colorFrameDescription.Width) &&
                            (colorSpacePoint.Y >= 0) &&
                            (colorSpacePoint.Y < this.colorFrameDescription.Height))
                        {
                            //対応するカラー画像の画素から色情報を取得して、
                            //新しいカラー画像のデータに与える。
                            int colorsIndex =
                                (colorY * this.colorFrameDescription.Width + colorX) * 4;

                            int bodyColorsIndex = depthsIndex * 4;

                            bodyColors[bodyColorsIndex]     = colors[colorsIndex];
                            bodyColors[bodyColorsIndex + 1] = colors[colorsIndex + 1];
                            bodyColors[bodyColorsIndex + 2] = colors[colorsIndex + 2];
                            bodyColors[bodyColorsIndex + 3] = 255;
                        }
                    }
                }
            }

            BitmapSource bitmapSource
                = BitmapSource.Create(depthFrameDescription.Width,
                                      depthFrameDescription.Height,
                                      96,
                                      96,
                                      PixelFormats.Bgra32,
                                      null,
                                      bodyColors,
                                      depthFrameDescription.Width * 4);

            return(bitmapSource);
        }
Exemple #47
0
 public SpacePointBase(ColorSpacePoint pt) : this(pt.X, pt.Y, SpaceMode.Color)
 {
 }
 /// <summary>
 /// Updates the position of the cursor.
 /// </summary>
 /// <param name="point">The specified point in the color space.</param>
 /// <param name="ratioX">The specified horizontal scale scale ratio.</param>
 /// <param name="ratioY">The specified vertical scale ratio.</param>
 public void Update(ColorSpacePoint point, double ratioX = 1.0, double ratioY = 1.0)
 {
     Update(point.X * ratioX, point.Y * ratioY);
 }
Exemple #49
0
        /// <summary>
        /// Procedure invoked by Kinect when new data are available
        /// </summary>
        private void MultisourceFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e)
        {
            if (KinectSensor == null || Reader == null)
            {
                return;
            }

            // acquire frame data
            MultiSourceFrame multiSourceFrame = e.FrameReference.AcquireFrame();

            // if the Frame has expired by the time we process this event, return.
            if (multiSourceFrame == null)
            {
                return;
            }

            // Continue only if buffer is empty
            if (!Buffer.IsEmpty())
            {
                return;
            }

            // declare variables for data from sensor
            ColorFrame    colorFrame    = null;
            DepthFrame    depthFrame    = null;
            InfraredFrame infraredFrame = null;

            byte[]             colorFrameData                 = null;
            ushort[]           depthData                      = null;
            ushort[]           infraredData                   = null;
            DepthSpacePoint[]  pointsFromColorToDepth         = null;
            ColorSpacePoint[]  pointsFromDepthToColor         = null;
            CameraSpacePoint[] cameraSpacePointsFromDepthData = null;

            try
            {
                // get frames from sensor
                colorFrame    = multiSourceFrame.ColorFrameReference.AcquireFrame();
                depthFrame    = multiSourceFrame.DepthFrameReference.AcquireFrame();
                infraredFrame = multiSourceFrame.InfraredFrameReference.AcquireFrame();

                // If any frame has expired by the time we process this event, return.
                if (colorFrame == null || depthFrame == null || infraredFrame == null)
                {
                    return;
                }

                // use frame data to fill arrays
                colorFrameData = new byte[ColorFrameDescription.LengthInPixels * 4];
                depthData      = new ushort[DepthFrameDescription.LengthInPixels];
                infraredData   = new ushort[InfraredFrameDescription.LengthInPixels];

                colorFrame.CopyConvertedFrameDataToArray(colorFrameData, ColorImageFormat.Bgra);
                depthFrame.CopyFrameDataToArray(depthData);
                infraredFrame.CopyFrameDataToArray(infraredData);

                pointsFromColorToDepth         = new DepthSpacePoint[ColorFrameDescription.LengthInPixels];
                pointsFromDepthToColor         = new ColorSpacePoint[DepthFrameDescription.LengthInPixels];
                cameraSpacePointsFromDepthData = new CameraSpacePoint[DepthFrameDescription.LengthInPixels];

                using (KinectBuffer depthFrameData = depthFrame.LockImageBuffer())
                {
                    CoordinateMapper.MapColorFrameToDepthSpaceUsingIntPtr(
                        depthFrameData.UnderlyingBuffer,
                        depthFrameData.Size,
                        pointsFromColorToDepth);

                    CoordinateMapper.MapDepthFrameToColorSpaceUsingIntPtr(
                        depthFrameData.UnderlyingBuffer,
                        depthFrameData.Size,
                        pointsFromDepthToColor);

                    CoordinateMapper.MapDepthFrameToCameraSpaceUsingIntPtr(
                        depthFrameData.UnderlyingBuffer,
                        depthFrameData.Size,
                        cameraSpacePointsFromDepthData);
                }
            }
            finally
            {
                // dispose frames so that Kinect can continue processing
                colorFrame?.Dispose();
                depthFrame?.Dispose();
                infraredFrame?.Dispose();

                // send data futher
                if (
                    colorFrameData != null &&
                    depthData != null &&
                    infraredData != null &&
                    cameraSpacePointsFromDepthData != null
                    )
                {
                    // store data to buffer and notify processing thread
                    Buffer.Store(
                        new KinectData(
                            colorFrameData,
                            depthData,
                            infraredData,
                            cameraSpacePointsFromDepthData,
                            pointsFromColorToDepth,
                            pointsFromDepthToColor
                            )
                        );

                    TrackingManager.SendKinectUpdate();
                }
            }
        }
 /// <summary>
 /// Copies face color points to the gpu
 /// </summary>
 /// <param name="context">Device context</param>
 /// <param name="points">Points array</param>
 public void Copy(DeviceContext context, ColorSpacePoint[] points)
 {
     Copy(context, points, points.Length);
 }
        /// <summary>
        /// 触发变脸的方法
        /// </summary>
        /// <param name="joint_head"></param>
        /// <param name="joint_handLeft"></param>
        /// <param name="joint_handRight"></param>
        /// <returns></returns>
        private bool changeFaceMethod(ColorSpacePoint joint_head, ColorSpacePoint joint_handLeft, ColorSpacePoint joint_handRight)
        {
            //通过判断左手或右手关节点与头结点的距离是否小于7厘米来触发变脸;单位:毫米
            bool isOverMatrix = ((Math.Abs(joint_handLeft.X - joint_head.X) < 70 && Math.Abs(joint_handLeft.Y - joint_head.Y) < 70) ||
                                 (Math.Abs(joint_handRight.X - joint_head.X) < 70 && Math.Abs(joint_handRight.Y - joint_head.Y) < 70));

            return(isOverMatrix);
        }
Exemple #52
0
        public ColorPointCloud(ColorFrame iColorFrame, DepthFrame iDepthFrame, BodyIndexFrame iBodyIndexFrame, CoordinateMapper iCoordinateMapper)
        {
            TimeStamp = DateTime.Now;

            Width  = iDepthFrame.FrameDescription.Width;
            Height = iDepthFrame.FrameDescription.Height;
            var depthData = new ushort[Width * Height];

            //BodyData = new int[Width * Height];

            iDepthFrame.CopyFrameDataToArray(depthData);

            var bodyData = new byte[Width * Height];

            iBodyIndexFrame.CopyFrameDataToArray(bodyData);

            var colorWidth  = iColorFrame.FrameDescription.Width;
            var colorHeight = iColorFrame.FrameDescription.Height;
            var colorData   = new byte[colorWidth * colorHeight * KinectConstants.BYTES_PER_PIXEL];

            if (iColorFrame.RawColorImageFormat == ColorImageFormat.Bgra)
            {
                iColorFrame.CopyRawFrameDataToArray(colorData);
            }
            else
            {
                iColorFrame.CopyConvertedFrameDataToArray(colorData, ColorImageFormat.Bgra);
            }

            var colorPoints = new ColorSpacePoint[Width * Height];

            iCoordinateMapper.MapDepthFrameToColorSpace(depthData, colorPoints);

            var displayPixels = new byte[Width * Height * KinectConstants.BYTES_PER_PIXEL];

            Array.Clear(displayPixels, 0, displayPixels.Length);

            var bodyDataBool          = new bool[Width * Height];
            var colorDataWithoutAlpha = new byte[Width * Height * 3];

            for (var y = 0; y < Height; ++y)
            {
                for (var x = 0; x < Width; ++x)
                {
                    var depthIndex = (y * Width) + x;

                    var player = bodyData[depthIndex];

                    if (player != 0xff)
                    {
                        bodyDataBool[depthIndex] = true;

                        var colorPoint = colorPoints[depthIndex];

                        var colorX = (int)Math.Floor(colorPoint.X + 0.5);
                        var colorY = (int)Math.Floor(colorPoint.Y + 0.5);

                        if ((colorX >= 0) && (colorX < colorWidth) && (colorY >= 0) && (colorY < colorHeight))
                        {
                            var colorIndex   = ((colorY * colorWidth) + colorX) * KinectConstants.BYTES_PER_PIXEL;
                            var displayIndex = depthIndex * KinectConstants.BYTES_PER_PIXEL;

                            displayPixels[displayIndex + 0] = colorData[colorIndex];     // B
                            displayPixels[displayIndex + 1] = colorData[colorIndex + 1]; // G
                            displayPixels[displayIndex + 2] = colorData[colorIndex + 2]; // R
                            displayPixels[displayIndex + 3] = 0xff;                      // Alpha

                            var colorDataIndex = depthIndex * 3;
                            colorDataWithoutAlpha[colorDataIndex + 0] = colorData[colorIndex + 0];
                            colorDataWithoutAlpha[colorDataIndex + 1] = colorData[colorIndex + 1];
                            colorDataWithoutAlpha[colorDataIndex + 2] = colorData[colorIndex + 2];
                        }
                    }
                }
            }

            DisplayPixels = displayPixels;

            BodyData  = bodyDataBool;
            DepthData = depthData;
            ColorData = colorDataWithoutAlpha;
        }
 private bool checkEnd(ColorSpacePoint joint_head, ColorSpacePoint joint_handLeft, ColorSpacePoint joint_handRight, int i)
 {
     //通过判断左手或右手关节点与头结点的距离是否小于7厘米来触发变脸;单位:毫米
     if (startChangeFace[i] == true)
     {
         return(-(joint_handLeft.Y - joint_head.Y) > 100 && -(joint_handRight.Y - joint_head.Y) > 100);
     }
     else
     {
         return(false);
     }
 }
Exemple #54
0
        public static void DrawLine(this Canvas canvas, Joint first, Joint second, KinectSensor _sensor)
        {
            if (first.TrackingState == TrackingState.NotTracked || second.TrackingState == TrackingState.NotTracked)
            {
                return;
            }

            //first = first.ScaleTo(canvas.ActualWidth, canvas.ActualHeight);
            //second = second.ScaleTo(canvas.ActualWidth, canvas.ActualHeight);


            // 3D space point
            // CameraSpacePoint jointPosition = joint.Position;

            CameraSpacePoint firstPosition  = first.Position;
            CameraSpacePoint secondPosition = second.Position;

            // 2D space point
            //Point point = new Point();
            Point firstPoint  = new Point();
            Point secondPoint = new Point();

            ColorSpacePoint firstColorPoint  = _sensor.CoordinateMapper.MapCameraPointToColorSpace(firstPosition);
            ColorSpacePoint secondColorPoint = _sensor.CoordinateMapper.MapCameraPointToColorSpace(secondPosition);

            //DepthSpacePoint depthPoint = _sensor.CoordinateMapper.MapCameraPointToDepthSpace(jointPosition);

            // point.X = float.IsInfinity(colorPoint.X) ? 0 : colorPoint.X;

            firstPoint.X  = float.IsInfinity(firstColorPoint.X) ? 0 : firstColorPoint.X;
            secondPoint.X = float.IsInfinity(secondColorPoint.X) ? 0 : secondColorPoint.X;
            // point.X = float.IsInfinity(depthPoint.X) ? 0 : depthPoint.X;

            //point.Y = float.IsInfinity(colorPoint.Y) ? 0 : colorPoint.Y;
            firstPoint.Y  = float.IsInfinity(firstColorPoint.Y) ? 0 : firstColorPoint.Y;
            secondPoint.Y = float.IsInfinity(secondColorPoint.Y) ? 0 : secondColorPoint.Y;

            // point.Y = float.IsInfinity(depthPoint.Y) ? 0 : depthPoint.Y;

            // Draw
            Ellipse ellipse = new Ellipse
            {
                Fill   = Brushes.Red,
                Width  = 6,
                Height = 6
            };

            /*       Canvas.SetLeft(ellipse, point.X - ellipse.Width / 2);
             *     Canvas.SetTop(ellipse, point.Y - ellipse.Height / 2);
             *
             *     canvas.Children.Add(ellipse); */

            /* Line line = new Line
             * {
             *   X1 = first.Position.X,
             *   Y1 = first.Position.Y,
             *   X2 = second.Position.X,
             *   Y2 = second.Position.Y,
             *   StrokeThickness = 8,
             *   Stroke = new SolidColorBrush(Colors.Blue)
             * }; */

            Line line = new Line
            {
                X1 = firstPoint.X,
                Y1 = firstPoint.Y,
                X2 = secondPoint.X,
                Y2 = secondPoint.Y,
                StrokeThickness = 8,
                Stroke          = new SolidColorBrush(Colors.Blue)
            };

            canvas.Children.Add(line);
        }
    private void RefreshData(ushort[] depthData)
    {
        var frameDesc = _Sensor.DepthFrameSource.FrameDescription;

        ColorSpacePoint[] colorSpace = new ColorSpacePoint[depthData.Length];
        _Mapper.MapDepthFrameToColorSpace(depthData, colorSpace);
        Hashtable depthCount = new Hashtable();

        if (_flagDeskDepth)
        {
            int maxCount = 0;
            for (int i = 0; i < depthData.Length; i++)
            {
                if (depthCount.ContainsKey(depthData[i]))
                {
                    if ((int)depthData[i] != 0)
                    {
                        depthCount[depthData[i]] = (int)depthCount[depthData[i]] + 1;
                    }
                    if ((int)depthCount[depthData[i]] > maxCount)
                    {
                        maxCount     = (int)depthCount[depthData[i]];
                        _maxCountKey = depthData[i];
                    }
                }
                else
                {
                    depthCount.Add(depthData[i], 1);
                }
            }
            _flagDeskDepth = false;
            Debug.Log(_maxCountKey);
            //Debug.Log("Max" + maxCountKey);
        }
        //foreach (ushort key in depthCount.Keys)
        //{
        //    if((int)depthCount[key] > maxCount)
        //    {
        //        maxCount = (int)depthCount[key];
        //        maxCountKey = key;
        //    }
        //}
        //Debug.Log(maxCountKey);
        //設定存放depth的Mat大小
        _Depth = new Mat(frameDesc.Height / _DownsampleSize, frameDesc.Width / _DownsampleSize, CvType.CV_8UC1);

        for (int y = 0; y < frameDesc.Height; y += _DownsampleSize)
        {
            for (int x = 0; x < frameDesc.Width; x += _DownsampleSize)
            {
                int indexX     = x / _DownsampleSize;
                int indexY     = y / _DownsampleSize;
                int smallIndex = (indexY * (frameDesc.Width / _DownsampleSize)) + indexX;

                double avg = GetAvg(depthData, x, y, frameDesc.Width, frameDesc.Height);

                //avg = avg * _DepthScale;

                if (indexX == 64 && indexY == 53 && _kinectDistance != null)
                {
                    _kinectDistance.text = "distance: " + avg;
                }
                if (x == 0 && y == 0)
                {
                    Distance_UpLeft = avg;
                }
                if (x == (frameDesc.Width - _DownsampleSize) && y == 0)
                {
                    Distance_UpRight = avg;
                }
                if (x == 0 && y == (frameDesc.Height - _DownsampleSize))
                {
                    Distance_DownLeft = avg;
                }
                if (x == (frameDesc.Width - _DownsampleSize) && y == (frameDesc.Height - _DownsampleSize))
                {
                    Distance_DonwRight = avg;
                }
                //距離1000mm正負200mm

                //avg = (avg > (double)(_maxCountKey + _binaryIndex)) ? 0 : 255;
                avg = 255 - (avg / 4000 * 255);
                avg = (avg == 255) ? 0 : avg;

                _Vertices[smallIndex].z = (float)avg;
                _Depth.put(y / _DownsampleSize, frameDesc.Width / _DownsampleSize - x / _DownsampleSize, avg);
                // Update UV mapping with CDRP
                //var colorSpacePoint = colorSpace[(y * frameDesc.Width) + x];
                // _UV[smallIndex] = new Vector2(colorSpacePoint.X / colorWidth, colorSpacePoint.Y / colorHeight);
            }
        }
    }
        private void BodyFrameArrived(object sender, BodyFrameArrivedEventArgs e)
        {
            if (null == e.FrameReference)
            {
                return;
            }

            // Remove all the old points and clipping lines
            while (0 < KinectCanvas.Children.Count)
            {
                KinectCanvas.Children.RemoveAt(0);
            }

            // If you do not dispose of the frame, you never get another one...
            using (BodyFrame _BodyFrame = e.FrameReference.AcquireFrame()) {
                if (null == _BodyFrame)
                {
                    return;
                }

                Body[] _Bodies = new Body[_BodyFrame.BodyFrameSource.BodyCount];
                _BodyFrame.GetAndRefreshBodyData(_Bodies);

                foreach (Body _Body in _Bodies)
                {
                    if (_Body.IsTracked)
                    {
                        foreach (Joint _Joint in _Body.Joints.Values)
                        {
                            if (TrackingState.Tracked == _Joint.TrackingState)
                            {
                                Ellipse _Ellipse = new Ellipse();
                                _Ellipse.Stroke = Brushes.Green;
                                _Ellipse.Fill   = Brushes.Green;
                                _Ellipse.Width  = 20;
                                _Ellipse.Height = 20;

                                ColorSpacePoint _ColorSpacePoint = Sensor.CoordinateMapper.MapCameraPointToColorSpace(_Joint.Position);
                                Canvas.SetLeft(_Ellipse, _ColorSpacePoint.X);
                                Canvas.SetTop(_Ellipse, _ColorSpacePoint.Y);
                                KinectCanvas.Children.Add(_Ellipse);
                            }
                        }

                        if (FrameEdges.Top == (FrameEdges.Top & _Body.ClippedEdges))
                        {
                            CreateClippingLine(0, 0, KinectCanvas.ActualWidth, 0);
                        }

                        if (FrameEdges.Left == (FrameEdges.Left & _Body.ClippedEdges))
                        {
                            CreateClippingLine(0, 0, 0, KinectCanvas.ActualHeight);
                        }

                        if (FrameEdges.Bottom == (FrameEdges.Bottom & _Body.ClippedEdges))
                        {
                            CreateClippingLine(0, KinectCanvas.ActualHeight, KinectCanvas.ActualWidth, KinectCanvas.ActualHeight);
                        }

                        if (FrameEdges.Right == (FrameEdges.Right & _Body.ClippedEdges))
                        {
                            CreateClippingLine(KinectCanvas.ActualWidth, 0, KinectCanvas.ActualWidth, KinectCanvas.ActualHeight);
                        }
                    }
                }
            }
        }
Exemple #57
0
        static void Main()
        {
            Application.EnableVisualStyles();
            Application.SetCompatibleTextRenderingDefault(false);

            RenderForm form = new RenderForm("Kinect multiple hd faces projected to rgb");

            RenderDevice device = new RenderDevice(SharpDX.Direct3D11.DeviceCreationFlags.BgraSupport | DeviceCreationFlags.Debug);
            RenderContext context = new RenderContext(device);
            DX11SwapChain swapChain = DX11SwapChain.FromHandle(device, form.Handle);

            VertexShader vertexShader = ShaderCompiler.CompileFromFile<VertexShader>(device, "ProjectedTextureHdFaceView.fx", "VS_Simple");
            VertexShader vertexShaderIndexed = ShaderCompiler.CompileFromFile<VertexShader>(device, "ProjectedTextureHdFaceView.fx", "VS_Indexed");
            PixelShader pixelShader = ShaderCompiler.CompileFromFile<PixelShader>(device, "ProjectedTextureHdFaceView.fx", "PS");

            int maxFaceCount = Consts.MaxBodyCount;
            int faceVertexCount = (int)Microsoft.Kinect.Face.FaceModel.VertexCount;

            var vertRgbTempBuffer = new ColorSpacePoint[faceVertexCount];
            ColorSpacePoint[] facePoints = new ColorSpacePoint[faceVertexCount * maxFaceCount];

            DX11StructuredBuffer lookupBuffer = DX11StructuredBuffer.CreateDynamic<uint>(device, maxFaceCount);

            //Note : since in this case we use instancing, we only need a buffer for a single face
            HdFaceIndexBuffer faceIndexBuffer = new HdFaceIndexBuffer(device, 1);

            DynamicRgbSpaceFaceStructuredBuffer faceRgbBuffer = new DynamicRgbSpaceFaceStructuredBuffer(device, maxFaceCount);

            KinectSensor sensor = KinectSensor.GetDefault();
            sensor.Open();

            bool doQuit = false;
            bool invalidateFace = false;

            KinectSensorBodyFrameProvider provider = new KinectSensorBodyFrameProvider(sensor);
            BodyTrackingProcessor bodyTracker = new BodyTrackingProcessor();
            MultipleHdFaceProcessor multiFace = new MultipleHdFaceProcessor(sensor, bodyTracker, maxFaceCount);

            form.KeyDown += (sender, args) => { if (args.KeyCode == Keys.Escape) { doQuit = true; } };

            bool uploadColor = false;
            ColorRGBAFrameData currentData = null;
            DynamicColorRGBATexture colorTexture = new DynamicColorRGBATexture(device);
            KinectSensorColorRGBAFrameProvider colorProvider = new KinectSensorColorRGBAFrameProvider(sensor);
            colorProvider.FrameReceived += (sender, args) => { currentData = args.FrameData; uploadColor = true; };

            provider.FrameReceived += (sender, args) =>
            {
                bodyTracker.Next(args.FrameData);
            };

            multiFace.OnFrameResultsChanged += (sender, args) =>
            {
                invalidateFace = true;
            };

            RenderLoop.Run(form, () =>
            {
                if (doQuit)
                {
                    form.Dispose();
                    return;
                }

                if (invalidateFace)
                {
                    int offset = 0;
                    foreach (var data in multiFace.CurrentResults)
                    {
                        var vertices = data.FaceModel.CalculateVerticesForAlignment(data.FaceAlignment).ToArray();
                        sensor.CoordinateMapper.MapCameraPointsToColorSpace(vertices, vertRgbTempBuffer);
                        Array.Copy(vertRgbTempBuffer, 0, facePoints, offset, faceVertexCount);
                        offset += faceVertexCount;
                    }
                    faceRgbBuffer.Copy(context, facePoints, multiFace.CurrentResults.Count * faceVertexCount);
                    invalidateFace = false;
                }

                if (uploadColor)
                {
                    colorTexture.Copy(context, currentData);
                    uploadColor = false;
                }

                context.Context.ClearRenderTargetView(swapChain.RenderView, SharpDX.Color.Black);
                context.RenderTargetStack.Push(swapChain);

                context.Context.Rasterizer.State = device.RasterizerStates.BackCullSolid;
                context.Context.OutputMerger.BlendState = device.BlendStates.Disabled;
                device.Primitives.ApplyFullTri(context, colorTexture.ShaderView);
                device.Primitives.FullScreenTriangle.Draw(context);

                if (multiFace.CurrentResults.Count > 0)
                {
                    context.Context.VertexShader.SetShaderResource(0, faceRgbBuffer.ShaderView);
                    context.Context.PixelShader.SetSampler(0, device.SamplerStates.LinearClamp);
                    context.Context.PixelShader.SetShaderResource(0, colorTexture.ShaderView);

                    if (multiFace.CurrentResults.Count > 1)
                    {
                        uint[] buffer = new uint[multiFace.CurrentResults.Count];
                        for (uint i = 0; i < multiFace.CurrentResults.Count; i++)
                        {
                            buffer[i] = (uint)((i + 1) % multiFace.CurrentResults.Count);
                        }
                        lookupBuffer.WriteData(context, buffer);

                        context.Context.VertexShader.Set(vertexShaderIndexed);
                        context.Context.VertexShader.SetShaderResource(1, lookupBuffer.ShaderView);
                    }
                    else
                    {
                        context.Context.VertexShader.Set(vertexShader);
                    }

                    context.Context.PixelShader.Set(pixelShader);

                    //Attach index buffer, null topology since we fetch
                    faceIndexBuffer.AttachWithLayout(context);
                    faceIndexBuffer.DrawInstanced(context, multiFace.CurrentResults.Count);
                }

                context.RenderTargetStack.Pop();

                swapChain.Present(0, SharpDX.DXGI.PresentFlags.None);
            });

            swapChain.Dispose();
            context.Dispose();
            device.Dispose();

            colorProvider.Dispose();
            colorTexture.Dispose();

            faceIndexBuffer.Dispose();
            faceRgbBuffer.Dispose();

            provider.Dispose();
            pixelShader.Dispose();
            vertexShader.Dispose();
            vertexShaderIndexed.Dispose();

            lookupBuffer.Dispose();

            sensor.Close();
        }
        /// <summary>
        /// Kinectにつないでる時だけ使えるコンストラクタ
        /// </summary>
        /// <param name="originalCoordinateMapper"></param>
        /// <param name="depthWidth"></param>
        /// <param name="depthHeight"></param>
        public LocalCoordinateMapper(CoordinateMapper originalCoordinateMapper, int depthWidth, int depthHeight)
        {
            this.depthWidth = depthWidth;
            this.depthHeight = depthHeight;
            this.depthFrameToCameraSpaceTable = originalCoordinateMapper.GetDepthFrameToCameraSpaceTable();

            int length = depthWidth * depthHeight;
            ushort[] depthBuffer = new ushort[length];
            ColorSpacePoint[] colorSpacePoints = new ColorSpacePoint[length];            
            this.depthFrameToColorSpacfeTable = new PointF[length];

            int depth = 1500; // なんでもいい
            for (int i = 0; i < length; i++)
            {
                depthBuffer[i] = (ushort)depth;
            }
            originalCoordinateMapper.MapDepthFrameToColorSpace(depthBuffer, colorSpacePoints);
            for (int i = 0; i < length; i++)
            {
                PointF tempP = new PointF();
                tempP.X = (float)(colorSpacePoints[i].X - D / depth);
                tempP.Y = colorSpacePoints[i].Y;
                this.depthFrameToColorSpacfeTable[i] = tempP;
            }
            Debug.WriteLine(1);
        }
        //Processes the Frame data from the Kinect camera.
        //Since events are called synchronously, this would bottleneck and cause an issue with framerate
        //By threading, we process the info on seperate threads, allowing execution to coninue with the rest of the game
        private void ProcessRGBVideo(ColorFrameReference aReference, BodyIndexFrameReference bifRef, DepthFrameReference depthRef)
        {
            using (ColorFrame colorImageFrame = aReference.AcquireFrame())
            {
                if (colorImageFrame != null)
                {
                    using (BodyIndexFrame bodyIndexFrame = bifRef.AcquireFrame())
                    {
                        if (bodyIndexFrame != null)
                        {
                            using (DepthFrame depthFrame = depthRef.AcquireFrame())
                            {
                                if (depthFrame != null)
                                {
                                    int depthHeight = depthFrame.FrameDescription.Height;
                                    int depthWidth = depthFrame.FrameDescription.Width;

                                    int colorHeight = colorImageFrame.FrameDescription.Height;
                                    int colorWidth = colorImageFrame.FrameDescription.Width;

                                    ushort[] _depthData = new ushort[depthFrame.FrameDescription.Width * depthFrame.FrameDescription.Height];
                                    byte[] _bodyData = new byte[bodyIndexFrame.FrameDescription.Width * bodyIndexFrame.FrameDescription.Height];
                                    byte[] _colorData = new byte[colorImageFrame.FrameDescription.Width * colorImageFrame.FrameDescription.Height * 4];
                                    ColorSpacePoint[] _colorPoints = new ColorSpacePoint[depthWidth * depthHeight];

                                    depthFrame.CopyFrameDataToArray(_depthData);
                                    bodyIndexFrame.CopyFrameDataToArray(_bodyData);
                                    colorImageFrame.CopyConvertedFrameDataToArray(_colorData, ColorImageFormat.Rgba);

                                    iSensor.CoordinateMapper.MapDepthFrameToColorSpace(_depthData, _colorPoints);

                                    Color[] color = new Color[depthWidth * depthHeight];
                                    Color c;

                                    for (int y = 0; y < depthHeight; ++y)
                                    {
                                        for (int x = 0; x < depthWidth; ++x)
                                        {
                                            int depthIndex = (y * depthHeight) + x;

                                            byte player = _bodyData[depthIndex];

                                            // Check whether this pixel belong to a human!!!
                                            if (player != 0xff)
                                            {
                                                ColorSpacePoint colorPoint = _colorPoints[depthIndex];

                                                int colorX = (int)Math.Floor(colorPoint.X + 0.5);
                                                int colorY = (int)Math.Floor(colorPoint.Y + 0.5);
                                                int colorIndex = ((colorY * colorWidth) + colorX);

                                                if ((colorX >= 0) && (colorX < colorWidth) && (colorY >= 0) && (colorY < colorHeight))
                                                {

                                                    int displayIndex = colorIndex * 4;

                                                    c = new Color(_colorData[displayIndex + 0], _colorData[displayIndex + 1], _colorData[displayIndex + 2], 0xff);
                                                    color[depthIndex] = c;
                                                }
                                            }
                                        }
                                    }

                                    if (iGraphicsDevice.IsDisposed) return;
                                    var video = new Texture2D(iGraphicsDevice, depthWidth, depthHeight);

                                    video.SetData(color);

                                    lock (iVideoLock)
                                    {
                                        iRGBVideo = video;
                                    }
                                }
                            }
                        }
                    }
                }
            }
        }
Exemple #60
0
        public static System.Drawing.PointF projectedPoint(ColorSpacePoint[,] shortRange , ColorSpacePoint[,] longRange, int short_range, int long_range, Point3 p )
        {
            int x = (int)p.X;
            int y = (int)p.Y;

            if (x < 0 || x >= 512) return new System.Drawing.PointF();

            if (y < 0 || y >= 424) return new System.Drawing.PointF();

            var shortP = shortRange[x, y];
            var longP = longRange[x, y];

            float shortP_X = shortP.X;
            float longP_X = longP.X;

            var b = ( longP_X * long_range - shortP_X * short_range) / ( long_range - short_range );
            var a = (shortP_X - b) * short_range ;

            var p_x = a / p.Z + b;

            float shortP_Y = shortP.Y;
            float longP_Y = longP.Y;

            b = (longP_Y * long_range - shortP_Y * short_range) / (long_range - short_range);
            a = (shortP_Y - b) * short_range;

            var p_y = a / p.Z + b;

            return new System.Drawing.PointF(p_x, p_y);
        }