/// <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();
}
}
});
}
}