/// <summary> /// Converts coordinate system to 3D Matrix. /// </summary> /// <param name="cs">Coordinate system to convert.</param> /// <returns>The converted matrix.</returns> public static Matrix3D GetMatrix3D(this euclidean.CoordinateSystem cs) { return(new Matrix3D( cs.Values[0], cs.Values[1], cs.Values[2], cs.Values[3], cs.Values[4], cs.Values[5], cs.Values[6], cs.Values[7], cs.Values[8], cs.Values[9], cs.Values[10], cs.Values[11], cs.Values[12], cs.Values[13], cs.Values[14], cs.Values[15])); }
private void UpdateDepthFramePoints(DepthImage depthImage, ICameraIntrinsics intrinsics, euclidean.CoordinateSystem position) { // Handle null cases by clearing the depthFramePoints if (depthImage == null || intrinsics == null || position == null) { if (this.rawDepth.Length > 0) { this.rawDepth = new int[0]; this.depthFramePoints = new System.Windows.Media.Media3D.Point3D[0]; } return; } if (this.depthFramePoints?.Length != (this.depthImage.Width * this.depthImage.Height)) { this.rawDepth = new int[this.depthImage.Width * this.depthImage.Height]; this.depthFramePoints = new System.Windows.Media.Media3D.Point3D[this.depthImage.Width * this.depthImage.Height]; } int width = depthImage.Width; int height = depthImage.Height; int cx = width / 2; int cy = height / 2; double scale = 0.001; unsafe { ushort *depthFrame = (ushort *)((byte *)depthImage.ImageData.ToPointer()); Parallel.For(0, height, iy => { for (int ix = 0; ix < width; ix++) { int i = (iy * width) + ix; this.rawDepth[i] = depthFrame[i]; if (this.rawDepth[i] == 0) { this.rawDepth[i] = -1; this.depthFramePoints[i] = default; } else { var other = intrinsics.ToCameraSpace(new euclidean.Point2D(ix, iy), this.rawDepth[i] * scale, true); this.depthFramePoints[i] = other.TransformBy(position).ToPoint3D(); } } }); } }