none
Kinect v2 calculate distance between floor and foot. Below is my code. Just how to use floorClipPlane ? RRS feed

  • Question

  • using System;
    using System.IO;
    using System.Collections.Generic;
    using System.ComponentModel;
    using System.Data;
    using System.Drawing;
    using System.Linq;
    using System.Text;
    using System.Threading.Tasks;
    using System.Windows.Forms;
    using Microsoft.Kinect;
    
    
    namespace kinect_sample
    {
        public partial class Form1 : Form
        {
            int i = 1;
            int _current = 0;
            bool startrecord = false;
            public string Folder { get; protected set; }
            public string Result { get; protected set; }
    
            KinectSensor kinectSensor = null;
            BodyFrameReader bodyFrameReader = null;
            Body[] bodies = null;
    
            public Form1()
            {
                InitializeComponent();
                initialiseKinect();
    
                Folder = DateTime.Now.ToString("yyy_MM_dd_HH_mm_ss");
                Directory.CreateDirectory(Folder);
            }
    
            public void initialiseKinect()
            {
                kinectSensor = KinectSensor.GetDefault();
    
                if (kinectSensor != null)
                {
                    kinectSensor.Open();
                }
    
                bodyFrameReader = kinectSensor.BodyFrameSource.OpenReader();
    
                if (bodyFrameReader != null)
                {
                    bodyFrameReader.FrameArrived += Reader_FrameArrived;
                }
            }
    
            bool dataReceived = false;
            public class Floor
            {
                public float X { get; internal set; }
                public float Y { get; internal set; }
                public float Z { get; internal set; }
                public float W { get; internal set; }
    
                public Floor(Vector4 floorClipPlane)
                {
                    X = floorClipPlane.X;
                    Y = floorClipPlane.Y;
                    Z = floorClipPlane.Z;
                    W = floorClipPlane.W;
                }
            }
    
            public void Reader_FrameArrived(object sender, BodyFrameArrivedEventArgs e)
            {
                using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame())
                {
                    if (bodyFrame != null)
                    {
                        if (bodies == null)
                        {
                            bodies = new Body[bodyFrame.BodyCount];
                            Vector4 floorClipPlane = bodyFrame.FloorClipPlane;
                            float height = floorClipPlane.W;
    
                            if (height < 1f) // 1 meter
                            {
                                Debug.WriteLine("The sensor is positioned too low!");
                            }
                        }
                    }
                    bodyFrame.GetAndRefreshBodyData(bodies);
                    dataReceived = true;
                }
    
                
                if (dataReceived)
                {
                    foreach (Body body in bodies)
                    {
                        if (body.IsTracked)
                        {
                            IReadOnlyDictionary<JointType, Joint> joints = body.Joints;
                            Dictionary<JointType, Point> joinPoints = new Dictionary<JointType, Point>();
                            string path = Path.Combine(Folder, _current.ToString() + ".line");
    
                            Floor floor = new Floor(frame.FloorClipPlane);
    
                            // Point (X, Y, Z)
                            CameraSpacePoint point = body.Joints[JointType.WristLeft].Position;
    
                            Joint head = joints[JointType.Head];
    
                            txtHeadX.Text = head.Position.X.ToString("#.##");
                            txtHeadY.Text = head.Position.Y.ToString("#.##");
                            txtHeadZ.Text = head.Position.Z.ToString("#.##");
    
                            Joint neck = joints[JointType.Neck];
    
                            txtneckX.Text = neck.Position.X.ToString("#.##");
                            txtNeckY.Text = neck.Position.Y.ToString("#.##");
                            txtNeckZ.Text = neck.Position.Z.ToString("#.##");
    
                            Joint leftShoulder = joints[JointType.ShoulderLeft];
    
                            txtL_ShoulderX.Text = leftShoulder.Position.X.ToString("#.##");
                            txtL_ShoulderY.Text = leftShoulder.Position.Y.ToString("#.##");
                            txtL_Shoulderz.Text = leftShoulder.Position.Z.ToString("#.##");
    
                            Joint shoulderSpine = joints[JointType.SpineShoulder];
    
                            txtS_SpineX.Text = shoulderSpine.Position.X.ToString("#.##");
                            textBox11.Text = shoulderSpine.Position.Y.ToString("#.##");
                            textBox12.Text = shoulderSpine.Position.Z.ToString("#.##");
    
                            Joint rightShoulder = joints[JointType.ShoulderRight];
    
                            textBox13.Text = rightShoulder.Position.X.ToString("#.##");
                            textBox14.Text = rightShoulder.Position.Y.ToString("#.##");
                            textBox15.Text = rightShoulder.Position.Z.ToString("#.##");
    
                            Joint leftElbow = joints[JointType.ElbowLeft];
    
                            textBox16.Text = leftElbow.Position.X.ToString("#.##");
                            textBox17.Text = leftElbow.Position.Y.ToString("#.##");
                            textBox18.Text = leftElbow.Position.Z.ToString("#.##");
    
                            Joint rightElbow = joints[JointType.ElbowRight];
    
                            textBox19.Text = rightElbow.Position.X.ToString("#.##");
                            textBox20.Text = rightElbow.Position.Y.ToString("#.##");
                            textBox21.Text = rightElbow.Position.Z.ToString("#.##");
    
                            Joint leftWrist = joints[JointType.WristLeft];
    
                            textBox22.Text = leftWrist.Position.X.ToString("#.##");
                            textBox23.Text = leftWrist.Position.Y.ToString("#.##");
                            textBox24.Text = leftWrist.Position.Z.ToString("#.##");
    
                            Joint rightWrist = joints[JointType.WristRight];
    
                            textBox25.Text = rightWrist.Position.X.ToString("#.##");
                            textBox26.Text = rightWrist.Position.Y.ToString("#.##");
                            textBox27.Text = rightWrist.Position.Z.ToString("#.##");
    
                            Joint leftHand = joints[JointType.HandLeft];
    
                            textBox28.Text = leftHand.Position.X.ToString("#.##");
                            textBox29.Text = leftHand.Position.Y.ToString("#.##");
                            textBox30.Text = leftHand.Position.Z.ToString("#.##");
    
                            Joint rightHand = joints[JointType.HandRight];
    
                            textBox31.Text = rightHand.Position.X.ToString("#.##");
                            textBox32.Text = rightHand.Position.Y.ToString("#.##");
                            textBox33.Text = rightHand.Position.Z.ToString("#.##");
    
                            Joint leftHandTip = joints[JointType.HandTipLeft];
    
                            textBox34.Text = leftHandTip.Position.X.ToString("#.##");
                            textBox35.Text = leftHandTip.Position.Y.ToString("#.##");
                            textBox36.Text = leftHandTip.Position.Z.ToString("#.##");
    
                            Joint rightHandTip = joints[JointType.HandTipRight];
    
                            textBox37.Text = rightHandTip.Position.X.ToString("#.##");
                            textBox38.Text = rightHandTip.Position.Y.ToString("#.##");
                            textBox39.Text = rightHandTip.Position.Z.ToString("#.##");
    
                            Joint leftThumb = joints[JointType.ThumbLeft];
    
                            textBox40.Text = leftThumb.Position.X.ToString("#.##");
                            textBox41.Text = leftThumb.Position.Y.ToString("#.##");
                            textBox42.Text = leftThumb.Position.Z.ToString("#.##");
    
                            Joint rightThumb = joints[JointType.ThumbRight];
    
                            textBox43.Text = rightThumb.Position.X.ToString("#.##");
                            textBox44.Text = rightThumb.Position.Y.ToString("#.##");
                            textBox45.Text = rightThumb.Position.Z.ToString("#.##");
    
                            Joint midSpine = joints[JointType.SpineMid];
    
                            textBox46.Text = midSpine.Position.X.ToString("#.##");
                            textBox47.Text = midSpine.Position.Y.ToString("#.##");
                            textBox48.Text = midSpine.Position.Z.ToString("#.##");
    
                            Joint baseSpine = joints[JointType.SpineBase];
    
                            textBox49.Text = baseSpine.Position.X.ToString("#.##");
                            textBox50.Text = baseSpine.Position.Y.ToString("#.##");
                            textBox51.Text = baseSpine.Position.Z.ToString("#.##");
    
                            Joint leftFoot = joints[JointType.FootLeft];
    
                            textBox52.Text = leftFoot.Position.X.ToString("#.##");
                            textBox53.Text = leftFoot.Position.Y.ToString("#.##");
                            textBox54.Text = leftFoot.Position.Z.ToString("#.##");
    
                            Joint rightFoot = joints[JointType.FootRight];
    
                            textBox55.Text = rightFoot.Position.X.ToString("#.##");
                            textBox56.Text = rightFoot.Position.Y.ToString("#.##");
                            textBox57.Text = rightFoot.Position.Z.ToString("#.##");
    
    
                            Joint leftHip = joints[JointType.HipLeft];
    
                            txtLeftHipX.Text = leftHip.Position.X.ToString("#.##");
                            txtLeftHipY.Text = leftHip.Position.Y.ToString("#.##");
                            txtLeftHipZ.Text = leftHip.Position.Z.ToString("#.##");
    
                            Joint leftKnee = joints[JointType.KneeLeft];
    
                            txtLeftKneeX.Text = leftKnee.Position.X.ToString("#.##");
                            txtLeftKneeY.Text = leftKnee.Position.Y.ToString("#.##");
                            txtLeftKneeZ.Text = leftKnee.Position.Z.ToString("#.##");
    
                            Joint leftAnkle = joints[JointType.AnkleLeft];
    
                            txtLeftAnkleX.Text = leftAnkle.Position.X.ToString("#.##");
                            txtLeftAnkleY.Text = leftAnkle.Position.Y.ToString("#.##");
                            txtLeftAnkleZ.Text = leftAnkle.Position.Z.ToString("#.##");
    
                            Joint rightHip = joints[JointType.HipRight];
    
                            
    
                            txtRightHipX.Text = rightHip.Position.X.ToString("#.##");
                            txtRightHipY.Text = rightHip.Position.Y.ToString("#.##");
                            txtRightHipZ.Text = rightHip.Position.Z.ToString("#.##");
    
                            Joint rightKnee = joints[JointType.KneeRight];
    
                            
    
                            txtRightKneeX.Text = rightKnee.Position.X.ToString("#.##");
                            txtRightKneeY.Text = rightKnee.Position.Y.ToString("#.##");
                            txtRightKneeZ.Text = rightKnee.Position.Z.ToString("#.##");
    
                            Joint rightAnkle = joints[JointType.AnkleRight];
    
                            
    
                            txtRightAnkleX.Text = rightAnkle.Position.X.ToString("#.##");
                            txtRightAnkleY.Text = rightAnkle.Position.Y.ToString("#.##");
                            txtRightAnkleZ.Text = rightAnkle.Position.Z.ToString("#.##");
    
                            float a = (leftHip.Position.X - leftKnee.Position.X);
                            float b = (leftHip.Position.Y - leftKnee.Position.Y);
                            float c = (leftHip.Position.Z - leftKnee.Position.Z);
    
                            float p = (leftAnkle.Position.X - leftKnee.Position.X);
                            float q = (leftAnkle.Position.Y - leftKnee.Position.Y);
                            float r = (leftAnkle.Position.Z - leftKnee.Position.Z);
    
                            float n = p * a + q * b + r * c;
                            double m = Math.Sqrt(a * a + b * b + c * c) * Math.Sqrt(p * p + q * q + r * r);
    
    
                            double leftangle = Math.Acos(n / m);
    
                            double finalleft = leftangle * (180.0 / Math.PI);
    
                            txtleftangle.Text = finalleft.ToString("#.##");
    
                            float a1 = (rightHip.Position.X - rightKnee.Position.X);
                            float b1 = (rightHip.Position.Y - rightKnee.Position.Y);
                            float c1 = (rightHip.Position.Z - rightKnee.Position.Z);
    
                            float p1 = (rightAnkle.Position.X - rightKnee.Position.X);
                            float q1 = (rightAnkle.Position.Y - rightKnee.Position.Y);
                            float r1 = (rightAnkle.Position.Z - rightKnee.Position.Z);
    
                            float n1 = p1 * a1 + q1 * b1 + r1 * c1;
                            double m1 = Math.Sqrt(a1 * a1 + b1 * b1 + c1 * c1) * Math.Sqrt(p1 * p1 + q1 * q1 + r1 * r1);
    
    
                            double rightangle = Math.Acos(n1 / m1);
    
                            double finalright = rightangle * (180.0 / Math.PI);
    
                            txtrightangle.Text = finalright.ToString("#.##");
    
    
    
                            var separator = ",";
                            string csvpath = "C:\\Users\\Dhruv\\Desktop\\Dataset\\user8.csv";
    
                            
    
    
                            while(i != 2)
                            {
                                StringBuilder csvcontentNew2 = new StringBuilder();
                                csvcontentNew2.Append("headX" + separator + "headY" + separator + "headZ" + separator + "neckX" + separator + "neckY" + separator + "neckZ" + separator + "leftShoulderX" + separator + "leftShoulderY" + separator + "leftShoulderZ" + separator + "shoulderSpineX" + separator + "shoulderSpineY" + separator + "shoulderSpineZ" + separator + "rightShoulderX" + separator + "rightShoulderY" + separator + "rightShoulderZ" + separator + "leftElbowX" + separator + "leftElbowY" + separator + "leftElbowZ" + separator + "rightElbowX" + separator + "rightElbowY" + separator + "rightElbowZ" + separator + "leftWristX" + separator + "leftWristY" + separator + "leftWristZ" + separator + "rightWristX" + separator + "rightWristY" + separator + "rightWristZ" + separator + "leftHandX" + separator + "leftHandY" + separator + "leftHandZ" + separator + "rightHandX" + separator + "rightHandY" + separator + "rightHandZ" + separator + "leftHandTipX" + separator + "leftHandTipY" + separator + "leftHandTipZ" + separator + "rightHandTipX" + separator + "rightHandTipY" + separator + "rightHandTipZ" + separator + "leftThumbX" + separator + "leftThumbY" + separator + "leftThumbZ" + separator + "rightThumbX" + separator + "rightThumbY" + separator + "rightThumbZ" + separator + "midSpineX" + separator + "midSpineY" + separator + "midSpineZ" + separator + "baseSpineX" + separator + "baseSpineY" + separator + "baseSpineZ" + separator + "leftHipX" + separator + "leftHipY" + separator + "leftHipZ" + separator + "rightHipX" + separator + "rightHipY" + separator + "rightHipZ" + separator + "leftKneeX" + separator + "leftKneeY" + separator + "leftKneeZ" + separator + "rightKneeX" + separator + "rightKneeY" + separator + "rightKneeZ" + separator + "leftAnkleX" + separator + "leftAnkleY" + separator + "leftAnkleZ" + separator + "rightAnkleX" + separator + "rightAnkleY" + separator + "rightAnkleZ" + separator + "leftFootX" + separator + "leftFootY" + separator + "leftFootZ" + separator + "rightFootX" + separator + "rightFootY" + separator + "rightFootZ" + separator + "LeftKneeAngle" + separator + "RightKneeAngle");
                                //File.AppendAllText(csvpath, csvcontentNew2.ToString());
                                csvcontentNew2.AppendLine();
                                File.AppendAllText(csvpath, csvcontentNew2.ToString());
    
                                i++;
                            }
                            
    
    
                            StringBuilder csvcontentNew = new StringBuilder();
                            StringBuilder csvcontentNew1 = new StringBuilder();
                            StringBuilder csvcontentNew3 = new StringBuilder();
    
    
    
                            csvcontentNew.Append( head.Position.X + separator + head.Position.Y + separator + head.Position.Z + separator + neck.Position.X + separator + neck.Position.Y + separator + neck.Position.Z + separator + leftShoulder.Position.X + separator + leftShoulder.Position.Y + separator + leftShoulder.Position.Z + separator + shoulderSpine.Position.X + separator + shoulderSpine.Position.Y + separator + shoulderSpine.Position.Z + separator + rightShoulder.Position.X + separator + rightShoulder.Position.Y + separator + rightShoulder.Position.Z + separator + leftElbow.Position.X + separator + leftElbow.Position.Y + separator + leftElbow.Position.Z + separator + rightElbow.Position.X + separator + rightElbow.Position.Y + separator + rightElbow.Position.Z + separator + leftWrist.Position.X + separator + leftWrist.Position.Y + separator + leftWrist.Position.Z + separator + rightWrist.Position.X + separator + rightWrist.Position.Y + separator + rightWrist.Position.Z + separator + leftHand.Position.X + separator + leftHand.Position.Y + separator + leftHand.Position.Z + separator + rightHand.Position.X + separator + rightHand.Position.Y + separator + rightHand.Position.Z  + separator + leftHandTip.Position.X + separator + leftHandTip.Position.Y + separator + leftHandTip.Position.Z + separator + rightHandTip.Position.X + separator + rightHandTip.Position.Y + separator + rightHandTip.Position.Z + separator + leftThumb.Position.X + separator + leftThumb.Position.Y + separator + leftThumb.Position.Z + separator + rightThumb.Position.X + separator + rightThumb.Position.Y + separator + rightThumb.Position.Z + separator + midSpine.Position.X + separator + midSpine.Position.Y + separator + midSpine.Position.Z  + separator + baseSpine.Position.X + separator + baseSpine.Position.Y + separator + baseSpine.Position.Z + separator + leftHip.Position.X + separator + leftHip.Position.Y + separator + leftHip.Position.Z + separator + rightHip.Position.X + separator + rightHip.Position.Y + separator + rightHip.Position.Z + separator + leftKnee.Position.X + separator + leftKnee.Position.Y + separator + leftKnee.Position.Z + separator + rightKnee.Position.X + separator + rightKnee.Position.Y + separator + rightKnee.Position.Z + separator + leftAnkle.Position.X + separator + leftAnkle.Position.Y + separator + leftAnkle.Position.Z + separator + rightAnkle.Position.X + separator + rightAnkle.Position.Y + separator + rightAnkle.Position.Z + separator + leftFoot.Position.X + separator + leftFoot.Position.Y + separator + leftFoot.Position.Z + separator + rightFoot.Position.X + separator + rightFoot.Position.Y + separator + rightFoot.Position.Z + separator + finalleft  + separator + finalright );   // Try it like this
                            
                            csvcontentNew3.AppendLine();
    
    
                            //String that will define CSV Location
                            
                            File.AppendAllText(csvpath, csvcontentNew.ToString());
                            
                            
                            File.AppendAllText(csvpath, csvcontentNew3.ToString());
                        }
                    }
    
                }
                
            }
    
            
    
    
    
        }
    }
    

    Wednesday, March 21, 2018 9:25 AM