kinect/codes/Azure-Kinect-Samples/body-tracking-samples/sample_unity_bodytracking/Assets/Scripts/SkeletalTrackingProvider.cs

130 lines
5.9 KiB
C#
Raw Normal View History

2024-03-06 18:05:53 +00:00
using Microsoft.Azure.Kinect.BodyTracking;
using Microsoft.Azure.Kinect.Sensor;
using System;
using System.IO;
using System.Runtime.InteropServices;
using System.Threading;
using UnityEngine;
public class SkeletalTrackingProvider : BackgroundDataProvider
{
bool readFirstFrame = false;
TimeSpan initialTimestamp;
public SkeletalTrackingProvider(int id) : base(id)
{
Debug.Log("in the skeleton provider constructor");
}
System.Runtime.Serialization.Formatters.Binary.BinaryFormatter binaryFormatter { get; set; } = new System.Runtime.Serialization.Formatters.Binary.BinaryFormatter();
public Stream RawDataLoggingFile = null;
protected override void RunBackgroundThreadAsync(int id, CancellationToken token)
{
try
{
UnityEngine.Debug.Log("Starting body tracker background thread.");
// Buffer allocations.
BackgroundData currentFrameData = new BackgroundData();
// Open device.
using (Device device = Device.Open(id))
{
device.StartCameras(new DeviceConfiguration()
{
CameraFPS = FPS.FPS30,
ColorResolution = ColorResolution.Off,
DepthMode = DepthMode.NFOV_Unbinned,
WiredSyncMode = WiredSyncMode.Standalone,
});
UnityEngine.Debug.Log("Open K4A device successful. id " + id + "sn:" + device.SerialNum);
var deviceCalibration = device.GetCalibration();
using (Tracker tracker = Tracker.Create(deviceCalibration, new TrackerConfiguration() { ProcessingMode = TrackerProcessingMode.Gpu, SensorOrientation = SensorOrientation.Default }))
{
UnityEngine.Debug.Log("Body tracker created.");
while (!token.IsCancellationRequested)
{
using (Capture sensorCapture = device.GetCapture())
{
// Queue latest frame from the sensor.
tracker.EnqueueCapture(sensorCapture);
}
// Try getting latest tracker frame.
using (Frame frame = tracker.PopResult(TimeSpan.Zero, throwOnTimeout: false))
{
if (frame == null)
{
UnityEngine.Debug.Log("Pop result from tracker timeout!");
}
else
{
IsRunning = true;
// Get number of bodies in the current frame.
currentFrameData.NumOfBodies = frame.NumberOfBodies;
// Copy bodies.
for (uint i = 0; i < currentFrameData.NumOfBodies; i++)
{
currentFrameData.Bodies[i].CopyFromBodyTrackingSdk(frame.GetBody(i), deviceCalibration);
}
// Store depth image.
Capture bodyFrameCapture = frame.Capture;
Image depthImage = bodyFrameCapture.Depth;
if (!readFirstFrame)
{
readFirstFrame = true;
initialTimestamp = depthImage.DeviceTimestamp;
}
currentFrameData.TimestampInMs = (float)(depthImage.DeviceTimestamp - initialTimestamp).TotalMilliseconds;
currentFrameData.DepthImageWidth = depthImage.WidthPixels;
currentFrameData.DepthImageHeight = depthImage.HeightPixels;
// Read image data from the SDK.
var depthFrame = MemoryMarshal.Cast<byte, ushort>(depthImage.Memory.Span);
// Repack data and store image data.
int byteCounter = 0;
currentFrameData.DepthImageSize = currentFrameData.DepthImageWidth * currentFrameData.DepthImageHeight * 3;
for (int it = currentFrameData.DepthImageWidth * currentFrameData.DepthImageHeight - 1; it > 0; it--)
{
byte b = (byte)(depthFrame[it] / (ConfigLoader.Instance.Configs.SkeletalTracking.MaximumDisplayedDepthInMillimeters) * 255);
currentFrameData.DepthImage[byteCounter++] = b;
currentFrameData.DepthImage[byteCounter++] = b;
currentFrameData.DepthImage[byteCounter++] = b;
}
if (RawDataLoggingFile != null && RawDataLoggingFile.CanWrite)
{
binaryFormatter.Serialize(RawDataLoggingFile, currentFrameData);
}
// Update data variable that is being read in the UI thread.
SetCurrentFrameData(ref currentFrameData);
}
}
}
Debug.Log("dispose of tracker now!!!!!");
tracker.Dispose();
}
device.Dispose();
}
if (RawDataLoggingFile != null)
{
RawDataLoggingFile.Close();
}
}
catch (Exception e)
{
Debug.Log($"catching exception for background thread {e.Message}");
token.ThrowIfCancellationRequested();
}
}
}