public static int GetHighestValidPointIndexInRectangle(int bIndex, int cIndex) { Point pB = GetPoint(bIndex); Point pC = GetPoint(cIndex); int rectangleWidth = pB.X - pC.X; CameraSpacePoint highestPoint = new CameraSpacePoint() { X = float.PositiveInfinity, Y = float.PositiveInfinity, Z = float.PositiveInfinity }; int highestPointIndex = -1; for (int i = 0; i < rectangleWidth; i++) { for (int j = 0; j < rectangleWidth; j++) { int currentIndex = GetIndex(pC.X + j, pB.Y + i); if (BoundaryCheck(currentIndex)) { CameraSpacePoint currentPoint = GlobVar.SubtractedFilteredPointCloud[currentIndex]; if (currentPoint.Z < highestPoint.Z && !float.IsInfinity(currentPoint.X) && !float.IsInfinity(currentPoint.Y)) { highestPoint = currentPoint; highestPointIndex = currentIndex; } } } } return highestPointIndex; }
public static float GetEuclideanDistance(CameraSpacePoint a, CameraSpacePoint b) { float distance = (float)Math.Sqrt((a.X - b.X) * (a.X - b.X) + (a.Y - b.Y) * (a.Y - b.Y) + (a.Z - b.Z) * (a.Z - b.Z)); return float.IsNaN(distance) ? float.MaxValue : distance; }
/// <summary> /// Normalizes the depths in the input frame into a 0-255 byte gray-value array. /// </summary> public static byte[] CalculateIntensityFromCameraSpacePoints(CameraSpacePoint[] cameraSpacePoint) { var intensityMap = new byte[cameraSpacePoint.Length]; for (int i = 0; i < cameraSpacePoint.Length; i++) { float depth = cameraSpacePoint[i].Z; if (depth != 0) { float currentMax = depth - GlobVar.MinSensingDepth; const float currentDepthRange = GlobVar.MaxSensingDepth - GlobVar.MinSensingDepth; if (depth < GlobVar.MaxSensingDepth && depth > GlobVar.MinSensingDepth) { intensityMap[i] = (byte)(255 - (255 * currentMax / currentDepthRange)); } else { intensityMap[i] = (byte)0; } } else { intensityMap[i] = (byte)0; } } return intensityMap; }
private static float[] CreateIntegralImage(CameraSpacePoint[] cameraSpacePoints) { float[] integralImage = new float[GlobVar.ScaledFrameLength]; for (var i = 0; i < GlobVar.ScaledFrameHeight; i++) { for (var j = 0; j < GlobVar.ScaledFrameWidth; j++) { int indexCurrent = i * GlobVar.ScaledFrameWidth + j; int indexUpLeft = (i - 1) * GlobVar.ScaledFrameWidth + j - 1; int indexUp = (i - 1) * GlobVar.ScaledFrameWidth + j; int indexLeft = i * GlobVar.ScaledFrameWidth + j - 1; if (i==0 && j==0) { integralImage[indexCurrent] = cameraSpacePoints[indexCurrent].Z; } else if (i==0) { integralImage[indexCurrent] = cameraSpacePoints[indexCurrent].Z + integralImage[indexLeft]; } else if (j == 0) { integralImage[indexCurrent] = cameraSpacePoints[indexCurrent].Z + integralImage[indexUp]; } else { integralImage[indexCurrent] = cameraSpacePoints[indexCurrent].Z + integralImage[indexUp] + integralImage[indexLeft] - integralImage[indexUpLeft]; } } } return integralImage; }
/// <summary> /// Returns the length of the segment defined by the specified points. /// </summary> /// <param name="p1">The first point (start of the segment).</param> /// <param name="p2">The second point (end of the segment).</param> /// <returns>The length of the segment in meters.</returns> public static double Distance(CameraSpacePoint p1, CameraSpacePoint p2) { return Math.Sqrt( Math.Pow(p1.X - p2.X, 2) + Math.Pow(p1.Y - p2.Y, 2) + Math.Pow(p1.Z - p2.Z, 2)); }
public bool checPeriodicMovements(Body newBody) { time = DateTime.Now- startTime; if(time.TotalMilliseconds>=4000) { result = checkAnalisys(); resetValues(); startTime = DateTime.Now; } else { hipNew = newBody.Joints[JointType.HipRight].Position; if (BodyFramePreAnalysis.bodyOld.Count > 0) { if(Math.Abs(hipNew.X-hipOld.X)>0.009 || Math.Abs(hipNew.Y - hipOld.Y) > 0.009 || Math.Abs(hipNew.Z - hipOld.Z) > 0.009) { hipOld = BodyFramePreAnalysis.bodyOld[JointType.HipRight].Position; storeValues(); } } } return result; }
/// <summary> /// Calculates the angle between the specified points. /// </summary> /// <param name="center">The center of the angle.</param> /// <param name="start">The start of the angle.</param> /// <param name="end">The end of the angle.</param> /// <returns>The angle, in degrees.</returns> public static double Angle(this CameraSpacePoint center, CameraSpacePoint start, CameraSpacePoint end) { Vector3D first = start.ToVector3() - center.ToVector3(); Vector3D second = end.ToVector3() - center.ToVector3(); return Vector3D.AngleBetween(first, second); }
public static List<Point3D> CameraSpacePointsToPoint3Ds(CameraSpacePoint[] cameraSpacePoints) { if (cameraSpacePoints == null) { return null; } if (cameraSpacePoints.Length == 0) { return null; } List<Point3D> point3Ds = new List<Point3D>(); foreach (CameraSpacePoint point in cameraSpacePoints) { if (GeometryHelper.IsValidPoint(point)) { point3Ds.Add(new Point3D { X = point.X, Y = point.Y, Z = point.Z }); } } return point3Ds; }
/// <summary> /// Calculates the angle between the specified points around the specified axis. /// </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> /// <param name="axis">The axis around which the angle is calculated.</param> /// <returns>The angle, in degrees.</returns> public static double Angle(this CameraSpacePoint center, CameraSpacePoint start, CameraSpacePoint end, Axis axis) { switch (axis) { case Axis.X: start.X = 0f; center.X = 0f; end.X = 0f; break; case Axis.Y: start.Y = 0f; center.Y = 0f; end.Y = 0f; break; case Axis.Z: start.Z = 0f; center.Z = 0f; end.Z = 0f; break; } Vector3D first = start.ToVector3() - center.ToVector3(); Vector3D second = end.ToVector3() - center.ToVector3(); return Vector3D.AngleBetween(first, second); }
public CameraSpacePoint Filter(CameraSpacePoint point) { if (this.lastTrend == null || this.lastOutput == null) { this.lastTrend = point; this.lastOutput = point; return point; } var newTrend = new CameraSpacePoint { X = gamma * (point.X - lastOutput.Value.X) + (1 - gamma) * lastTrend.Value.X, Y = gamma * (point.Y - lastOutput.Value.Y) + (1 - gamma) * lastTrend.Value.Y, Z = gamma * (point.Z - lastOutput.Value.Z) + (1 - gamma) * lastTrend.Value.Z }; var newOutput = new CameraSpacePoint { X = alpha * point.X + (1 - alpha) * (lastOutput.Value.X + lastTrend.Value.X), Y = alpha * point.Y + (1 - alpha) * (lastOutput.Value.Y + lastTrend.Value.Y), Z = alpha * point.Z + (1 - alpha) * (lastOutput.Value.Z + lastTrend.Value.Z) }; this.lastTrend = newTrend; this.lastOutput = newOutput; return newOutput; }
// Update is called once per frame void Update () { float deltaTime = Time.deltaTime; mousePosX = CrossPlatformInputManager.GetAxis ("Mouse X"); translateVector.Set (-deltaTime*50*mousePosX, 0, 0); if (Input.GetMouseButtonDown (0)) mouseClicked = true; else if (Input.GetMouseButtonUp (0)) mouseClicked = false; if (mouseClicked) { transform.Translate(translateVector); } //Kinect if (bodyManager == null) { return; } bodies = bodyManager.GetData (); if (bodies == null) { return; } foreach (var body in bodies) { if (body == null){ continue; } if (body.IsTracked) { Debug.Log(body.HandLeftState.ToString()); if (body.HandLeftState == HandState.Closed){ if(grab == false){ lastPos = body.Joints[TrackedJoint].Position; lastObjPos = gameObject.transform.position; } grab = true; var pos = body.Joints[TrackedJoint].Position; Debug.Log(pos.X-lastPos.X); transform.position = new Vector3(lastObjPos.x + (pos.X - lastPos.X) * multiplier, 1, -10); //translateVector.Set (deltaTime*50*pos.X-lastPos.X , 0, 0); //transform.Translate(translateVector); } else if (body.HandLeftState == HandState.Open){ grab = false; } } } }
private void SetValues(CameraSpacePoint hand) { KinectX = hand.X; KinectY = hand.Y; ScreenX = (MaxValue / 2) + (KinectX * MaxValue); ScreenY = (MaxValue / 2) - (KinectY * MaxValue); }
public void CheckActiveWorkspace(CameraSpacePoint[] handPositions) { if (handPositions.Length > 0) { CheckActiveWorkspace(Converter.CameraSpacePointsToPoint3Ds(handPositions).ToArray()); } }
private static CameraSpacePoint CopyPosition(CameraSpacePoint position) { CameraSpacePoint result = new CameraSpacePoint(); result.X = position.X; result.Y = position.Y; result.Z = position.Z; return result; }
/// <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; }
private static double DistanceBetweenPoints(CameraSpacePoint point1, CameraSpacePoint point2) { double dx = Math.Abs(point2.X - point1.X); double dy = Math.Abs(point2.Y - point1.Y); double dz = Math.Abs(point2.Z - point1.Z); return Math.Sqrt(dx * dx + dy * dy + dz * dz); }
private static CameraSpacePoint SubstractPoints(CameraSpacePoint position1, CameraSpacePoint position2) { CameraSpacePoint result = new CameraSpacePoint(); result.X = position1.X - position2.X; result.Y = position1.Y - position2.Y; result.Z = position1.Z - position2.Z; return result; }
/* Depricated */ public static CameraSpacePoint getEinsvektor(CameraSpacePoint point) { var e = (float)Math.Sqrt(Math.Pow(point.X, 2) + Math.Pow(point.Y, 2) + Math.Pow(point.Z, 2)); point.X = point.X / e; point.Y = point.Y / e; point.Z = point.Z / e; return point; }
public static Point3D CameraSpacePointToPoint3D(CameraSpacePoint cameraSpacePoint) { return new Point3D { X = cameraSpacePoint.X, Y = cameraSpacePoint.Y, Z = cameraSpacePoint.Z }; }
public void AddFrame(CameraSpacePoint[] newFrame) { _temporalFrames.Add(_frameCounter,newFrame); _frameCounter++; if (_frameCounter == _maxTemporalFrames) { CalculateTemporalMedianImage(); } }
/// <summary> /// Copies camera space points from face to the gpu /// </summary> /// <param name="context"></param> /// <param name="points"></param> public void Copy(DeviceContext context, CameraSpacePoint[] points) { if (points.Length == 0) return; fixed (CameraSpacePoint* cptr = &points[0]) { this.buffer.Upload(context, new IntPtr(cptr), points.Length * 12); } }
CameraSpacePoint CSVectorSubtract(CameraSpacePoint p1, CameraSpacePoint p2) { CameraSpacePoint diff = new CameraSpacePoint(); diff.X = p1.X - p2.X; diff.Y = p1.Y - p2.Y; diff.Z = p1.Z - p2.Z; return diff; }
public Head(int highestPointIndex) { HighestPointIndex = highestPointIndex; AvgCenterPoint = new CameraSpacePoint() { X = float.NaN, Y = float.NaN, Z = float.NaN }; }
public Point MapCameraSpacePoint(CameraSpacePoint point) { double canvasWidth = canvas.ActualWidth; double canvasHeight = canvas.ActualHeight; var colourSpacePosition = mapper.MapCameraPointToColorSpace(point); return (new Point() { X = (colourSpacePosition.X / colourFrameSize.Width) * canvasWidth, Y = (colourSpacePosition.Y / colourFrameSize.Height) * canvasHeight }); }
public static Joint[] ValidRandomJointsSpinePosition(CameraSpacePoint cp) { Joint[] result = new Joint[Microsoft.Kinect.Body.JointCount]; JointType[] jt = (JointType[])Enum.GetValues(typeof(JointType)); for (int i = 0; i < result.Length; i++) { result[i].JointType = jt[i]; if (jt[i] == JointType.SpineBase) { result[i].Position = cp; } } return result; }
public static CameraSpacePoint[] CreateEmptyPointCloud() { CameraSpacePoint[] emptyPointCloud = new CameraSpacePoint[GlobVar.ScaledFrameLength]; float maxDepth = GlobVar.MaxSensingDepth; for (int i = 0; i < emptyPointCloud.Length; i++) { emptyPointCloud[i] = new CameraSpacePoint() { X = float.PositiveInfinity, Y = float.PositiveInfinity, Z = maxDepth }; } return emptyPointCloud; }
public static double Process(CameraSpacePoint md1, CameraSpacePoint md2, CameraSpacePoint md3, bool twoD, bool supplementary = false, bool relative = false) { double rx1 = md1.X - md2.X; double ry1 = md1.Y - md2.Y; double rz1 = md1.Z - md2.Z; double rx2 = md3.X - md2.X; double ry2 = md3.Y - md2.Y; double rz2 = md3.Z - md2.Z; double angle = double.NaN; if (twoD) angle = GetAngleBetweenPoints(rx1, ry1, rx2, ry2); else angle = GetAngleBetweenPoints(rx1, ry1, rz1, rx2, ry2, rz2); double ret = supplementary ? 180.0 - angle : angle; if (twoD) ret = -ret; return ret; }
public void Update(CameraSpacePoint currentPoint) { wasReset = false; anchorPoint.X = CumulativeAverage(anchorPoint.X, currentPoint.X, this.count); anchorPoint.Y = CumulativeAverage(anchorPoint.Y, currentPoint.Y, this.count); anchorPoint.Z = CumulativeAverage(anchorPoint.Z, currentPoint.Z, this.count); ++count; if (CSVectorLength(CSVectorSubtract(anchorPoint, currentPoint)) > resetThreshold) { Reset(currentPoint); wasReset = true; } }
public static CameraSpacePoint GetAccumulatedAvarage(this CameraSpacePoint lastAnchor, CameraSpacePoint newPosition, ref Int32 count) { var result = new CameraSpacePoint(); // TODO ! RORU find out poblem of this filtering //result.X = (lastAnchor.X * count) / (count + 1) + newPosition.X * (1 / (count + 1)); //result.Y = (lastAnchor.Y * count) / (count + 1) + newPosition.Y * (1 / (count + 1)); //result.Z = (lastAnchor.Z * count) / (count + 1) + newPosition.Z * (1 / (count + 1)); result.X = (lastAnchor.X + newPosition.X) / 2; result.Y = (lastAnchor.Y + newPosition.Y) / 2; result.Z = (lastAnchor.Z + newPosition.Z) / 2; ++count; return result; }
/* Depricated */ public static CameraSpacePoint getNormalVector(CameraSpacePoint p1, CameraSpacePoint p2, CameraSpacePoint p3) { var a = new CameraSpacePoint(); a.X = p1.X - p2.X; a.Y = p1.Y - p2.Y; a.Z = p1.Z - p2.Z; var b = new CameraSpacePoint(); b.X = p1.X - p3.X; b.Y = p1.Y - p3.Y; b.Z = p1.Z - p3.Z; var n = new CameraSpacePoint(); n.X = a.Y * b.Z - a.Z * b.Y; n.Y = a.X * b.Z - a.Z * b.X; n.Z = a.X * b.Y - a.Y * b.X; return n; }
/// <summary> /// 获取两个三维的点组成的向量, /// 如向量AB /// </summary> /// <param name="A"></param> /// <param name="B"></param> /// <returns></returns> public static CameraSpacePoint GetVector(CameraSpacePoint A, CameraSpacePoint B) { return(VectorHelp.GetSubtract(B, A)); }
public Point projectToScreen(CameraSpacePoint point) { Vector3D convertedPoint = new Vector3D(point.X, point.Y, point.Z); return(mapCameraScreenPointToPixelCoord(mapCameraSpacePointToScreenPoint(mapKinectSpacePointToCameraSpace(convertedPoint)))); }
internal void RecoverCalibrationFromSensor(Microsoft.Kinect.KinectSensor kinectSensor) { var stopWatch = new System.Diagnostics.Stopwatch(); stopWatch.Start(); var objectPoints1 = new List <Vector <double> >(); var colorPoints1 = new List <System.Drawing.PointF>(); var depthPoints1 = new List <System.Drawing.PointF>(); int n = 0; for (float x = -2f; x < 2f; x += 0.2f) { for (float y = -2f; y < 2f; y += 0.2f) { for (float z = 0.4f; z < 4.5f; z += 0.4f) { var kinectCameraPoint = new CameraSpacePoint(); kinectCameraPoint.X = x; kinectCameraPoint.Y = y; kinectCameraPoint.Z = z; // use SDK's projection // adjust Y to make RH coordinate system that is a projection of Kinect 3D points var kinectColorPoint = kinectSensor.CoordinateMapper.MapCameraPointToColorSpace(kinectCameraPoint); kinectColorPoint.Y = colorImageHeight - kinectColorPoint.Y; var kinectDepthPoint = kinectSensor.CoordinateMapper.MapCameraPointToDepthSpace(kinectCameraPoint); kinectDepthPoint.Y = depthImageHeight - kinectDepthPoint.Y; if ((kinectColorPoint.X >= 0) && (kinectColorPoint.X < colorImageWidth) && (kinectColorPoint.Y >= 0) && (kinectColorPoint.Y < colorImageHeight) && (kinectDepthPoint.X >= 0) && (kinectDepthPoint.X < depthImageWidth) && (kinectDepthPoint.Y >= 0) && (kinectDepthPoint.Y < depthImageHeight)) { n++; var objectPoint = Vector <double> .Build.Dense(3); objectPoint[0] = kinectCameraPoint.X; objectPoint[1] = kinectCameraPoint.Y; objectPoint[2] = kinectCameraPoint.Z; objectPoints1.Add(objectPoint); var colorPoint = new System.Drawing.PointF(); colorPoint.X = kinectColorPoint.X; colorPoint.Y = kinectColorPoint.Y; colorPoints1.Add(colorPoint); //Console.WriteLine(objectPoint[0] + "\t" + objectPoint[1] + "\t" + colorPoint.X + "\t" + colorPoint.Y); var depthPoint = new System.Drawing.PointF(); depthPoint.X = kinectDepthPoint.X; depthPoint.Y = kinectDepthPoint.Y; depthPoints1.Add(depthPoint); } } } } this.colorCameraMatrix[0, 0] = 1000; //fx this.colorCameraMatrix[1, 1] = 1000; //fy this.colorCameraMatrix[0, 2] = colorImageWidth / 2; //cx this.colorCameraMatrix[1, 2] = colorImageHeight / 2; //cy this.colorCameraMatrix[2, 2] = 1; var rotation = Vector <double> .Build.Dense(3); var translation = Vector <double> .Build.Dense(3); var colorError = CalibrateColorCamera(objectPoints1, colorPoints1, colorCameraMatrix, colorLensDistortion, rotation, translation, this.silent); var rotationMatrix = AxisAngleToMatrix(rotation); this.depthToColorTransform = Matrix <double> .Build.DenseIdentity(4, 4); for (int i = 0; i < 3; i++) { this.depthToColorTransform[i, 3] = translation[i]; for (int j = 0; j < 3; j++) { this.depthToColorTransform[i, j] = rotationMatrix[i, j]; } } this.depthCameraMatrix[0, 0] = 360; //fx this.depthCameraMatrix[1, 1] = 360; //fy this.depthCameraMatrix[0, 2] = depthImageWidth / 2.0; //cx this.depthCameraMatrix[1, 2] = depthImageHeight / 2.0; //cy this.depthCameraMatrix[2, 2] = 1; var depthError = CalibrateDepthCamera(objectPoints1, depthPoints1, depthCameraMatrix, depthLensDistortion, silent); // check projections double depthProjectionError = 0; double colorProjectionError = 0; var testObjectPoint4 = Vector <double> .Build.Dense(4); for (int i = 0; i < n; i++) { var testObjectPoint = objectPoints1[i]; var testDepthPoint = depthPoints1[i]; var testColorPoint = colorPoints1[i]; // "camera space" == depth camera space // depth camera projection double depthU, depthV; Project(depthCameraMatrix, depthLensDistortion, testObjectPoint[0], testObjectPoint[1], testObjectPoint[2], out depthU, out depthV); double dx = testDepthPoint.X - depthU; double dy = testDepthPoint.Y - depthV; depthProjectionError += (dx * dx) + (dy * dy); // color camera projection testObjectPoint4[0] = testObjectPoint[0]; testObjectPoint4[1] = testObjectPoint[1]; testObjectPoint4[2] = testObjectPoint[2]; testObjectPoint4[3] = 1; var color = depthToColorTransform * testObjectPoint4; color *= (1.0 / color[3]); // not necessary for this transform double colorU, colorV; Project(colorCameraMatrix, colorLensDistortion, color[0], color[1], color[2], out colorU, out colorV); dx = testColorPoint.X - colorU; dy = testColorPoint.Y - colorV; colorProjectionError += (dx * dx) + (dy * dy); } depthProjectionError /= n; colorProjectionError /= n; stopWatch.Stop(); if (!this.silent) { Console.WriteLine("FakeCalibration :"); Console.WriteLine("n = " + n); Console.WriteLine("color error = " + colorError); Console.WriteLine("depth error = " + depthError); Console.WriteLine("depth reprojection error = " + depthProjectionError); Console.WriteLine("color reprojection error = " + colorProjectionError); Console.WriteLine("depth camera matrix = \n" + depthCameraMatrix); Console.WriteLine("depth lens distortion = \n" + depthLensDistortion); Console.WriteLine("color camera matrix = \n" + colorCameraMatrix); Console.WriteLine("color lens distortion = \n" + colorLensDistortion); Console.WriteLine(stopWatch.ElapsedMilliseconds + " ms"); Console.WriteLine("________________________________________________________"); } }
/// <summary> /// Handles the body frame data arriving from the sensor /// </summary> /// <param name="sender">object sending the event</param> /// <param name="e">event arguments</param> private void Reader_FrameArrived(object sender, BodyFrameArrivedEventArgs e) { bool dataReceived = false; using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame()) { if (bodyFrame != null) { if (this.bodies == null) { this.bodies = new Body[bodyFrame.BodyCount]; } // 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. bodyFrame.GetAndRefreshBodyData(this.bodies); dataReceived = true; } } if (dataReceived) { using (DrawingContext dc = this.drawingGroup.Open()) { // Draw a transparent background to set the render size //dc.DrawRectangle(Brushes.Black, null, new Rect(0.0, 0.0, this.displayWidth, this.displayHeight)); dc.DrawRectangle(Brushes.Black, null, this.displayRect); //Count Bodies Observed bodies_currently_observed = 0; //Check for alerts global_contamination_alert = Contamination_Alert(); global_proximity_alert = Proximity_Alert(); //foreach (Body body in this.bodies) for (int i = 0; i < this.bodies.Length; i++) { //Change pen color to red if that pedestrian is currently causing an alert Pen drawPen = new Pen(Brushes.White, 6); if (proximity_alert_offenders.Contains(i) || contamination_alert_offenders.Contains(i)) { drawPen = new Pen(Brushes.Red, 6); } this.DrawFaceFrameResults(i, dc); if (bodies[i].IsTracked) { //Increment Body Count bodies_currently_observed++; // draw face frame results this.DrawClippedEdges(bodies[i], dc); IReadOnlyDictionary <JointType, Joint> joints = bodies[i].Joints; // convert the joint points to depth (display) space Dictionary <JointType, Point> jointPoints = new Dictionary <JointType, Point>(); foreach (JointType jointType in joints.Keys) { // sometimes the depth(Z) of an inferred joint may show as negative // clamp down to 0.1f to prevent coordinatemapper from returning (-Infinity, -Infinity) CameraSpacePoint position = joints[jointType].Position; if (position.Z < 0) { position.Z = InferredZPositionClamp; } DepthSpacePoint depthSpacePoint = this.coordinateMapper.MapCameraPointToDepthSpace(position); jointPoints[jointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y); } this.DrawBody(joints, jointPoints, dc, drawPen); } } //Play Audio Alerts //this.PropertyChanged(this, new PropertyChangedEventArgs("ImageSource")); //Face-Touch Contamination Alert if (global_contamination_alert) { if (!contamination_alarm_currently_playing && !proximity_alarm_currently_playing) { contamination_alarm_currently_playing = true; alarm_1.PlayLooping(); this.PropertyChanged(this, new PropertyChangedEventArgs("ImageSource")); } } else { if (contamination_alarm_currently_playing) { alarm_1.Stop(); contamination_alarm_currently_playing = false; this.PropertyChanged(this, new PropertyChangedEventArgs("ImageSource")); } } //Ped - Ped Proximity Alert - SHOULD BE THE MASTER ALARM IF THIS ONE IS ACTIVE THEN A FACE ALARM WON'T MATTER if (global_proximity_alert) { if (!proximity_alarm_currently_playing) { contamination_alarm_currently_playing = false; alarm_1.Stop(); proximity_alarm_currently_playing = true; alarm_2.PlayLooping(); this.PropertyChanged(this, new PropertyChangedEventArgs("ImageSource")); } } else { if (proximity_alarm_currently_playing) { alarm_2.Stop(); proximity_alarm_currently_playing = false; this.PropertyChanged(this, new PropertyChangedEventArgs("ImageSource")); } } //Display the Number of bodies tracked currently dc.DrawText( new FormattedText( ("Number of Bodies Detected = " + bodies_currently_observed + "\nProximity Alert: " + global_proximity_alert + "\nFace Touch Alert: " + global_contamination_alert), CultureInfo.GetCultureInfo("en-us"), FlowDirection.LeftToRight, new Typeface("Georgia"), DrawTextFontSize, Brushes.White), new Point(displayWidth / 2, displayHeight - 50) ); // prevent drawing outside of our render area this.drawingGroup.ClipGeometry = new RectangleGeometry(this.displayRect);//new RectangleGeometry(new Rect(0.0, 0.0, this.displayWidth, this.displayHeight)); } } }
public void Read_BodyFrameArrived(object sender, BodyFrameArrivedEventArgs e) { bool dataReceived = false; 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) { using (DrawingContext dc = this.drawingGroup.Open()) { dc.DrawRectangle(Brushes.Transparent, null, new Rect(0.0, 0.0, this.displayWidth, this.displayHeight)); //Transparent int penIndex = 0; foreach (Body body in this.bodies) { Pen drawPen = this.bodyColors[penIndex++]; if (body.IsTracked) { this.DrawClippedEdges(body, dc); IReadOnlyDictionary <JointType, Joint> joints = body.Joints; Dictionary <JointType, Point> jointPoints = new Dictionary <JointType, Point>(); foreach (JointType jointType in joints.Keys) { CameraSpacePoint position = joints[jointType].Position; if (position.Z < 0) { position.Z = InferredZPositionClamp; } DepthSpacePoint depthSpacePoint = this.coordinateMapper.MapCameraPointToDepthSpace(position); jointPoints[jointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y); } this.DrawBody(joints, jointPoints, dc, drawPen); HandMotion_Tracking.DrawHand_Right(jointPoints[JointType.HandRight], dc); //HandMotion_Tracking.DrawHand_left(body.HandLeftState, jointPoints[JointType.HandLeft], dc); } } this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, this.displayWidth, this.displayHeight)); } } }
private double DistBetPoints(CameraSpacePoint P1, CameraSpacePoint P2) { return(Math.Sqrt(((P2.X - P1.X) * (P2.X - P1.X)) + ((P2.Y - P1.Y) * (P2.Y - P1.Y)) + ((P2.Z - P1.Z) * (P2.Z - P1.Z)))); }
public static Vector3 CSPtoVector3(CameraSpacePoint a_csp) { return(new Vector3(a_csp.X, a_csp.Y, a_csp.Z)); }
/// <summary> /// Handles the body frame data arriving from the sensor /// </summary> /// <param name="sender">object sending the event</param> /// <param name="e">event arguments</param> //TODO ほんとにこれでよい? private void Reader_MultiSourceFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e) { bool dataReceived = false; using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame().BodyFrameReference.AcquireFrame()) { if (bodyFrame != null) { if (this.bodies == null) { this.bodies = new Body[bodyFrame.BodyCount]; } // 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. bodyFrame.GetAndRefreshBodyData(this.bodies); dataReceived = true; } } using (ColorFrame colorFrame = e.FrameReference.AcquireFrame().ColorFrameReference.AcquireFrame()) { if (colorFrame != null) { if (this.bodyColors == null) { } } } using (DrawingContext dc = this.drawingGroup.Open()) { if (dataReceived) { // Draw a transparent background to set the render size dc.DrawRectangle(Brushes.Black, null, new Rect(0.0, 0.0, this.displayWidth, this.displayHeight)); int penIndex = 0; foreach (Body body in this.bodies) { Pen drawPen = this.bodyColors[penIndex++]; if (body.IsTracked) { if (!bodyDrawDictionary.ContainsKey(body)) { List <List <Point> > newPointListList = new List <List <Point> >(); newPointListList.Add(new List <Point>()); bodyDrawDictionary.Add(body, newPointListList); } List <List <Point> > pointListList = bodyDrawDictionary[body]; if (pointListList.Count > 10) { pointListList.RemoveAt(0); } 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) { // sometimes the depth(Z) of an inferred joint may show as negative // clamp down to 0.1f to prevent coordinatemapper from returning (-Infinity, -Infinity) CameraSpacePoint position = joints[jointType].Position; if (position.Z < 0) { position.Z = InferredZPositionClamp; } DepthSpacePoint depthSpacePoint = this.coordinateMapper.MapCameraPointToDepthSpace(position); jointPoints[jointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y); } this.DrawBody(joints, jointPoints, dc, drawPen); this.DrawHand(body.HandLeftState, jointPoints[JointType.HandLeft], dc); this.DrawHand(body.HandRightState, jointPoints[JointType.HandRight], dc); // クリア(全消去)する //if ( body.HandLeftState == HandState.Closed ) //{ // pointListList = new List<List<Point>>(); // pointListList.Add(new List<Point>()); //} List <Point> lastPointList = pointListList[pointListList.Count - 1]; // 長過ぎる線は先端から消していく if (lastPointList.Count > 100) { lastPointList.RemoveAt(0); } // 条件を満たしたとき pointListに追加する if (lastPointList.Count > 0 && body.HandRightState == HandState.Closed) { Point tmpPoint = jointPoints[JointType.HandRight]; Point lastPoint = lastPointList[lastPointList.Count - 1]; if (Math.Abs(tmpPoint.X - lastPoint.X) > 10 || Math.Abs(tmpPoint.Y - lastPoint.Y) > 10) { lastPointList.Add(tmpPoint); } } HandState prevHandState; if (prevHandStateDictionary.ContainsKey(body)) { prevHandState = prevHandStateDictionary[body]; } else { prevHandState = HandState.Unknown; } if (body.HandRightState != HandState.NotTracked && body.HandRightState != HandState.Unknown && prevHandState != body.HandRightState) { if (body.HandRightState == HandState.Closed) { //閉じた手に変わった瞬間 isRightHandClosedDictionary[body] = false; String str = ""; str += " " + DateTime.Now.ToString(); str += " Start"; Console.WriteLine(str); controlState = ControlState.Start; startPoint = jointPoints[JointType.HandRight]; lastPointList.Add(startPoint); } else if (prevHandState == HandState.Closed) { //閉じた手だったのがそれ以外に変わった瞬間 isRightHandClosedDictionary[body] = true; String str = ""; str += body.ToString(); str += " " + DateTime.Now.ToString(); str += " End"; str += " tmpHandState:" + prevHandState; str += " body.HandRightState: " + body.HandRightState; Console.WriteLine(str); controlState = ControlState.None; endPoint = jointPoints[JointType.HandRight]; lastPointList.Add(endPoint); pointListList.Add(new List <Point>()); } } if (body.HandRightState != HandState.Unknown && body.HandRightState != HandState.NotTracked) { prevHandStateDictionary[body] = body.HandRightState; } if (body.HandLeftState == HandState.Lasso) { Point currentPoint = jointPoints[JointType.HandLeft]; double cX = currentPoint.X; double cY = currentPoint.Y; for (int j = 0; j < pointListList.Count; j++) { List <Point> pointList = pointListList[j]; for (int i = 0; i < pointList.Count - 1; i++) { Point prevPoint = pointList[i]; Point nextPoint = pointList[i + 1]; double dX = nextPoint.X - prevPoint.X; double dY = nextPoint.Y - prevPoint.Y; double d = Math.Abs(dY / dX * cX - cY); if (d < 100) { pointListList.RemoveAt(j); break; } } } } } } // prevent drawing outside of our render area this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, this.displayWidth, this.displayHeight)); } foreach (KeyValuePair <Body, List <List <Point> > > pair in bodyDrawDictionary) { List <List <Point> > pointListList = pair.Value; /* 線の描画 */ for (int j = 0; j < pointListList.Count; j++) { List <Point> pointList = pointListList[j]; for (int i = 0; i < pointList.Count - 1; i++) { Point lineStartPoint = pointList[i]; Point lineEndPoint = pointList[i + 1]; dc.DrawLine(pen, lineStartPoint, lineEndPoint); } } } } }
/// <summary> /// Returns the Euclidean distante betweeen two 3D vectors p1 - p2. /// </summary> /// <param name="p1">First 3D vector</param> /// <param name="p2">Second 3D vector</param> /// <returns></returns> public double EuclideanDistance3d(CameraSpacePoint p1, CameraSpacePoint p2) { return(Math.Sqrt(Math.Pow(p1.X - p2.X, 2) + Math.Pow(p1.Y - p2.Y, 2) + Math.Pow(p1.Z - p2.Z, 2))); }
private void Reader_MultiSourceFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e) { MultiSourceFrame frameReference = e.FrameReference.AcquireFrame(); using (ColorFrame colorFrame = frameReference.ColorFrameReference.AcquireFrame()) { if (colorFrame != null) { mainWindow.cameraImage.Source = colorFrame.ToBitmap(); } } bool dataReceived = false; using (BodyFrame bodyFrame = frameReference.BodyFrameReference.AcquireFrame()) { if (bodyFrame != null) { mainWindow.bodyCanvas.Children.Clear(); if (this.bodies == null) { this.bodies = new Body[bodyFrame.BodyCount]; } bodyFrame.GetAndRefreshBodyData(bodies); dataReceived = true; } } if (dataReceived) { int bodyColor = 0; if (this.bodies != null) { mainWindow.client.Bodies = this.bodies; int maxBodies = this.kinectSensor.BodyFrameSource.BodyCount; int trackedBodies = 0; for (int i = 0; i < maxBodies; i++) { if (bodies[i] != null) { if (bodies[i].IsTracked) { trackedBodies++; IReadOnlyDictionary <JointType, Joint> joints = bodies[i].Joints; Dictionary <JointType, Point> jointPoints = new Dictionary <JointType, Point>(); foreach (JointType jointType in joints.Keys) { CameraSpacePoint position = joints[jointType].Position; if (position.Z < 0) { position.Z = InferredZPosition; } ColorSpacePoint colorSpacePoint = this.coordinateMapper.MapCameraPointToColorSpace(position); double x = float.IsInfinity(colorSpacePoint.X) ? 0 : colorSpacePoint.X / COLOR_SPACE_MAX_WIDTH * mainWindow.bodyCanvas.ActualWidth; double y = float.IsInfinity(colorSpacePoint.Y) ? 0 : colorSpacePoint.Y / COLOR_SPACE_MAX_HEIGHT * mainWindow.bodyCanvas.ActualHeight; jointPoints[jointType] = new Point(x, y); } mainWindow.bodyCanvas.DrawSkeleton(joints, jointPoints, color[2 * bodyColor], color[2 * bodyColor + 1]); } } bodyColor++; } changeNumberOfTrackedPeople(trackedBodies); } } }
/// <summary> /// Handles the body frame data arriving from the sensor /// </summary> /// <param name="sender">object sending the event</param> /// <param name="e">event arguments</param> private void Reader_FrameArrived(object sender, BodyFrameArrivedEventArgs e) { bool dataReceived = false; using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame()) { if (bodyFrame != null) { if (this.bodies == null) { this.bodies = new Body[bodyFrame.BodyCount]; } // 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. bodyFrame.GetAndRefreshBodyData(this.bodies); dataReceived = true; } } if (dataReceived) { using (DrawingContext dc = this.drawingGroup.Open()) { // Draw a transparent background to set the render size dc.DrawRectangle(Brushes.Black, null, new Rect(0.0, 0.0, this.displayWidth, this.displayHeight)); int penIndex = 0; foreach (Body body in this.bodies) { Pen drawPen = this.bodyColors[penIndex++]; 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) { // sometimes the depth(Z) of an inferred joint may show as negative // clamp down to 0.1f to prevent coordinatemapper from returning (-Infinity, -Infinity) CameraSpacePoint position = joints[jointType].Position; if (position.Z < 0) { position.Z = InferredZPositionClamp; } DepthSpacePoint depthSpacePoint = this.coordinateMapper.MapCameraPointToDepthSpace(position); jointPoints[jointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y); } Joint kneeR = body.Joints[JointType.KneeRight]; Joint ankleR = body.Joints[JointType.AnkleRight]; Joint hipR = body.Joints[JointType.HipRight]; Joint spine = body.Joints[JointType.SpineBase]; Joint kneeL = body.Joints[JointType.KneeLeft]; int myangle = (int)angle(kneeL, spine, kneeR); //Console.WriteLine(angle(hipR, ankleR, kneeR)); int rAngle = (int)angle(ankleR, kneeR, hipR); this.DrawBody(joints, jointPoints, dc, drawPen); this.DrawHand(body.HandLeftState, jointPoints[JointType.HandLeft], dc); this.DrawHand(body.HandRightState, jointPoints[JointType.HandRight], dc); if (recording) { x.Add(myangle); kneeAngle.Add(rAngle); y.Add(count++); //Console.WriteLine(myangle); file.WriteLine(myangle); file2.WriteLine(rAngle); if (count % 10 == 0) { drawgraph(y, x, plotter); drawgraph(y, kneeAngle, plotter2); } } } } // prevent drawing outside of our render area this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, this.displayWidth, this.displayHeight)); } } }
// Handles the body frame data arriving from the sensor private void Reader_FrameArrived(object sender, BodyFrameArrivedEventArgs e) { bool dataReceived = false; using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame()) { if (bodyFrame != null) { if (this.bodies == null) { this.bodies = new Body[bodyFrame.BodyCount]; } // 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. bodyFrame.GetAndRefreshBodyData(this.bodies); dataReceived = true; } } if (dataReceived) { using (DrawingContext dc = this.drawingGroup.Open()) { /*******************************************************/ // Dibuja la batería this.DrawDrums(dc); /*******************************************************/ // Draw a transparent background to set the render size dc.DrawRectangle(Brushes.Transparent, null, new Rect(0.0, 0.0, this.displayWidth, this.displayHeight)); int penIndex = 0; //Mantiene la cuenta de cuerpos detectados para solo mostrar el máximo establecido int bodiesCount = 0; foreach (Body body in this.bodies) { Pen drawPen = this.bodyColors[penIndex++]; if (body.IsTracked) { 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) { // sometimes the depth(Z) of an inferred joint may show as negative // clamp down to 0.1f to prevent coordinatemapper from returning (-Infinity, -Infinity) CameraSpacePoint position = joints[jointType].Position; if (position.Z < 0) { position.Z = InferredZPositionClamp; } DepthSpacePoint depthSpacePoint = this.coordinateMapper.MapCameraPointToDepthSpace(position); jointPoints[jointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y); } this.DrawBody(joints, jointPoints, dc, drawPen); this.DrawCircle(body.HandLeftState, jointPoints[JointType.HandTipLeft], dc); this.DrawCircle(body.HandRightState, jointPoints[JointType.HandTipRight], dc); this.DrawCircle(body.HandLeftState, jointPoints[JointType.FootLeft], dc); this.DrawCircle(body.HandRightState, jointPoints[JointType.FootRight], dc); /*************************************************************************************************************/ //Comprueba si ha tocado un tambor OnDrumHit(jointPoints[JointType.HandTipLeft], jointPoints[JointType.HandTipRight], jointPoints[JointType.FootLeft], jointPoints[JointType.FootRight]); //Obtiene el ID del body actual ulong trackingId = body.TrackingId; // Si el TrackingId de un Body cambia, actualiza su correspondiente GestureDetector con el nuevo valor if (trackingId != this.gestureDetectorList[bodiesCount].TrackingId) { this.gestureDetectorList[bodiesCount].TrackingId = trackingId; // Si el Body actual esta siendo detectado, inicia su GestureDetector // Si no está siendo detectado, pausa el GestureDetector para ahorrar recursos this.gestureDetectorList[bodiesCount].IsPaused = trackingId == 0; } //Incrementa los cuerpos detectados; bodiesCount++; //Si ya se ha pintado el máximo de bodies, termina de pintarlos if (bodiesCount == maxBodies) { break; } /*************************************************************************************************************/ } } // prevent drawing outside of our render area this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, this.displayWidth, this.displayHeight)); } } }
public double vectMag(CameraSpacePoint point) { return(Math.Sqrt(Math.Pow((float)(point.X), 2) + Math.Pow((float)(point.Y), 2) + Math.Pow((float)(point.Z), 2))); }
//Update position of the raised hand public void Update() { if (trackingId != 0) { UserId = trackingId; } if (!isPaused) { //Get hand, elbow position of left and right hand with tracking ID from kinect camera #region GetPosition CameraSpacePoint leftHandPos = UserManager.bodies[UserManager.Users[trackingId]].Joints[JointType.HandLeft].Position; CameraSpacePoint rightHandPos = UserManager.bodies[UserManager.Users[trackingId]].Joints[JointType.HandRight].Position; CameraSpacePoint leftEl = UserManager.bodies[UserManager.Users[trackingId]].Joints[JointType.ElbowLeft].Position; CameraSpacePoint rightEl = UserManager.bodies[UserManager.Users[trackingId]].Joints[JointType.ElbowRight].Position; #endregion //================================================================================= //Check left hand rise #region CheckLeftHand if (CheckHandRaise(leftHandPos, leftEl)) { if (countLeft) { stopwatchLeft.Start(); countLeft = false; } long leftElapsed = stopwatchLeft.ElapsedMilliseconds; if (leftElapsed > RaiseTime) { OnHandRaise(JointType.HandLeft, UserId); ResetCountLeft(); } } else { ResetCountLeft(); } #endregion //================================================================================= //Check right hand rise #region CheckRightHand if (CheckHandRaise(rightHandPos, rightEl)) { if (countRight) { stopwatchRight.Start(); countRight = false; } long rightElapsed = stopwatchRight.ElapsedMilliseconds; if (rightElapsed > RaiseTime) { OnHandRaise(JointType.HandRight, UserId); ResetCountRight(); } } else { ResetCountRight(); } #endregion //================================================================================= } }
/// <summary> /// Handles the body frame data arriving from the sensor /// </summary> /// <param name="sender">object sending the event</param> /// <param name="e">event arguments</param> private void Reader_FrameArrived(object sender, BodyFrameArrivedEventArgs e) { bool dataReceived = true; using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame()) { if (ranOunce == false) { textBox.Text = username; using (DataService.Service1Client s = new DataService.Service1Client()) { retrievedData = s.GetBodyData(username, sessionname).ToArray(); } ranOunce = true; } if (bodyFrame != null) { if (this.bodies == null) { this.bodies = new Body[bodyFrame.BodyCount]; } // 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. bodyFrame.GetAndRefreshBodyData(this.bodies); dataReceived = true; } } if (dataReceived) { using (DrawingContext dc = this.drawingGroup.Open()) { // Draw a transparent background to set the render size dc.DrawRectangle(Brushes.Black, null, new Rect(0.0, 0.0, this.displayWidth, this.displayHeight)); int penIndex = 0; Pen drawPen = this.bodyColors[penIndex++]; if (true) { this.DrawClippedEdges(this.bodies[0], dc); IReadOnlyDictionary <JointType, Joint> joints = this.bodies[0].Joints; // convert the joint points to depth (display) space Dictionary <JointType, Point> jointPoints = new Dictionary <JointType, Point>(); foreach (JointType jointType in joints.Keys) { // sometimes the depth(Z) of an inferred joint may show as negative // clamp down to 0.1f to prevent coordinatemapper from returning (-Infinity, -Infinity) CameraSpacePoint position = joints[jointType].Position; // if (sendMe) // { // using (XboxWCFService.Service1Client s = new XboxWCFService.Service1Client()) //{ // textBox.Text = s.GetData(Convert.ToInt32(position.Z)); // sendMe = false; // } //} if (position.Z < 0) { position.Z = InferredZPositionClamp; } DepthSpacePoint depthSpacePoint = this.coordinateMapper.MapCameraPointToDepthSpace(position); jointPoints[jointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y); } joints = Newtonsoft.Json.JsonConvert.DeserializeObject <IReadOnlyDictionary <JointType, Joint> >(retrievedData[replayCount].Joints); jointPoints = Newtonsoft.Json.JsonConvert.DeserializeObject <Dictionary <JointType, Point> >(retrievedData[replayCount].JointPoints); if (replayCount < retrievedData.Length - 1) { replayCount++; } this.DrawBody(joints, jointPoints, dc, drawPen); this.DrawHand(this.bodies[0].HandLeftState, jointPoints[JointType.HandLeft], dc); this.DrawHand(this.bodies[0].HandRightState, jointPoints[JointType.HandRight], dc); } // prevent drawing outside of our render area this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, this.displayWidth, this.displayHeight)); } } }
// multi frame reader event handler private void ReaderMultiFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e) { // Get a reference to the multi-frame var reference = e.FrameReference.AcquireFrame(); // depth using (DepthFrame depthFrame = reference.DepthFrameReference.AcquireFrame()) { string label_coords_blob = "";//label_blobsの文字列 if (depthFrame != null) { FrameDescription depthFrameDescription = depthFrame.FrameDescription; int width = depthFrameDescription.Width; int height = depthFrameDescription.Height; var depthData = new ushort[width * height]; depthFrame.CopyFrameDataToArray(depthData); this.depthBitmap.WritePixels(depthRect, depthData, width * (int)depthFrameDescription.BytesPerPixel, 0); // depthData -> camera space data CameraSpacePoint[] cs_points = new CameraSpacePoint[width * height]; _sensor.CoordinateMapper.MapDepthFrameToCameraSpace(depthData, cs_points); // calculate camera space coordinates of each marker(blob) // plan: 以下のループは別functionにする float[,] coord_blobs_center = new float[list_arr_index.Count(), 3]; // blob中心座標の2次元配列 //label_sample.Content = list_arr_index.Count().ToString(); int i_blob = 0; // blobのindex foreach (int[] arr_index in list_arr_index) { // 各blobのcamera space pointからx, y, z座標を取り出して配列 -> 平均 float[] coord_blob_center = new float[3];//blob (反射マーカー)の中心座標を入れる // select camera space points corresponding each blob CameraSpacePoint[] cs_points_blob = new CameraSpacePoint[arr_index.Length];// camera space配列宣言 // x,y,z座標のlist List <float> list_x_cs_points_blob = new List <float>(); List <float> list_y_cs_points_blob = new List <float>(); List <float> list_z_cs_points_blob = new List <float>(); // x,y,z座標の平均 float x_coord_cs_points_blob = 0; float y_coord_cs_points_blob = 0; float z_coord_cs_points_blob = 0; // listの初期化. 念のため list_x_cs_points_blob.Clear(); list_y_cs_points_blob.Clear(); list_z_cs_points_blob.Clear(); // for loop int i_points_blob = 0; // blob内のcs_pointsのindex //int i_coord_blob = 0; // blob内の座標のindex foreach (int i_point in arr_index) { // arr_index: blobのcamera space pointsに対応するindexes // cs_points_blobをまとめる cs_points_blob[i_points_blob] = cs_points[i_point]; i_points_blob += 1; // x,y,z座標のlistを求める: infinityを外す if (!Double.IsInfinity(cs_points[i_point].X)) { list_x_cs_points_blob.Add(cs_points[i_point].X); list_y_cs_points_blob.Add(cs_points[i_point].Y); list_z_cs_points_blob.Add(cs_points[i_point].Z); // 座標の足し算 x_coord_cs_points_blob += cs_points[i_point].X; y_coord_cs_points_blob += cs_points[i_point].Y; z_coord_cs_points_blob += cs_points[i_point].Z; } } // listを配列に変換 float[] arr_x_cs_points_blob = list_x_cs_points_blob.ToArray(); float[] arr_y_cs_points_blob = list_y_cs_points_blob.ToArray(); float[] arr_z_cs_points_blob = list_z_cs_points_blob.ToArray(); // cs_points_blobからblobの中心座標を求める //////////////////// // infの割合を求める float ratio_valid_points_blob = (float)arr_x_cs_points_blob.Length / (float)arr_index.Length;// blobの内infinityでなかったpointの割合 // infの割合が1割以以上だったら中心座標の計算 if (ratio_valid_points_blob > 0.0) { // 足し算したものを数で割る x_coord_cs_points_blob = x_coord_cs_points_blob / (float)arr_x_cs_points_blob.Count(); y_coord_cs_points_blob = y_coord_cs_points_blob / (float)arr_y_cs_points_blob.Count(); // 分母はどれも同じ z_coord_cs_points_blob = z_coord_cs_points_blob / (float)arr_z_cs_points_blob.Count(); // 分母はどれも同じ } else { x_coord_cs_points_blob = 0; y_coord_cs_points_blob = 0; z_coord_cs_points_blob = 0; } coord_blob_center = new float[] { x_coord_cs_points_blob, y_coord_cs_points_blob, z_coord_cs_points_blob }; // 座標coord_blob_centerを二次元配列にまとめる+ label_coordsのstringを生成 for (int i_xyz = 0; i_xyz < 3; i_xyz++) { coord_blobs_center[i_blob, i_xyz] = coord_blob_center[i_xyz]; } label_coords_blob += string.Format("X: {0:+000.0;-000.0;+ 0.0}, ", coord_blob_center[0] * 100) + string.Format("Y: {0:+000.0;-000.0;+ 0.0}, ", coord_blob_center[1] * 100) + string.Format("Z: {0:+000.0;-000.0;+ 0.0}\n", coord_blob_center[2] * 100); i_blob += 1; } // coord_blobs_centerを画面に出力 label_coords.Content = label_coords_blob; } } // IR using (InfraredFrame infraredFrame = reference.InfraredFrameReference.AcquireFrame()) { if (infraredFrame != null) { FrameDescription infraredFrameDescription = infraredFrame.FrameDescription; int width = infraredFrameDescription.Width; int height = infraredFrameDescription.Height; //ushort[] infraredData = new ushort[width * height]; // http://www.naturalsoftware.jp/entry/2014/07/25/020750 var infraredData = new ushort[width * height]; // ushort array infraredFrame.CopyFrameDataToArray(infraredData); this.infraredBitmap.Lock(); this.infraredBitmap.WritePixels(infraredRect, infraredData, width * (int)infraredFrameDescription.BytesPerPixel, 0); //depthImage.WritePixels(depthRect, depthBuffer, depthStride, 0);// template this.infraredBitmap.Unlock(); ColorImage.Source = this.infraredBitmap; // OpenCV: Count blobs and CountBlobs(this.infraredBitmap); } } }
private void UpdateFacePoints() { if (_faceModel == null) { return; } var vertices = _faceModel.CalculateVerticesForAlignment(_faceAlignment); if (vertices.Count > 0) { if (_points.Count == 0) { for (int index = 0; index < vertices.Count; index++) { Ellipse ellipse = new Ellipse { Width = 2.0, Height = 2.0, Fill = new SolidColorBrush(Colors.Blue) }; _points.Add(ellipse); } foreach (Ellipse ellipse in _points) { canvas.Children.Add(ellipse); } } for (int index = 0; index < vertices.Count; index++) { CameraSpacePoint vertice = vertices[index]; DepthSpacePoint point = _sensor.CoordinateMapper.MapCameraPointToDepthSpace(vertice); //inner eyebrow check if (index == 803 || index == 346) { DepthSpacePoint innerbrowl = _sensor.CoordinateMapper.MapCameraPointToDepthSpace(vertices[803]); DepthSpacePoint innerbrowr = _sensor.CoordinateMapper.MapCameraPointToDepthSpace(vertices[346]); double dx = innerbrowl.X - innerbrowr.X; double dy = innerbrowl.Y - innerbrowr.Y; double dist = dx * dx + dy * dy; // Debug.WriteLine("eyebrowdistance " + Math.Round(Math.Sqrt( dist), 1)); } //eyelid check if (index == 866 || index == 868) { DepthSpacePoint eyelid1 = _sensor.CoordinateMapper.MapCameraPointToDepthSpace(vertices[866]); DepthSpacePoint eyelid2 = _sensor.CoordinateMapper.MapCameraPointToDepthSpace(vertices[868]); double dx = eyelid1.X - eyelid2.X; double dy = eyelid1.Y - eyelid2.Y; double dist = dx * dx + dy * dy; // Debug.WriteLine("eyelidopen " + Math.Round(Math.Sqrt(dist), 1)); } //nose wrinkler if (index == 673 || index == 24) { DepthSpacePoint nose1 = _sensor.CoordinateMapper.MapCameraPointToDepthSpace(vertices[11]); DepthSpacePoint nose2 = _sensor.CoordinateMapper.MapCameraPointToDepthSpace(vertices[1]); double dx = nose1.X - nose2.X; double dy = nose1.Y - nose2.Y; double dist = dx * dx + dy * dy; // Debug.WriteLine("side nose distance " + Math.Round(Math.Sqrt(dist), 1)); } //lip level raise if (index == 309 || index == 761) { DepthSpacePoint nose1 = _sensor.CoordinateMapper.MapCameraPointToDepthSpace(vertices[309]); DepthSpacePoint nose2 = _sensor.CoordinateMapper.MapCameraPointToDepthSpace(vertices[761]); double dx = nose1.X - nose2.X; double dy = nose1.Y - nose2.Y; double dist = dx * dx + dy * dy; // Debug.WriteLine("lip level raise " + Math.Round(Math.Sqrt(dist), 1)); } if (float.IsInfinity(point.X) || float.IsInfinity(point.Y)) { return; } Ellipse ellipse = _points[index]; Canvas.SetLeft(ellipse, point.X); Canvas.SetTop(ellipse, point.Y); } } }
public static BitmapSource SliceDepthImageWithRectWithoutPlane(this DepthFrame image, Floor floor, CoordinateMapper coordinateMapper, float planePos, int min = 20, int max = 1000, int left = 0, int top = 0, int right = 512, int bottom = 424) { ushort[] _depthData = new ushort[512 * 424]; image.CopyFrameDataToArray(_depthData); CameraSpacePoint[] depthMappedToCameraPoints = new CameraSpacePoint[512 * 424]; coordinateMapper.MapDepthFrameToCameraSpace( _depthData, depthMappedToCameraPoints); CameraSpacePoint s; double dist; var tmFrameDescription = image.FrameDescription; int width = tmFrameDescription.Width; //image.Width; int height = tmFrameDescription.Height; // image.Height; //var depthFrame = image.Image.Bits; //short[] rawDepthData = new short[tmFrameDescription.LengthInPixels]; //new short[image.PixelDataLength]; ushort[] rawDepthData = new ushort[tmFrameDescription.LengthInPixels]; //image.CopyPixelDataTo(rawDepthData); image.CopyFrameDataToArray(rawDepthData); var pixels = new byte[height * width * 4]; const int BlueIndex = 0; const int GreenIndex = 1; const int RedIndex = 2; int w, h = 0; for (int depthIndex = 0, colorIndex = 0; depthIndex < rawDepthData.Length && colorIndex < pixels.Length; depthIndex++, colorIndex += 4) { h = depthIndex / 512; w = depthIndex % 512; if (w < left || right < w || h < top || bottom < h) { continue; } s = depthMappedToCameraPoints[depthIndex]; dist = floor.DistanceFrom(s); //if (dist > -0.2f) if (dist > planePos) { // Calculate the distance represented by the two depth bytes //int depth = rawDepthData[depthIndex] >> DepthImageFrame.PlayerIndexBitmaskWidth; //int depth = rawDepthData[depthIndex] >> image.DepthMinReliableDistance; int depth = rawDepthData[depthIndex]; // Map the distance to an intesity that can be represented in RGB var intensity = CalculateIntensityFromDistance(depth); if (depth > min && depth < max) { // Apply the intensity to the color channels pixels[colorIndex + BlueIndex] = intensity; //blue pixels[colorIndex + GreenIndex] = intensity; //green pixels[colorIndex + RedIndex] = intensity; //red } } } return(BitmapSource.Create(width, height, 96, 96, PixelFormats.Bgr32, null, pixels, width * 4)); }
void bfReader_FrameArrived(object sender, BodyFrameArrivedEventArgs e) { using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame()) { if (bodyFrame != null) { if (this.bodies == null) { this.bodies = new Body[bodyFrame.BodyCount]; } // 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. bodyFrame.GetAndRefreshBodyData(this.bodies); } } if (this.bodies != null && this.bodies.Length > 0) { foreach (Body body in this.bodies) { if (body.IsTracked) { lblLefthand.Text = "Left Hand : " + GetHandStateString(body.HandLeftState); lblRighthand.Text = "Right Hand : " + GetHandStateString(body.HandRightState); //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) //{ // // sometimes the depth(Z) of an inferred joint may show as negative // // clamp down to 0.1f to prevent coordinatemapper from returning (-Infinity, -Infinity) // CameraSpacePoint position = joints[jointType].Position; // if (position.Z < 0) // { // position.Z = InferredZPositionClamp; // } // DepthSpacePoint depthSpacePoint = this.coordinateMapper.MapCameraPointToDepthSpace(position); // jointPoints[jointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y); //} Joint rightThumb = body.Joints[JointType.HandTipRight]; Joint rightWrist = body.Joints[JointType.WristRight]; Joint rightShoulder = body.Joints[JointType.ShoulderRight]; Joint rightTb = body.Joints[JointType.ThumbRight]; Joint leftTip = body.Joints[JointType.HandTipLeft]; Joint leftShoulder = body.Joints[JointType.ShoulderLeft]; CameraSpacePoint cspt = rightThumb.Position; CameraSpacePoint csptW = rightWrist.Position; CameraSpacePoint csptS = rightShoulder.Position; CameraSpacePoint csptT = rightTb.Position; CameraSpacePoint csptLTip = leftTip.Position; CameraSpacePoint csptLShoulder = leftShoulder.Position; string rightThumbPosition = "\n Right Thumb : "; if (rightThumb != null && rightThumb.TrackingState != TrackingState.NotTracked) { rightThumbPosition += " tracked/infered.. "; if (cspt.Z < 0) { cspt.Z = 0.1f; //to avoid mapping errors as it may return -ve value for Z some times } DepthSpacePoint dspt = mapper.MapCameraPointToDepthSpace(cspt); DepthSpacePoint dsptW = mapper.MapCameraPointToDepthSpace(csptW); DepthSpacePoint dsptS = mapper.MapCameraPointToDepthSpace(csptS); DepthSpacePoint dsptT = mapper.MapCameraPointToDepthSpace(csptT); DepthSpacePoint dsptLTip = mapper.MapCameraPointToDepthSpace(csptLTip); DepthSpacePoint dsptLShoulder = mapper.MapCameraPointToDepthSpace(csptLShoulder); rightThumbPosition += "Depth space -- X : " + dspt.X + " Y : " + dspt.Y + "\n"; // rightThumbPosition += "Depth spaceW -- X : " + dsptW.X + " Y : " + dsptW.Y + "\n"; //srightThumbPosition += "Depth spaceS -- Y : " + dsptS.Y ; lock (this) { if (DoAutoHelp && !IsProcessing && !CanRecord) { IsProcessing = true; CheckAutoHelpCoordinates(dspt, dsptLTip); IsProcessing = false; } } switch (body.HandLeftState) { case HandState.Open: break; case HandState.Closed: break; case HandState.Lasso: lock (this) { if (CanRecord && !IsProcessing && dsptLShoulder.Y > dsptLTip.Y) { IsProcessing = true; RecordCoordinates(dspt, dsptW, dsptS, dsptT); IsProcessing = false; } } break; } } else { rightThumbPosition += "NOT tracked.. "; } lblRighthand.Text += rightThumbPosition; lblStatus.Text = "Bodies are being tracked..."; switch (body.HandRightState) { case HandState.Open: break; case HandState.Closed: break; case HandState.Lasso: break; } } } } else { lblStatus.Text = "No bodies to track.."; } }
public float dotProduct(CameraSpacePoint point1, CameraSpacePoint point2) { return(point1.X * point2.X + point1.Y * point2.Y + point1.Z * point2.Z); }
/// <summary> /// Handles the body frame data arriving from the sensor /// </summary> /// <param name="sender">object sending the event</param> /// <param name="e">event arguments</param> private void Reader_FrameArrived(object sender, BodyFrameArrivedEventArgs e) { bool dataReceived = false; using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame()) { if (bodyFrame != null) { if (this.bodies == null) { this.bodies = new Body[bodyFrame.BodyCount]; } // 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. bodyFrame.GetAndRefreshBodyData(this.bodies); dataReceived = true; } } if (dataReceived) { using (DrawingContext dc = this.drawingGroup.Open()) { // Draw a transparent background to set the render size dc.DrawRectangle(Brushes.Black, null, new Rect(0.0, 0.0, 1920, 1080)); int penIndex = 0; foreach (Body body in this.bodies) { Pen drawPen = this.bodyColors[penIndex++]; 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) { // sometimes the depth(Z) of an inferred joint may show as negative // clamp down to 0.1f to prevent coordinatemapper from returning (-Infinity, -Infinity) CameraSpacePoint position = joints[jointType].Position; if (position.Z < 0) { position.Z = InferredZPositionClamp; } DepthSpacePoint depthSpacePoint = this.coordinateMapper.MapCameraPointToDepthSpace(position); jointPoints[jointType] = screenMapper.projectToScreen(position); } this.DrawBody(joints, jointPoints, dc, drawPen); //this.DrawHand(body.HandLeftState, jointPoints[JointType.HandLeft], dc); //this.DrawHand(body.HandRightState, jointPoints[JointType.HandRight], dc); BitmapImage head = new BitmapImage(new Uri(@"C:/skeleton-head.png")); //this.DrawImage(jointPoints[JointType.Head], jointPoints[JointType.Neck], head, dc); //draw head } } // prevent drawing outside of our render area this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, 1920, 1080)); } } }
//public float AngleBetweenTwoVectors(Vector3 b1, Vector3 b2) //{ // AngleBetweenTwoVectors(Vector3.Normalize(b1), Vector3.Normlize(b2).ToString()); // float dotProduct = 0.0f; // dotProduct = Vector3.Dot(b1, b2); // double Out = Math.Acos(dotProduct); // return (float)Math.Acos(dotProduct); //} public double AngleThreePoints(CameraSpacePoint point1, CameraSpacePoint point2, CameraSpacePoint point3) { CameraSpacePoint vect1 = subVector(point2, point3); CameraSpacePoint vect2 = subVector(point1, point2); double angle = Math.Round((180 - (Math.Acos((dotProduct(vect1, vect2)) / ((vectMag(vect1) * vectMag(vect2)))) * (180 / Math.PI)))); return(angle); }
public static IPoint3D Point3DFromCameraSpacePoint(CameraSpacePoint csp) { return(new Vector3D(csp.X, csp.Y, csp.Z)); }
/// <summary> /// Draws a body /// </summary> /// <param name="joints">joints to draw</param> /// <param name="jointPoints">translated positions of joints to draw</param> /// <param name="drawingContext">drawing context to draw to</param> /// <param name="drawingPen">specifies color to draw a specific body</param> private void DrawBody(IReadOnlyDictionary <JointType, Joint> joints, IDictionary <JointType, Point> jointPoints, DrawingContext drawingContext, Pen drawingPen) { CameraSpacePoint point1 = new CameraSpacePoint(); CameraSpacePoint point2 = new CameraSpacePoint(); CameraSpacePoint point3 = new CameraSpacePoint(); Boolean point1init = false; Boolean point2init = false; Boolean point3init = false; double angle = 0; double check = 0; int i = 0; // Draw the bones Thread t = new Thread(new ThreadStart(ThreadProc)); t.Start(); foreach (var bone in this.bones) { //joints[16 or 17 or 18].position -> returns x, y, z coordinates this.DrawBone(joints, jointPoints, bone.Item1, bone.Item2, drawingContext, drawingPen); //Debug.WriteLine("Joint " + bone.Item1 + " : " + joints[bone.Item1].Position.X); if (bone.Item1 == JointType.KneeRight) { point2 = joints[bone.Item1].Position; point1 = joints[bone.Item2].Position; point1init = true; point2init = true; } if (bone.Item1 == JointType.HipRight) { point3 = joints[bone.Item1].Position; point3init = true; } if (point1init && point2init && point3init) { angle = AngleThreePoints(point1, point2, point3); //check array for 2 length //if not, only push //if so, first subtract numbers and check //then delete first element //then push //if subtract is less than 0 counter++; angleOld = angleNew; angleNew = angle; //Debug.WriteLine("ANGLE: " + angle); //Debug.WriteLine("STATE: " + state); if (angle < 90) { check = angleOld - angleNew; //Debug.WriteLine(check + "a" ); //Debug.WriteLine(angles[0] + "angle 0"); //Debug.WriteLine(angles[1] + "angle 1"); if (!state) { counter++; state = true; Debug.WriteLine(counter); //Debug.WriteLine(""); //Debug.WriteLine("COUNT: " + counter); //Debug.WriteLine("old angle: " + angleOld); //Debug.WriteLine("new angle: " + angleNew); //Debug.WriteLine("angle: " + angle); //Debug.WriteLine("state: " + state); //Debug.WriteLine("check: " + check); if (t.IsAlive) { t.Abort(); } t = new Thread(new ThreadStart(ThreadProc)); t.Start(); } } else if (angle > 95) { state = false; //Debug.WriteLine(" RESET"); } point1init = false; point2init = false; point3init = false; } } // Draw the joints foreach (JointType jointType in joints.Keys) { Brush drawBrush = null; TrackingState trackingState = joints[jointType].TrackingState; if (trackingState == TrackingState.Tracked) { drawBrush = this.trackedJointBrush; } else if (trackingState == TrackingState.Inferred) { drawBrush = this.inferredJointBrush; } if (drawBrush != null) { drawingContext.DrawEllipse(drawBrush, null, jointPoints[jointType], JointThickness, JointThickness); } } }
/// <summary> /// Handles the body frame data arriving from the sensor /// </summary> /// <param name="sender">object sending the event</param> /// <param name="e">event arguments</param> private void Reader_FrameArrived(object sender, BodyFrameArrivedEventArgs e) { bool dataReceived = false; using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame()) { if (bodyFrame != null) { if (this.bodies == null) { this.bodies = new Body[bodyFrame.BodyCount]; } // 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. bodyFrame.GetAndRefreshBodyData(this.bodies); dataReceived = true; } } //if there are bodies: if (dataReceived) { //recieve boxes using (DrawingContext dc = this.drawingGroup.Open()) { // Draw a transparent background to set the render size dc.DrawRectangle(Brushes.Black, null, new Rect(0.0, 0.0, this.displayWidth, this.displayHeight)); int penIndex = 0; foreach (Body body in this.bodies) { Pen drawPen = this.bodyColors[penIndex++]; 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) { // sometimes the depth(Z) of an inferred joint may show as negative // clamp down to 0.1f to prevent coordinatemapper from returning (-Infinity, -Infinity) CameraSpacePoint position = joints[jointType].Position; if (position.Z < 0) { position.Z = InferredZPositionClamp; } DepthSpacePoint depthSpacePoint = this.coordinateMapper.MapCameraPointToDepthSpace(position); jointPoints[jointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y); } this.DrawBody(joints, jointPoints, dc, drawPen); ///hand pos!! Point leftHandPos = jointPoints[JointType.HandLeft]; Point rightHandPos = jointPoints[JointType.HandRight]; HandState leftHand = body.HandLeftState; HandState rightHand = body.HandRightState; this.DrawHand(body.HandLeftState, jointPoints[JointType.HandLeft], dc); this.DrawHand(body.HandRightState, jointPoints[JointType.HandRight], dc); // add boxes SolidColorBrush color = Brushes.Red; //draw test box foreach (var label in this.Labels) { DrawBox(label.Box[0], label.Box[1], label.Box[2], label.Box[3], dc, color); if (HandInBox(label.Box, leftHandPos, leftHand, rightHandPos, rightHand)) { color = Brushes.Green; Debug.WriteLine(label.Name); } else { color = Brushes.Red; } } // prevent drawing outside of our render area this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, this.displayWidth, this.displayHeight)); } } } } }
private double AccelBetPoints(CameraSpacePoint P1, CameraSpacePoint P2, CameraSpacePoint P3) { return((SpeedBetPoints(P2, P3) - SpeedBetPoints(P1, P2)) / 0.0333333333); }
float CSVectorLength(CameraSpacePoint p) { return(Convert.ToSingle(Math.Sqrt(p.X * p.X + p.Y * p.Y + p.Z * p.Z))); }
private double SpeedBetPoints(CameraSpacePoint P1, CameraSpacePoint P2) { return(DistBetPoints(P1, P2) / 0.0333333333); }
public HandsState(Body body) { CameraSpacePoint handLeft = body.Joints[JointType.HandLeft].Position; CameraSpacePoint handRight = body.Joints[JointType.HandRight].Position; wristLeft = body.Joints[JointType.WristLeft].Position; wristRight = body.Joints[JointType.WristRight].Position; CameraSpacePoint spineBase = body.Joints[JointType.SpineBase].Position; input = new HandInput(); input.isLeftGrip = (body.HandLeftState == HandState.Closed); input.isRightGrip = (body.HandRightState == HandState.Closed); //select wrist if (prime_hand) { primeHandy = (int)(wristRight.Y * 100); isRight = true; } else { primeHandy = (int)(wristLeft.Y * 100); isRight = false; } //set left hand position float leftDepth = spineBase.Z - handLeft.Z; input._LPressExtent = (leftDepth - TOUCH_REGION) / (EMBED_REGION - TOUCH_REGION); if (leftDepth > EMBED_REGION) { LeftHandPosition = HandPositionZ.EMBED; } else if (leftDepth > TOUCH_REGION) { LeftHandPosition = HandPositionZ.TOUCH; } else { LeftHandPosition = HandPositionZ.UNKNOW; } //set right hand position float rightDepth = spineBase.Z - handRight.Z; input._RPressExtent = (rightDepth - TOUCH_REGION) / (EMBED_REGION - TOUCH_REGION); if (rightDepth > EMBED_REGION) { RightHandPosition = HandPositionZ.EMBED; } else if (rightDepth > TOUCH_REGION) { RightHandPosition = HandPositionZ.TOUCH; } else { RightHandPosition = HandPositionZ.UNKNOW; } //set left hand state LeftHandState = body.HandLeftState; //set right hand state RightHandState = body.HandRightState; //no hand if (LeftHandPosition == HandPositionZ.UNKNOW && RightHandPosition == HandPositionZ.UNKNOW) { operation = Operation.no_operation; input._isWhich = 0; } //single hand else if (LeftHandPosition == HandPositionZ.UNKNOW || RightHandPosition == HandPositionZ.UNKNOW) { //left hand operate if (LeftHandPosition != HandPositionZ.UNKNOW) { SelectHandPosition = LeftHandPosition; SelectHandState = LeftHandState; input._isWhich = 1; isRight = false; } //right hand operate else { SelectHandPosition = RightHandPosition; SelectHandState = RightHandState; input._isWhich = 2; isRight = true; } //single hand touch region if (SelectHandPosition == HandPositionZ.TOUCH) { if (SelectHandState == HandState.Closed) { if (mouse_click_region) { operation = Operation.left_down; } else { operation = Operation.right_down; } } else { operation = Operation.move; } } //single hand embed region else { if (SelectHandState == HandState.Closed) { if (mouse_click_region) { operation = Operation.right_down; } else { operation = Operation.left_down; } } else { operation = Operation.move; } } } else { //two hand closed will operate wheel input._isWhich = 3; if (LeftHandState == HandState.Closed && RightHandState == HandState.Closed) { if (middle_button_and_wheel) { operation = Operation.wheel; } else { operation = Operation.middle_down; } } //one hand closed else if (LeftHandState == HandState.Closed || RightHandState == HandState.Closed) { if (middle_button_and_wheel) { operation = Operation.middle_down; } else { operation = Operation.wheel; } } else { operation = Operation.move; } } }
private void MultiSourceFrameReader_MultiSourceFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e) { bool dataReceived = false; MultiSourceFrame multiSourceFrame = e.FrameReference.AcquireFrame(); using (ColorFrame colorFrame = multiSourceFrame.ColorFrameReference.AcquireFrame()) { if (colorFrame != null) { using (KinectBuffer colorBuffer = colorFrame.LockRawImageBuffer()) { colorBitmap.Lock(); if ((colorFrameDescription.Width == colorBitmap.PixelWidth) && (colorFrameDescription.Height == colorBitmap.PixelHeight)) { colorFrame.CopyConvertedFrameDataToIntPtr( colorBitmap.BackBuffer, (uint)(colorFrameDescription.Width * colorFrameDescription.Height * 4), ColorImageFormat.Bgra); colorBitmap.AddDirtyRect(new Int32Rect(0, 0, colorBitmap.PixelWidth, colorBitmap.PixelHeight)); } colorBitmap.Unlock(); } } } using (BodyFrame bodyFrame = multiSourceFrame.BodyFrameReference.AcquireFrame()) { if (bodyFrame != null) { if (bodies == null) { bodies = new Body[bodyFrame.BodyCount]; } bodyFrame.GetAndRefreshBodyData(bodies); dataReceived = true; } } if (dataReceived) { canvas.Children.Clear(); foreach (Body body in bodies.Where(b => b.IsTracked)) { int colorIndex = 0; foreach (var joint in body.Joints) { SolidColorBrush colorBrush = bodyColors[colorIndex++]; Dictionary <JointType, Point> jointColorPoints = new Dictionary <JointType, Point>(); CameraSpacePoint position = joint.Value.Position; if (position.Z < 0) { position.Z = 0.1f; } ColorSpacePoint colorSpacePoint = coordinateMapper.MapCameraPointToColorSpace(position); jointColorPoints[joint.Key] = new Point(colorSpacePoint.X, colorSpacePoint.Y); if (joint.Value.TrackingState == TrackingState.Tracked) { DrawJoint(new Point(colorSpacePoint.X, colorSpacePoint.Y), new SolidColorBrush(Colors.Purple)); } if (joint.Value.TrackingState == TrackingState.Inferred) { DrawJoint(new Point(colorSpacePoint.X, colorSpacePoint.Y), new SolidColorBrush(Colors.LightGray)); } foreach (var bone in bones) { DrawBone(body.Joints, jointColorPoints, bone.Item1, bone.Item2, colorBrush); } DrawClippedEdges(body); DrawHandStates(body.HandRightState, jointColorPoints[JointType.HandRight]); DrawHandStates(body.HandLeftState, jointColorPoints[JointType.HandLeft]); } } } }