You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
1846 lines
79 KiB
1846 lines
79 KiB
#if (UNITY_STANDALONE_WIN)
|
|
using UnityEngine;
|
|
using System.Collections;
|
|
using Windows.Kinect;
|
|
using System.Runtime.InteropServices;
|
|
//using Microsoft.Kinect.Face;
|
|
using System.Collections.Generic;
|
|
using System;
|
|
|
|
namespace com.rfilkov.kinect
|
|
{
|
|
/// <summary>
|
|
/// Kinect2Interface is sensor-interface to the Kinect-v2 sensors.
|
|
/// </summary>
|
|
public class Kinect2Interface : DepthSensorBase
|
|
{
|
|
// change this to false, if you aren't using Kinect-v2 only and want KM to check for available sensors
|
|
public static bool sensorAlwaysAvailable = true;
|
|
|
|
[Tooltip("Whether the detected users face backwards or not.")]
|
|
public bool usersFaceBackwards = false;
|
|
|
|
public KinectSensor kinectSensor;
|
|
public CoordinateMapper coordMapper;
|
|
|
|
private BodyFrameReader bodyFrameReader;
|
|
private BodyIndexFrameReader bodyIndexFrameReader;
|
|
private ColorFrameReader colorFrameReader;
|
|
private DepthFrameReader depthFrameReader;
|
|
private InfraredFrameReader infraredFrameReader;
|
|
|
|
private MultiSourceFrameReader multiSourceFrameReader;
|
|
private MultiSourceFrame multiSourceFrame;
|
|
|
|
private ColorFrame msColorFrame = null;
|
|
private DepthFrame msDepthFrame = null;
|
|
private InfraredFrame msInfraredFrame = null;
|
|
private BodyFrame msBodyFrame = null;
|
|
private BodyIndexFrame msBodyIndexFrame = null;
|
|
|
|
|
|
private int kinectBodyCount = 0;
|
|
private int kinectJointCount = 0;
|
|
private Body[] kinectBodyData;
|
|
|
|
private bool floorPlaneDetected = false;
|
|
private Windows.Kinect.Vector4 vFloorPlane;
|
|
|
|
|
|
// depth sensor settings
|
|
[System.Serializable]
|
|
public class K2SensorSettings : DepthSensorBase.BaseSensorSettings
|
|
{
|
|
public bool usersFaceBackwards;
|
|
}
|
|
|
|
|
|
public override KinectInterop.DepthSensorPlatform GetSensorPlatform()
|
|
{
|
|
return KinectInterop.DepthSensorPlatform.KinectV2;
|
|
}
|
|
|
|
|
|
public override System.Type GetSensorSettingsType()
|
|
{
|
|
return typeof(K2SensorSettings);
|
|
}
|
|
|
|
|
|
public override BaseSensorSettings GetSensorSettings(BaseSensorSettings settings)
|
|
{
|
|
if (settings == null)
|
|
{
|
|
settings = new K2SensorSettings();
|
|
}
|
|
|
|
K2SensorSettings extSettings = (K2SensorSettings)base.GetSensorSettings(settings);
|
|
extSettings.usersFaceBackwards = usersFaceBackwards;
|
|
|
|
return settings;
|
|
}
|
|
|
|
public override void SetSensorSettings(BaseSensorSettings settings)
|
|
{
|
|
if (settings == null)
|
|
return;
|
|
|
|
base.SetSensorSettings(settings);
|
|
|
|
K2SensorSettings extSettings = (K2SensorSettings)settings;
|
|
usersFaceBackwards = extSettings.usersFaceBackwards;
|
|
}
|
|
|
|
public override List<KinectInterop.SensorDeviceInfo> GetAvailableSensors()
|
|
{
|
|
List<KinectInterop.SensorDeviceInfo> alSensorInfo = new List<KinectInterop.SensorDeviceInfo>();
|
|
KinectInterop.SensorDeviceInfo sensorInfo = new KinectInterop.SensorDeviceInfo();
|
|
|
|
KinectSensor sensor = KinectSensor.GetDefault();
|
|
if (sensor != null)
|
|
{
|
|
if (sensorAlwaysAvailable)
|
|
{
|
|
sensorInfo.sensorId = "KinectV2";
|
|
sensorInfo.sensorName = "Kinect-v2 Sensor";
|
|
sensorInfo.sensorCaps = KinectInterop.FrameSource.TypeAll; // & ~KinectInterop.FrameSource.TypePose;
|
|
if (consoleLogMessages)
|
|
Debug.Log(string.Format(" D{0}: {1}, id: {2}", 0, sensorInfo.sensorName, sensorInfo.sensorId));
|
|
|
|
alSensorInfo.Add(sensorInfo);
|
|
|
|
return alSensorInfo;
|
|
}
|
|
else
|
|
{
|
|
// check for available sensor
|
|
if (!sensor.IsOpen)
|
|
{
|
|
sensor.Open();
|
|
}
|
|
|
|
float fWaitTime = Time.realtimeSinceStartup + 3f;
|
|
while (!sensor.IsAvailable && Time.realtimeSinceStartup < fWaitTime)
|
|
{
|
|
// wait for availability
|
|
}
|
|
|
|
if (sensor.IsAvailable)
|
|
{
|
|
sensorInfo.sensorId = "KinectV2";
|
|
sensorInfo.sensorName = "Kinect-v2 Sensor";
|
|
sensorInfo.sensorCaps = KinectInterop.FrameSource.TypeAll; // & ~KinectInterop.FrameSource.TypePose;
|
|
if (consoleLogMessages)
|
|
Debug.Log(string.Format(" D{0}: {1}, id: {2}", 0, sensorInfo.sensorName, sensorInfo.sensorId));
|
|
|
|
alSensorInfo.Add(sensorInfo);
|
|
}
|
|
|
|
if (sensor.IsOpen)
|
|
{
|
|
sensor.Close();
|
|
}
|
|
|
|
fWaitTime = Time.realtimeSinceStartup + 3f;
|
|
while (sensor.IsOpen && Time.realtimeSinceStartup < fWaitTime)
|
|
{
|
|
// wait for sensor to close
|
|
}
|
|
}
|
|
|
|
sensor = null;
|
|
}
|
|
|
|
//if(alSensorInfo.Count == 0)
|
|
//{
|
|
// Debug.Log(" No sensor devices found.");
|
|
//}
|
|
|
|
return alSensorInfo;
|
|
}
|
|
|
|
public override KinectInterop.SensorData OpenSensor(KinectManager kinectManager, KinectInterop.FrameSource dwFlags, bool bSyncDepthAndColor, bool bSyncBodyAndDepth)
|
|
{
|
|
// save initial parameters
|
|
base.OpenSensor(kinectManager, dwFlags, bSyncDepthAndColor, bSyncBodyAndDepth);
|
|
|
|
if (deviceStreamingMode == KinectInterop.DeviceStreamingMode.PlayRecording)
|
|
{
|
|
Debug.LogWarning("Please use Kinect Studio v2.0 to play the sensor data recording!");
|
|
//return null;
|
|
}
|
|
|
|
List<KinectInterop.SensorDeviceInfo> alSensors = GetAvailableSensors();
|
|
if (deviceIndex >= alSensors.Count)
|
|
{
|
|
Debug.LogError(" D" + deviceIndex + " is not available. You can set the device index to -1, to disable it.");
|
|
return null;
|
|
}
|
|
|
|
// try to get reference to the default sensor
|
|
sensorDeviceId = alSensors[deviceIndex].sensorId;
|
|
sensorPlatform = KinectInterop.DepthSensorPlatform.KinectV2;
|
|
|
|
kinectSensor = KinectSensor.GetDefault();
|
|
if (kinectSensor == null)
|
|
{
|
|
Debug.LogError("Kinect-v2 sensor not found!");
|
|
return null;
|
|
}
|
|
|
|
KinectInterop.SensorData sensorData = new KinectInterop.SensorData();
|
|
sensorData.sensorIntPlatform = sensorPlatform;
|
|
|
|
sensorData.sensorId = alSensors[deviceIndex].sensorId;
|
|
sensorData.sensorName = alSensors[deviceIndex].sensorName;
|
|
sensorData.sensorCaps = alSensors[deviceIndex].sensorCaps;
|
|
|
|
// get reference to the coordinate mapper
|
|
coordMapper = kinectSensor.CoordinateMapper;
|
|
|
|
// flip color & depth image vertically
|
|
sensorData.colorImageScale = new Vector3(1f, -1f, 1f);
|
|
sensorData.depthImageScale = new Vector3(1f, -1f, 1f);
|
|
sensorData.infraredImageScale = new Vector3(1f, -1f, 1f);
|
|
sensorData.sensorSpaceScale = new Vector3(1f, 1f, 1f);
|
|
sensorData.unitToMeterFactor = 1f;
|
|
|
|
// depth camera offset & matrix z-flip
|
|
sensorRotOffset = Vector3.zero; // if for instance the depth camera is tilted downwards
|
|
sensorRotFlipZ = false;
|
|
sensorRotIgnoreY = false;
|
|
|
|
// color
|
|
var frameDesc = kinectSensor.ColorFrameSource.CreateFrameDescription(ColorImageFormat.Rgba);
|
|
sensorData.colorImageWidth = frameDesc.Width;
|
|
sensorData.colorImageHeight = frameDesc.Height;
|
|
|
|
sensorData.colorImageFormat = TextureFormat.RGBA32;
|
|
sensorData.colorImageStride = 4; // 4 bytes per pixel
|
|
|
|
if ((dwFlags & KinectInterop.FrameSource.TypeColor) != 0)
|
|
{
|
|
if (!isSyncDepthAndColor)
|
|
colorFrameReader = kinectSensor.ColorFrameSource.OpenReader();
|
|
|
|
rawColorImage = new byte[frameDesc.LengthInPixels * frameDesc.BytesPerPixel];
|
|
|
|
sensorData.colorImageTexture = new Texture2D(sensorData.colorImageWidth, sensorData.colorImageHeight, TextureFormat.RGBA32, false);
|
|
sensorData.colorImageTexture.wrapMode = TextureWrapMode.Clamp;
|
|
sensorData.colorImageTexture.filterMode = FilterMode.Point;
|
|
}
|
|
|
|
// depth
|
|
sensorData.depthImageWidth = kinectSensor.DepthFrameSource.FrameDescription.Width;
|
|
sensorData.depthImageHeight = kinectSensor.DepthFrameSource.FrameDescription.Height;
|
|
|
|
if ((dwFlags & KinectInterop.FrameSource.TypeDepth) != 0)
|
|
{
|
|
if (!isSyncDepthAndColor)
|
|
depthFrameReader = kinectSensor.DepthFrameSource.OpenReader();
|
|
|
|
rawDepthImage = new ushort[kinectSensor.DepthFrameSource.FrameDescription.LengthInPixels];
|
|
sensorData.depthImage = new ushort[kinectSensor.DepthFrameSource.FrameDescription.LengthInPixels];
|
|
}
|
|
|
|
// infrared
|
|
if ((dwFlags & KinectInterop.FrameSource.TypeInfrared) != 0)
|
|
{
|
|
if (!isSyncDepthAndColor)
|
|
infraredFrameReader = kinectSensor.InfraredFrameSource.OpenReader();
|
|
|
|
rawInfraredImage = new ushort[kinectSensor.InfraredFrameSource.FrameDescription.LengthInPixels];
|
|
sensorData.infraredImage = new ushort[kinectSensor.InfraredFrameSource.FrameDescription.LengthInPixels];
|
|
|
|
minInfraredValue = 0f;
|
|
maxInfraredValue = 10000f;
|
|
}
|
|
|
|
if ((dwFlags & (KinectInterop.FrameSource.TypeBody | KinectInterop.FrameSource.TypeBodyIndex)) != 0)
|
|
{
|
|
if ((dwFlags & KinectInterop.FrameSource.TypeBodyIndex) != 0)
|
|
{
|
|
if (!(isSyncDepthAndColor && isSyncBodyAndDepth))
|
|
bodyIndexFrameReader = kinectSensor.BodyIndexFrameSource.OpenReader();
|
|
|
|
//rawBodyIndexImage = new byte[kinectSensor.BodyIndexFrameSource.FrameDescription.LengthInPixels]; // created by InitBodyTracking()
|
|
}
|
|
|
|
if ((dwFlags & KinectInterop.FrameSource.TypeBody) != 0)
|
|
{
|
|
if (!(isSyncDepthAndColor && isSyncBodyAndDepth))
|
|
bodyFrameReader = kinectSensor.BodyFrameSource.OpenReader();
|
|
|
|
kinectBodyCount = 6;
|
|
kinectJointCount = 25;
|
|
kinectBodyData = new Body[kinectBodyCount];
|
|
}
|
|
|
|
// init body tracking data
|
|
InitBodyTracking(dwFlags, sensorData);
|
|
}
|
|
|
|
// don't get all frames
|
|
getAllSensorFrames = false;
|
|
|
|
//if(!kinectSensor.IsOpen)
|
|
{
|
|
//Debug.Log("Opening sensor, available: " + kinectSensor.IsAvailable);
|
|
kinectSensor.Open();
|
|
}
|
|
|
|
float fWaitTime = Time.realtimeSinceStartup + 3f;
|
|
while (!kinectSensor.IsAvailable && Time.realtimeSinceStartup < fWaitTime)
|
|
{
|
|
// wait for sensor to be available
|
|
}
|
|
|
|
//fWaitTime = Time.realtimeSinceStartup + 3f;
|
|
while (!kinectSensor.IsOpen && Time.realtimeSinceStartup < fWaitTime)
|
|
{
|
|
// wait for sensor to open
|
|
}
|
|
|
|
if (consoleLogMessages)
|
|
Debug.Log("D" + deviceIndex + " K2-sensor " + (kinectSensor.IsOpen ? "opened" : "closed") + ", available: " + kinectSensor.IsAvailable);
|
|
|
|
if (isSyncDepthAndColor && dwFlags != KinectInterop.FrameSource.TypeNone && kinectSensor.IsOpen)
|
|
{
|
|
multiSourceFrameReader = kinectSensor.OpenMultiSourceFrameReader((FrameSourceTypes)((int)dwFlags & 0x3F));
|
|
}
|
|
|
|
//if (deviceMode == KinectInterop.DepthSensorMode.CreateRecording)
|
|
//{
|
|
// Debug.LogWarning("Please use Kinect Studio v2.0 to save sensor data recordings.");
|
|
//}
|
|
|
|
return sensorData;
|
|
}
|
|
|
|
public override void CloseSensor(KinectInterop.SensorData sensorData)
|
|
{
|
|
base.CloseSensor(sensorData);
|
|
|
|
if (coordMapper != null)
|
|
{
|
|
coordMapper = null;
|
|
}
|
|
|
|
if (colorFrameReader != null)
|
|
{
|
|
colorFrameReader.Dispose();
|
|
colorFrameReader = null;
|
|
}
|
|
|
|
if (depthFrameReader != null)
|
|
{
|
|
depthFrameReader.Dispose();
|
|
depthFrameReader = null;
|
|
}
|
|
|
|
if (infraredFrameReader != null)
|
|
{
|
|
infraredFrameReader.Dispose();
|
|
infraredFrameReader = null;
|
|
}
|
|
|
|
if (bodyFrameReader != null)
|
|
{
|
|
bodyFrameReader.Dispose();
|
|
bodyFrameReader = null;
|
|
}
|
|
|
|
if (bodyIndexFrameReader != null)
|
|
{
|
|
bodyIndexFrameReader.Dispose();
|
|
bodyIndexFrameReader = null;
|
|
}
|
|
|
|
if (multiSourceFrameReader != null)
|
|
{
|
|
multiSourceFrameReader.Dispose();
|
|
multiSourceFrameReader = null;
|
|
}
|
|
|
|
if (kinectSensor != null)
|
|
{
|
|
//if (kinectSensor.IsOpen)
|
|
{
|
|
//Debug.Log("Closing sensor, available: " + kinectSensor.IsAvailable);
|
|
kinectSensor.Close();
|
|
}
|
|
|
|
float fWaitTime = Time.realtimeSinceStartup + 3f;
|
|
while (kinectSensor.IsOpen && Time.realtimeSinceStartup < fWaitTime)
|
|
{
|
|
// wait for sensor to close
|
|
}
|
|
|
|
if (consoleLogMessages)
|
|
Debug.Log("D" + deviceIndex + " K2-sensor " + (kinectSensor.IsOpen ? "opened" : "closed") + ", available: " + kinectSensor.IsAvailable);
|
|
|
|
kinectSensor = null;
|
|
}
|
|
}
|
|
|
|
public override void PollSensorFrames(KinectInterop.SensorData sensorData)
|
|
{
|
|
// check for single-source or multi-source frames
|
|
if (multiSourceFrameReader == null)
|
|
{
|
|
// single source - check for color frame
|
|
if (colorFrameReader != null)
|
|
{
|
|
msColorFrame = colorFrameReader.AcquireLatestFrame();
|
|
}
|
|
|
|
// check for depth frame
|
|
if (depthFrameReader != null)
|
|
{
|
|
msDepthFrame = depthFrameReader.AcquireLatestFrame();
|
|
}
|
|
|
|
// check for IR frame
|
|
if (infraredFrameReader != null)
|
|
{
|
|
msInfraredFrame = infraredFrameReader.AcquireLatestFrame();
|
|
}
|
|
|
|
// check for body index frame
|
|
if (bodyIndexFrameReader != null)
|
|
{
|
|
msBodyIndexFrame = bodyIndexFrameReader.AcquireLatestFrame();
|
|
}
|
|
|
|
// check for body frame
|
|
if (bodyFrameReader != null)
|
|
{
|
|
msBodyFrame = bodyFrameReader.AcquireLatestFrame();
|
|
}
|
|
|
|
// process currently read sensor frames
|
|
ProcessSensorFrames(sensorData);
|
|
}
|
|
else
|
|
{
|
|
// multi-source frames
|
|
multiSourceFrame = multiSourceFrameReader.AcquireLatestFrame();
|
|
|
|
if (multiSourceFrame != null)
|
|
{
|
|
// try to get all frames at once
|
|
msColorFrame = (frameSourceFlags & KinectInterop.FrameSource.TypeColor) != 0 ? multiSourceFrame.ColorFrameReference.AcquireFrame() : null;
|
|
msDepthFrame = (frameSourceFlags & KinectInterop.FrameSource.TypeDepth) != 0 ? multiSourceFrame.DepthFrameReference.AcquireFrame() : null;
|
|
msInfraredFrame = (frameSourceFlags & KinectInterop.FrameSource.TypeInfrared) != 0 ? multiSourceFrame.InfraredFrameReference.AcquireFrame() : null;
|
|
msBodyFrame = (frameSourceFlags & KinectInterop.FrameSource.TypeBody) != 0 ? multiSourceFrame.BodyFrameReference.AcquireFrame() : null;
|
|
msBodyIndexFrame = (frameSourceFlags & KinectInterop.FrameSource.TypeBodyIndex) != 0 ? multiSourceFrame.BodyIndexFrameReference.AcquireFrame() : null;
|
|
|
|
bool bAllSet =
|
|
((frameSourceFlags & KinectInterop.FrameSource.TypeColor) == 0 || msColorFrame != null) &&
|
|
((frameSourceFlags & KinectInterop.FrameSource.TypeDepth) == 0 || msDepthFrame != null) &&
|
|
((frameSourceFlags & KinectInterop.FrameSource.TypeInfrared) == 0 || msInfraredFrame != null);
|
|
|
|
if(isSyncBodyAndDepth)
|
|
{
|
|
bAllSet &= ((frameSourceFlags & KinectInterop.FrameSource.TypeBody) == 0 || msBodyFrame != null) &&
|
|
((frameSourceFlags & KinectInterop.FrameSource.TypeBodyIndex) == 0 || msBodyIndexFrame != null);
|
|
}
|
|
|
|
if (bAllSet)
|
|
{
|
|
// process currently read sensor frames
|
|
ProcessSensorFrames(sensorData);
|
|
}
|
|
|
|
//release all frames
|
|
if (msColorFrame != null)
|
|
{
|
|
msColorFrame.Dispose();
|
|
msColorFrame = null;
|
|
}
|
|
|
|
if (msDepthFrame != null)
|
|
{
|
|
msDepthFrame.Dispose();
|
|
msDepthFrame = null;
|
|
}
|
|
|
|
if (msInfraredFrame != null)
|
|
{
|
|
msInfraredFrame.Dispose();
|
|
msInfraredFrame = null;
|
|
}
|
|
|
|
if (msBodyFrame != null)
|
|
{
|
|
msBodyFrame.Dispose();
|
|
msBodyFrame = null;
|
|
}
|
|
|
|
if (msBodyIndexFrame != null)
|
|
{
|
|
msBodyIndexFrame.Dispose();
|
|
msBodyIndexFrame = null;
|
|
}
|
|
|
|
if (multiSourceFrame != null)
|
|
{
|
|
multiSourceFrame = null;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// processes the currently read sensor frames
|
|
// todo: provide thread sync
|
|
private void ProcessSensorFrames(KinectInterop.SensorData sensorData)
|
|
{
|
|
// depth frame
|
|
ulong depthFrameTime = msDepthFrame != null ? (ulong)msDepthFrame.RelativeTime.Ticks : 0;
|
|
|
|
if (msDepthFrame != null)
|
|
{
|
|
if (getAllSensorFrames || rawDepthTimestamp == sensorData.lastDepthFrameTime)
|
|
{
|
|
lock (depthFrameLock)
|
|
{
|
|
var pDepthData = GCHandle.Alloc(rawDepthImage, GCHandleType.Pinned);
|
|
msDepthFrame.CopyFrameDataToIntPtr(pDepthData.AddrOfPinnedObject(), (uint)rawDepthImage.Length * sizeof(ushort));
|
|
pDepthData.Free();
|
|
|
|
rawDepthTimestamp = (ulong)msDepthFrame.RelativeTime.Ticks;
|
|
//Debug.Log("RawDepthTimestamp: " + rawDepthTimestamp);
|
|
}
|
|
}
|
|
|
|
msDepthFrame.Dispose();
|
|
msDepthFrame = null;
|
|
}
|
|
|
|
// color frame
|
|
if (msColorFrame != null)
|
|
{
|
|
if(getAllSensorFrames || rawColorTimestamp == sensorData.lastColorFrameTime)
|
|
{
|
|
lock (colorFrameLock)
|
|
{
|
|
var pColorData = GCHandle.Alloc(rawColorImage, GCHandleType.Pinned);
|
|
msColorFrame.CopyConvertedFrameDataToIntPtr(pColorData.AddrOfPinnedObject(), (uint)rawColorImage.Length, ColorImageFormat.Rgba);
|
|
pColorData.Free();
|
|
|
|
rawColorTimestamp = isSyncDepthAndColor && depthFrameTime != 0 ? depthFrameTime : (ulong)msColorFrame.RelativeTime.Ticks;
|
|
//Debug.Log("RawColorTimestamp: " + rawColorTimestamp);
|
|
}
|
|
}
|
|
|
|
msColorFrame.Dispose();
|
|
msColorFrame = null;
|
|
}
|
|
|
|
// infrared frame
|
|
if (msInfraredFrame != null)
|
|
{
|
|
if(getAllSensorFrames || rawInfraredTimestamp == sensorData.lastInfraredFrameTime)
|
|
{
|
|
lock (infraredFrameLock)
|
|
{
|
|
var pInfraredData = GCHandle.Alloc(rawInfraredImage, GCHandleType.Pinned);
|
|
msInfraredFrame.CopyFrameDataToIntPtr(pInfraredData.AddrOfPinnedObject(), (uint)rawInfraredImage.Length * sizeof(ushort));
|
|
pInfraredData.Free();
|
|
|
|
rawInfraredTimestamp = isSyncDepthAndColor && depthFrameTime != 0 ? depthFrameTime : (ulong)msInfraredFrame.RelativeTime.Ticks;
|
|
//Debug.Log("RawInfraredTimestamp: " + rawInfraredTimestamp);
|
|
}
|
|
}
|
|
|
|
msInfraredFrame.Dispose();
|
|
msInfraredFrame = null;
|
|
}
|
|
|
|
// body index frame
|
|
//bool bProcessBodyFrame = (getAllSensorFrames || rawBodyTimestamp == sensorData.lastBodyFrameTime);
|
|
|
|
if (msBodyIndexFrame != null)
|
|
{
|
|
//if(bProcessBodyFrame)
|
|
if (getAllSensorFrames || rawBodyIndexTimestamp == sensorData.lastBodyIndexFrameTime)
|
|
{
|
|
lock (bodyTrackerLock)
|
|
{
|
|
var pBodyIndexData = GCHandle.Alloc(rawBodyIndexImage, GCHandleType.Pinned);
|
|
msBodyIndexFrame.CopyFrameDataToIntPtr(pBodyIndexData.AddrOfPinnedObject(), (uint)rawBodyIndexImage.Length);
|
|
pBodyIndexData.Free();
|
|
|
|
rawBodyIndexTimestamp = isSyncBodyAndDepth && depthFrameTime != 0 ? depthFrameTime : (ulong)msBodyIndexFrame.RelativeTime.Ticks;
|
|
//Debug.Log("RawBodyIndexTimestamp: " + rawBodyIndexTimestamp);
|
|
}
|
|
}
|
|
|
|
msBodyIndexFrame.Dispose();
|
|
msBodyIndexFrame = null;
|
|
}
|
|
|
|
// body frame
|
|
if (msBodyFrame != null)
|
|
{
|
|
//if (bProcessBodyFrame)
|
|
if(getAllSensorFrames || rawBodyTimestamp == sensorData.lastBodyFrameTime)
|
|
{
|
|
lock (bodyTrackerLock)
|
|
{
|
|
ProcessBodyFrame(msBodyFrame, depthFrameTime, sensorData);
|
|
}
|
|
|
|
if (floorPlaneDetected && ((frameSourceFlags & KinectInterop.FrameSource.TypePose) != 0))
|
|
{
|
|
lock (poseFrameLock)
|
|
{
|
|
// update the sensor pose
|
|
if(vFloorPlane.X != 0f || vFloorPlane.Y != 0f || vFloorPlane.Z != 0f)
|
|
{
|
|
Vector3 vFloorNormal = new Vector3(vFloorPlane.X, vFloorPlane.Y, vFloorPlane.Z);
|
|
rawPoseRotation = Quaternion.FromToRotation(vFloorNormal, Vector3.up);
|
|
|
|
if (vFloorPlane.W != 0f)
|
|
{
|
|
rawPosePosition = new Vector3(0f, vFloorPlane.W, 0f) - initialPosePosition;
|
|
}
|
|
|
|
rawPoseTimestamp = rawBodyTimestamp;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
msBodyFrame.Dispose();
|
|
msBodyFrame = null;
|
|
}
|
|
}
|
|
|
|
|
|
// processes the acquired body frame
|
|
private void ProcessBodyFrame(BodyFrame frame, ulong depthFrameTime, KinectInterop.SensorData sensorData)
|
|
{
|
|
frame.GetAndRefreshBodyData(kinectBodyData);
|
|
ulong bodyFrameTimestamp = (ulong)frame.RelativeTime.Ticks;
|
|
|
|
// get the floor plane
|
|
vFloorPlane = frame.FloorClipPlane;
|
|
floorPlaneDetected = true;
|
|
|
|
frame.Dispose();
|
|
frame = null;
|
|
|
|
// get sensor-to-world matrix
|
|
Matrix4x4 sensorToWorld = GetSensorToWorldMatrix();
|
|
float scaleX = sensorData.depthImageScale.x;
|
|
//float scaleY = sensorData.depthImageScale.y;
|
|
|
|
// create the needed slots
|
|
while (alTrackedBodies.Count < kinectBodyCount)
|
|
{
|
|
alTrackedBodies.Add(new KinectInterop.BodyData((int)KinectInterop.JointType.Count));
|
|
}
|
|
|
|
trackedBodiesCount = 0;
|
|
|
|
for (int i = 0; i < kinectBodyCount; i++)
|
|
{
|
|
Body body = kinectBodyData[i];
|
|
|
|
if (body == null)
|
|
continue;
|
|
|
|
KinectInterop.BodyData bodyData = alTrackedBodies[(int)trackedBodiesCount];
|
|
|
|
bodyData.liTrackingID = body.TrackingId;
|
|
bodyData.iBodyIndex = i;
|
|
|
|
bodyData.bIsTracked = body.IsTracked;
|
|
bodyData.bodyTimestamp = bodyFrameTimestamp;
|
|
|
|
if (!bodyData.bIsTracked)
|
|
continue;
|
|
|
|
// cache the body joints (following the advice of Brian Chasalow)
|
|
Dictionary<Windows.Kinect.JointType, Windows.Kinect.Joint> bodyJoints = body.Joints;
|
|
|
|
for (int jKJ = 0; jKJ < kinectJointCount; jKJ++)
|
|
{
|
|
Windows.Kinect.Joint joint = bodyJoints[(Windows.Kinect.JointType)jKJ];
|
|
|
|
int j = KinectJoint2JointType[jKJ];
|
|
|
|
if (j >= 0)
|
|
{
|
|
KinectInterop.JointData jointData = bodyData.joint[j];
|
|
|
|
jointData.trackingState = (KinectInterop.TrackingState)joint.TrackingState;
|
|
|
|
float jPosZ = (bIgnoreZCoordinates && j > 0) ? bodyData.joint[0].kinectPos.z : joint.Position.Z;
|
|
jointData.kinectPos = new Vector3(joint.Position.X, joint.Position.Y, joint.Position.Z);
|
|
jointData.position = sensorToWorld.MultiplyPoint3x4(new Vector3(joint.Position.X * scaleX, joint.Position.Y, jPosZ));
|
|
|
|
jointData.orientation = Quaternion.identity;
|
|
|
|
if (j == 0)
|
|
{
|
|
bodyData.kinectPos = jointData.kinectPos;
|
|
bodyData.position = jointData.position;
|
|
bodyData.orientation = jointData.orientation;
|
|
//floorPlaneDetected = true;
|
|
}
|
|
|
|
bodyData.joint[j] = jointData;
|
|
}
|
|
}
|
|
|
|
bodyJoints.Clear();
|
|
|
|
// hand states
|
|
bodyData.leftHandState = (KinectInterop.HandState)body.HandLeftState;
|
|
bodyData.rightHandState = (KinectInterop.HandState)body.HandRightState;
|
|
//bodyData.leftHandConfidence = (KinectInterop.TrackingConfidence)body.HandLeftConfidence;
|
|
//bodyData.rightHandConfidence = (KinectInterop.TrackingConfidence)body.HandRightConfidence;
|
|
|
|
if (usersFaceBackwards)
|
|
{
|
|
// switch the left and right joints
|
|
SwitchLeftRightJoints(ref bodyData);
|
|
}
|
|
|
|
// estimate additional joints
|
|
CalcBodySpecialJoints(ref bodyData);
|
|
|
|
// filter joint positions
|
|
if (jointPositionFilter != null)
|
|
{
|
|
jointPositionFilter.UpdateFilter(ref bodyData);
|
|
}
|
|
|
|
// calculate bone dirs
|
|
KinectInterop.CalcBodyJointDirs(ref bodyData);
|
|
|
|
// calculate joint orientations
|
|
CalcBodyJointOrients(ref bodyData);
|
|
|
|
// body orientation
|
|
bodyData.normalRotation = bodyData.joint[0].normalRotation;
|
|
bodyData.mirroredRotation = bodyData.joint[0].mirroredRotation;
|
|
|
|
alTrackedBodies[(int)trackedBodiesCount] = bodyData;
|
|
trackedBodiesCount++;
|
|
|
|
//Debug.Log(" (T)User ID: " + bodyData.liTrackingID + ", body: " + (trackedBodiesCount - 1) + ", bi: " + bodyData.iBodyIndex + ", pos: " + bodyData.joint[0].kinectPos + ", rot: " + bodyData.joint[0].normalRotation.eulerAngles);
|
|
}
|
|
|
|
// clean up user history
|
|
if (jointPositionFilter != null)
|
|
{
|
|
jointPositionFilter.CleanUpUserHistory();
|
|
}
|
|
|
|
rawBodyTimestamp = isSyncBodyAndDepth && depthFrameTime != 0 ? depthFrameTime : bodyFrameTimestamp;
|
|
//Debug.Log("RawBodyTimestamp: " + rawBodyTimestamp + ", Time: " + DateTime.Now.ToString("HH.mm.ss.fff"));
|
|
}
|
|
|
|
// switch the left and right body joints
|
|
private void SwitchLeftRightJoints(ref KinectInterop.BodyData bodyData)
|
|
{
|
|
//SwitchJointsData(ref bodyData, (int)KinectInterop.JointType.ClavicleLeft, (int)KinectInterop.JointType.ClavicleRight);
|
|
SwitchJointsData(ref bodyData, (int)KinectInterop.JointType.ShoulderLeft, (int)KinectInterop.JointType.ShoulderRight);
|
|
SwitchJointsData(ref bodyData, (int)KinectInterop.JointType.ElbowLeft, (int)KinectInterop.JointType.ElbowRight);
|
|
SwitchJointsData(ref bodyData, (int)KinectInterop.JointType.WristLeft, (int)KinectInterop.JointType.WristRight);
|
|
SwitchJointsData(ref bodyData, (int)KinectInterop.JointType.HandLeft, (int)KinectInterop.JointType.HandRight);
|
|
SwitchJointsData(ref bodyData, (int)KinectInterop.JointType.ThumbLeft, (int)KinectInterop.JointType.ThumbRight);
|
|
SwitchJointsData(ref bodyData, (int)KinectInterop.JointType.HandtipLeft, (int)KinectInterop.JointType.HandtipRight);
|
|
|
|
SwitchJointsData(ref bodyData, (int)KinectInterop.JointType.HipLeft, (int)KinectInterop.JointType.HipRight);
|
|
SwitchJointsData(ref bodyData, (int)KinectInterop.JointType.KneeLeft, (int)KinectInterop.JointType.KneeRight);
|
|
SwitchJointsData(ref bodyData, (int)KinectInterop.JointType.AnkleLeft, (int)KinectInterop.JointType.AnkleRight);
|
|
SwitchJointsData(ref bodyData, (int)KinectInterop.JointType.FootLeft, (int)KinectInterop.JointType.FootRight);
|
|
|
|
//SwitchJointsData(ref bodyData, (int)KinectInterop.JointType.EyeLeft, (int)KinectInterop.JointType.EyeRight);
|
|
//SwitchJointsData(ref bodyData, (int)KinectInterop.JointType.EarLeft, (int)KinectInterop.JointType.EarRight);
|
|
}
|
|
|
|
// switches the positional data of two joints
|
|
private void SwitchJointsData(ref KinectInterop.BodyData bodyData, int jointL, int jointR)
|
|
{
|
|
KinectInterop.TrackingState trackingStateL = bodyData.joint[jointL].trackingState;
|
|
Vector3 kinectPosL = bodyData.joint[jointL].kinectPos;
|
|
Vector3 positionL = bodyData.joint[jointL].position;
|
|
|
|
KinectInterop.TrackingState trackingStateR = bodyData.joint[jointR].trackingState;
|
|
Vector3 kinectPosR = bodyData.joint[jointR].kinectPos;
|
|
Vector3 positionR = bodyData.joint[jointR].position;
|
|
|
|
bodyData.joint[jointL].trackingState = trackingStateR;
|
|
bodyData.joint[jointL].kinectPos = kinectPosR;
|
|
bodyData.joint[jointL].position = positionR;
|
|
|
|
bodyData.joint[jointR].trackingState = trackingStateL;
|
|
bodyData.joint[jointR].kinectPos = kinectPosL;
|
|
bodyData.joint[jointR].position = positionL;
|
|
}
|
|
|
|
// estimates additional joints for the given body
|
|
protected override void CalcBodySpecialJoints(ref KinectInterop.BodyData bodyData)
|
|
{
|
|
// clavicle right
|
|
{
|
|
int l = (int)KinectInterop.JointType.ClavicleLeft;
|
|
int r = (int)KinectInterop.JointType.ClavicleRight;
|
|
|
|
KinectInterop.JointData jointData = bodyData.joint[r];
|
|
jointData.trackingState = bodyData.joint[l].trackingState;
|
|
jointData.orientation = bodyData.joint[l].orientation;
|
|
|
|
jointData.kinectPos = bodyData.joint[l].kinectPos;
|
|
jointData.position = bodyData.joint[l].position;
|
|
|
|
bodyData.joint[r] = jointData;
|
|
}
|
|
|
|
// spine naval
|
|
{
|
|
int p = (int)KinectInterop.JointType.Pelvis;
|
|
int sc = (int)KinectInterop.JointType.SpineChest;
|
|
int sn = (int)KinectInterop.JointType.SpineNaval;
|
|
|
|
KinectInterop.JointData jointData = bodyData.joint[sn];
|
|
jointData.trackingState = bodyData.joint[sc].trackingState;
|
|
jointData.orientation = bodyData.joint[sc].orientation;
|
|
|
|
Vector3 posChest = bodyData.joint[sc].kinectPos;
|
|
Vector3 posPelvis = bodyData.joint[p].kinectPos;
|
|
jointData.kinectPos = (posPelvis + posChest) * 0.5f;
|
|
|
|
posChest = bodyData.joint[sc].position;
|
|
posPelvis = bodyData.joint[p].position;
|
|
jointData.position = (posPelvis + posChest) * 0.5f;
|
|
|
|
bodyData.joint[sn] = jointData;
|
|
}
|
|
|
|
// ankle left
|
|
int knee = (int)KinectInterop.JointType.KneeLeft;
|
|
int ank = (int)KinectInterop.JointType.AnkleLeft;
|
|
int foot = (int)KinectInterop.JointType.FootLeft;
|
|
|
|
if (bodyData.joint[knee].trackingState != KinectInterop.TrackingState.NotTracked &&
|
|
bodyData.joint[ank].trackingState != KinectInterop.TrackingState.NotTracked &&
|
|
bodyData.joint[foot].trackingState != KinectInterop.TrackingState.NotTracked)
|
|
{
|
|
Vector3 vAnkDir = bodyData.joint[ank].kinectPos - bodyData.joint[knee].kinectPos;
|
|
Vector3 vFootDir = bodyData.joint[foot].kinectPos - bodyData.joint[ank].kinectPos;
|
|
|
|
Vector3 vFootProj = Vector3.Project(vFootDir, vAnkDir);
|
|
bodyData.joint[ank].kinectPos += vFootProj;
|
|
|
|
vAnkDir = bodyData.joint[ank].position - bodyData.joint[knee].position;
|
|
vFootDir = bodyData.joint[foot].position - bodyData.joint[ank].position;
|
|
|
|
vFootProj = Vector3.Project(vFootDir, vAnkDir);
|
|
bodyData.joint[ank].position += vFootProj;
|
|
}
|
|
|
|
// ankle right
|
|
knee = (int)KinectInterop.JointType.KneeRight;
|
|
ank = (int)KinectInterop.JointType.AnkleRight;
|
|
foot = (int)KinectInterop.JointType.FootRight;
|
|
|
|
if (bodyData.joint[knee].trackingState != KinectInterop.TrackingState.NotTracked &&
|
|
bodyData.joint[ank].trackingState != KinectInterop.TrackingState.NotTracked &&
|
|
bodyData.joint[foot].trackingState != KinectInterop.TrackingState.NotTracked)
|
|
{
|
|
Vector3 vAnkDir = bodyData.joint[ank].kinectPos - bodyData.joint[knee].kinectPos;
|
|
Vector3 vFootDir = bodyData.joint[foot].kinectPos - bodyData.joint[ank].kinectPos;
|
|
|
|
Vector3 vFootProj = Vector3.Project(vFootDir, vAnkDir);
|
|
bodyData.joint[ank].kinectPos += vFootProj;
|
|
|
|
vAnkDir = bodyData.joint[ank].position - bodyData.joint[knee].position;
|
|
vFootDir = bodyData.joint[foot].position - bodyData.joint[ank].position;
|
|
|
|
vFootProj = Vector3.Project(vFootDir, vAnkDir);
|
|
bodyData.joint[ank].position += vFootProj;
|
|
}
|
|
}
|
|
|
|
|
|
protected static readonly int[] KinectJoint2JointType =
|
|
{
|
|
(int)KinectInterop.JointType.Pelvis,
|
|
(int)KinectInterop.JointType.SpineChest,
|
|
(int)KinectInterop.JointType.Neck,
|
|
(int)KinectInterop.JointType.Head,
|
|
|
|
(int)KinectInterop.JointType.ShoulderLeft,
|
|
(int)KinectInterop.JointType.ElbowLeft,
|
|
(int)KinectInterop.JointType.WristLeft,
|
|
(int)KinectInterop.JointType.HandLeft,
|
|
|
|
(int)KinectInterop.JointType.ShoulderRight,
|
|
(int)KinectInterop.JointType.ElbowRight,
|
|
(int)KinectInterop.JointType.WristRight,
|
|
(int)KinectInterop.JointType.HandRight,
|
|
|
|
(int)KinectInterop.JointType.HipLeft,
|
|
(int)KinectInterop.JointType.KneeLeft,
|
|
(int)KinectInterop.JointType.AnkleLeft,
|
|
(int)KinectInterop.JointType.FootLeft,
|
|
|
|
(int)KinectInterop.JointType.HipRight,
|
|
(int)KinectInterop.JointType.KneeRight,
|
|
(int)KinectInterop.JointType.AnkleRight,
|
|
(int)KinectInterop.JointType.FootRight,
|
|
|
|
(int)KinectInterop.JointType.ClavicleLeft,
|
|
|
|
(int)KinectInterop.JointType.HandtipLeft,
|
|
(int)KinectInterop.JointType.ThumbLeft,
|
|
(int)KinectInterop.JointType.HandtipRight,
|
|
(int)KinectInterop.JointType.ThumbRight
|
|
};
|
|
|
|
|
|
public override bool UpdateSensorData(KinectInterop.SensorData sensorData, KinectManager kinectManager, bool isPlayMode)
|
|
{
|
|
base.UpdateSensorData(sensorData, kinectManager, isPlayMode);
|
|
|
|
if (sensorData.depthCamIntr == null && coordMapper != null)
|
|
{
|
|
lock (depthFrameLock)
|
|
{
|
|
// get depth camera intrinsics
|
|
CameraIntrinsics depthCamIntr = coordMapper.GetDepthCameraIntrinsics();
|
|
|
|
if (depthCamIntr.PrincipalPointX != 0f && depthCamIntr.PrincipalPointY != 0f)
|
|
{
|
|
GetDepthCameraIntrinsics(depthCamIntr, ref sensorData.depthCamIntr, sensorData.depthImageWidth, sensorData.depthImageHeight);
|
|
}
|
|
}
|
|
}
|
|
|
|
if (sensorData.colorCamIntr == null && coordMapper != null)
|
|
{
|
|
lock (colorFrameLock)
|
|
{
|
|
GetColorCameraIntrinsics(ref sensorData.colorCamIntr, sensorData.colorImageWidth, sensorData.colorImageHeight);
|
|
}
|
|
}
|
|
|
|
if((sensorData.depth2ColorExtr == null || sensorData.color2DepthExtr == null) &&
|
|
sensorData.depthCamIntr != null && sensorData.colorCamIntr != null)
|
|
{
|
|
GetDepth2ColorCameraExtrinsics(ref sensorData.depth2ColorExtr);
|
|
GetColor2DepthCameraExtrinsics(ref sensorData.color2DepthExtr);
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
// gets the depth camera intrinsics
|
|
private void GetDepthCameraIntrinsics(CameraIntrinsics camIntr, ref KinectInterop.CameraIntrinsics intr, int camWidth, int camHeight)
|
|
{
|
|
intr = new KinectInterop.CameraIntrinsics();
|
|
|
|
intr.cameraType = 0;
|
|
intr.width = camWidth;
|
|
intr.height = camHeight;
|
|
|
|
intr.ppx = camIntr.PrincipalPointX;
|
|
intr.ppy = camHeight - camIntr.PrincipalPointY;
|
|
|
|
intr.fx = camIntr.FocalLengthX;
|
|
intr.fy = camIntr.FocalLengthY;
|
|
|
|
intr.distCoeffs = new float[3];
|
|
intr.distCoeffs[0] = camIntr.RadialDistortionSecondOrder;
|
|
intr.distCoeffs[1] = camIntr.RadialDistortionFourthOrder;
|
|
intr.distCoeffs[2] = camIntr.RadialDistortionSixthOrder;
|
|
|
|
intr.distType = KinectInterop.DistortionType.BrownConrady;
|
|
|
|
EstimateFOV(intr);
|
|
}
|
|
|
|
|
|
// gets the color camera intrinsics
|
|
private void GetColorCameraIntrinsics(ref KinectInterop.CameraIntrinsics intr, int camWidth, int camHeight)
|
|
{
|
|
intr = new KinectInterop.CameraIntrinsics();
|
|
|
|
intr.cameraType = 1;
|
|
intr.width = camWidth;
|
|
intr.height = camHeight;
|
|
|
|
intr.ppx = 946.0374f;
|
|
intr.ppy = camHeight - 537.392f;
|
|
|
|
intr.fx = 1065.267f;
|
|
intr.fy = 1065.409f;
|
|
|
|
intr.distCoeffs = new float[3];
|
|
intr.distCoeffs[0] = 0.014655f;
|
|
intr.distCoeffs[1] = -0.000476f;
|
|
intr.distCoeffs[2] = 0f;
|
|
|
|
intr.distType = KinectInterop.DistortionType.BrownConrady;
|
|
|
|
EstimateFOV(intr);
|
|
}
|
|
|
|
|
|
// gets the depth to color camera extrinsics
|
|
private void GetDepth2ColorCameraExtrinsics(ref KinectInterop.CameraExtrinsics extr)
|
|
{
|
|
extr = new KinectInterop.CameraExtrinsics();
|
|
|
|
extr.rotation = new float[9];
|
|
|
|
extr.translation = new float[3];
|
|
}
|
|
|
|
|
|
// gets the color to depth camera extrinsics
|
|
private void GetColor2DepthCameraExtrinsics(ref KinectInterop.CameraExtrinsics extr)
|
|
{
|
|
extr = new KinectInterop.CameraExtrinsics();
|
|
|
|
extr.rotation = new float[9];
|
|
|
|
extr.translation = new float[3];
|
|
}
|
|
|
|
|
|
// unprojects plane point into the space
|
|
public override Vector3 UnprojectPoint(KinectInterop.CameraIntrinsics intr, Vector2 pixel, float depth)
|
|
{
|
|
float x = (pixel.x - intr.ppx) / intr.fx;
|
|
float y = (intr.ppy - pixel.y) / intr.fy;
|
|
|
|
// undistort
|
|
float r2 = x * x + y * y;
|
|
float f = 1 - intr.distCoeffs[0] * r2 - intr.distCoeffs[1] * r2 * r2 - intr.distCoeffs[2] * r2 * r2 * r2;
|
|
|
|
x = x * f * depth;
|
|
y = y * f * depth;
|
|
|
|
return new Vector3(x, y, depth);
|
|
}
|
|
|
|
|
|
// projects space point onto a plane
|
|
public override Vector2 ProjectPoint(KinectInterop.CameraIntrinsics intr, Vector3 point)
|
|
{
|
|
float x = point.x / point.z;
|
|
float y = point.y / point.z;
|
|
|
|
// distort
|
|
float r2 = x * x + y * y;
|
|
float f = 1f + intr.distCoeffs[0] * r2 + intr.distCoeffs[1] * r2 * r2 + intr.distCoeffs[2] * r2 * r2 * r2;
|
|
|
|
x = x * f * intr.fx + intr.ppx;
|
|
y = intr.ppy - y * f * intr.fy;
|
|
|
|
return new Vector2(x, y);
|
|
}
|
|
|
|
|
|
// transforms a point from one space to another
|
|
public override Vector3 TransformPoint(KinectInterop.CameraExtrinsics extr, Vector3 point)
|
|
{
|
|
float toPointX = extr.rotation[0] * point.x + extr.rotation[3] * point.y + extr.rotation[6] * point.z + extr.translation[0];
|
|
float toPointY = extr.rotation[1] * point.x + extr.rotation[4] * point.y + extr.rotation[7] * point.z + extr.translation[1];
|
|
float toPointZ = extr.rotation[2] * point.x + extr.rotation[5] * point.y + extr.rotation[8] * point.z + extr.translation[2];
|
|
|
|
return new Vector3(toPointX, toPointY, toPointZ);
|
|
}
|
|
|
|
|
|
public override void PollCoordTransformFrames(KinectInterop.SensorData sensorData)
|
|
{
|
|
if (lastDepthCoordFrameTime != rawDepthTimestamp)
|
|
{
|
|
lastDepthCoordFrameTime = rawDepthTimestamp;
|
|
|
|
//// depth2space frame
|
|
//if (depth2SpaceCoordFrame != null)
|
|
//{
|
|
// lock (depth2SpaceFrameLock)
|
|
// {
|
|
// MapDepthFrameToSpaceCoords(sensorData, ref depth2SpaceCoordFrame);
|
|
// lastDepth2SpaceFrameTime = lastDepthCoordFrameTime;
|
|
// }
|
|
//}
|
|
|
|
// depth2color frame
|
|
if (depthCamColorCoordFrame != null && rawDepthImage != null)
|
|
{
|
|
lock (depthCamColorFrameLock)
|
|
{
|
|
var pDepthData = GCHandle.Alloc(rawDepthImage, GCHandleType.Pinned);
|
|
var pColorCoordsData = GCHandle.Alloc(depthCamColorCoordFrame, GCHandleType.Pinned);
|
|
|
|
coordMapper.MapDepthFrameToColorSpaceUsingIntPtr(
|
|
pDepthData.AddrOfPinnedObject(),
|
|
rawDepthImage.Length * sizeof(ushort),
|
|
pColorCoordsData.AddrOfPinnedObject(),
|
|
(uint)depthCamColorCoordFrame.Length);
|
|
|
|
pColorCoordsData.Free();
|
|
pDepthData.Free();
|
|
|
|
//int di = (sensorData.depthImageHeight / 2) * sensorData.depthImageWidth + (sensorData.depthImageWidth / 2);
|
|
//Debug.Log("d2cCoordData: " + depth2ColorCoordFrame[di]);
|
|
|
|
lastDepthCamColorFrameTime = lastDepthCoordFrameTime;
|
|
//Debug.Log("DepthCamColorFrameTime: " + lastDepthCamColorFrameTime);
|
|
}
|
|
}
|
|
|
|
// color2depth frame
|
|
if (colorCamDepthCoordFrame != null)
|
|
{
|
|
lock (colorCamDepthFrameLock)
|
|
{
|
|
var pDepthData = GCHandle.Alloc(rawDepthImage, GCHandleType.Pinned);
|
|
var pDepthCoordsData = GCHandle.Alloc(colorCamDepthCoordFrame, GCHandleType.Pinned);
|
|
|
|
coordMapper.MapColorFrameToDepthSpaceUsingIntPtr(
|
|
pDepthData.AddrOfPinnedObject(),
|
|
(uint)rawDepthImage.Length * sizeof(ushort),
|
|
pDepthCoordsData.AddrOfPinnedObject(),
|
|
(uint)colorCamDepthCoordFrame.Length);
|
|
|
|
//int ci = (sensorData.colorImageHeight / 2) * sensorData.colorImageWidth + (sensorData.colorImageWidth / 2);
|
|
//Debug.Log("c2dCoordData: " + color2DepthCoordFrame[ci]);
|
|
|
|
pDepthCoordsData.Free();
|
|
pDepthData.Free();
|
|
|
|
lastColorCamDepthFrameTime = lastDepthCoordFrameTime;
|
|
if (pointCloudInfraredBuffer != null)
|
|
lastColorCamInfraredFrameTime = lastDepthCoordFrameTime;
|
|
|
|
//Debug.Log("ColorCamDepthFrameTime: " + lastColorCamDepthFrameTime);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
//public override bool UpdateSensorData(KinectInterop.SensorData sensorData)
|
|
//{
|
|
// base.UpdateSensorData(sensorData);
|
|
// return true;
|
|
//}
|
|
|
|
|
|
// creates the point-cloud vertex shader and its respective buffers, as needed
|
|
protected override bool CreatePointCloudVertexShader(KinectInterop.SensorData sensorData)
|
|
{
|
|
if (pointCloudResolution != PointCloudResolution.ColorCameraResolution)
|
|
{
|
|
return base.CreatePointCloudVertexShader(sensorData);
|
|
}
|
|
|
|
// for K2 color camera resolution only
|
|
pointCloudVertexRes = GetPointCloudTexResolution(sensorData);
|
|
pointCloudPlayerIndices = GetPointCloudPlayerIndices();
|
|
|
|
if (pointCloudVertexRT == null)
|
|
{
|
|
pointCloudVertexRT = new RenderTexture(pointCloudVertexRes.x, pointCloudVertexRes.y, 0, RenderTextureFormat.ARGBHalf);
|
|
pointCloudVertexRT.enableRandomWrite = true;
|
|
pointCloudVertexRT.Create();
|
|
}
|
|
|
|
if (pointCloudVertexShader == null)
|
|
{
|
|
if(pointCloudPlayerIndices == null)
|
|
pointCloudVertexShader = Resources.Load("PointCloudVertexShaderCRK2") as ComputeShader;
|
|
else
|
|
pointCloudVertexShader = Resources.Load("PointCloudBodyVertexShaderCRK2") as ComputeShader;
|
|
|
|
pointCloudVertexKernel = pointCloudVertexShader != null ? pointCloudVertexShader.FindKernel("BakeVertexTexColorResK2") : -1;
|
|
}
|
|
|
|
if (pointCloudSpaceBuffer == null)
|
|
{
|
|
int spaceBufferLength = sensorData.depthImageWidth * sensorData.depthImageHeight * 3;
|
|
pointCloudSpaceBuffer = new ComputeBuffer(spaceBufferLength, sizeof(float));
|
|
|
|
// depth2space table
|
|
//int depthImageLength = sensorData.depthImageWidth * sensorData.depthImageHeight;
|
|
//Vector3[] depth2SpaceTable = new Vector3[depthImageLength];
|
|
|
|
//for (int dy = 0, di = 0; dy < sensorData.depthImageHeight; dy++)
|
|
//{
|
|
// for (int dx = 0; dx < sensorData.depthImageWidth; dx++, di++)
|
|
// {
|
|
// Vector2 depthPos = new Vector2(dx, dy);
|
|
// depth2SpaceTable[di] = MapDepthPointToSpaceCoords(sensorData, depthPos, 1000);
|
|
// }
|
|
//}
|
|
|
|
Vector3[] depth2SpaceTable = GetDepthCameraSpaceTable(sensorData);
|
|
pointCloudSpaceBuffer.SetData(depth2SpaceTable);
|
|
depth2SpaceTable = null;
|
|
}
|
|
|
|
if (pointCloudDepthBuffer == null)
|
|
{
|
|
int depthBufferLength = sensorData.depthImageWidth * sensorData.depthImageHeight / 2;
|
|
pointCloudDepthBuffer = new ComputeBuffer(depthBufferLength, sizeof(uint));
|
|
}
|
|
|
|
if (pointCloudCoordBuffer == null)
|
|
{
|
|
int coordBufferLength = sensorData.colorImageWidth * sensorData.colorImageHeight * 2; // Vector2 = 2 x float
|
|
pointCloudCoordBuffer = new ComputeBuffer(coordBufferLength, sizeof(float));
|
|
}
|
|
|
|
if (colorCamDepthCoordFrame == null)
|
|
{
|
|
colorCamDepthCoordFrame = new Vector2[sensorData.colorImageWidth * sensorData.colorImageHeight];
|
|
}
|
|
|
|
if (pointCloudPlayerIndices != null)
|
|
{
|
|
int biBufferLength = pointCloudVertexRes.x * pointCloudVertexRes.y / 4;
|
|
pointCloudBodyIndexBuffer = new ComputeBuffer(biBufferLength, sizeof(uint));
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
// updates the point-cloud vertex shader with the actual data
|
|
protected override bool UpdatePointCloudVertexShader(KinectInterop.SensorData sensorData, KinectManager kinectManager)
|
|
{
|
|
if (pointCloudResolution != PointCloudResolution.ColorCameraResolution)
|
|
{
|
|
return base.UpdatePointCloudVertexShader(sensorData, kinectManager);
|
|
}
|
|
|
|
// for K2 color camera resolution only
|
|
if (pointCloudVertexShader != null && sensorData.depthImage != null && pointCloudVertexRT != null &&
|
|
sensorData.lastDepth2SpaceFrameTime != sensorData.lastDepthFrameTime)
|
|
{
|
|
sensorData.lastDepth2SpaceFrameTime = sensorData.lastDepthFrameTime;
|
|
|
|
KinectInterop.SetComputeBufferData(pointCloudDepthBuffer, sensorData.depthImage, sensorData.depthImage.Length >> 1, sizeof(uint));
|
|
|
|
lock (colorCamDepthFrameLock)
|
|
{
|
|
int coordBufferLength = sensorData.colorImageWidth * sensorData.colorImageHeight * 2;
|
|
KinectInterop.SetComputeBufferData(pointCloudCoordBuffer, colorCamDepthCoordFrame, coordBufferLength, sizeof(float));
|
|
}
|
|
|
|
if (GetPointCloudBodyIndices(sensorData, kinectManager, pointCloudPlayerIndices, ref pointCloudBodyIndices))
|
|
{
|
|
// body index
|
|
if (pointCloudBodyIndexBuffer != null && sensorData.bodyIndexImage != null)
|
|
{
|
|
KinectInterop.SetComputeBufferData(pointCloudBodyIndexBuffer, sensorData.bodyIndexImage, sensorData.bodyIndexImage.Length >> 2, sizeof(uint));
|
|
pointCloudVertexShader.SetBuffer(pointCloudVertexKernel, "BodyIndexMap", pointCloudBodyIndexBuffer);
|
|
}
|
|
|
|
// body params
|
|
int uCount = pointCloudBodyIndices.Length;
|
|
pointCloudVertexShader.SetInt("BodyIndexAll", uCount == 1 && pointCloudPlayerIndices[0] == -1 ? 1 : 0);
|
|
|
|
pointCloudVertexShader.SetInt("BodyIndex0", uCount > 0 ? pointCloudBodyIndices[0] : -1);
|
|
pointCloudVertexShader.SetInt("BodyIndex1", uCount > 1 ? pointCloudBodyIndices[1] : -1);
|
|
pointCloudVertexShader.SetInt("BodyIndex2", uCount > 2 ? pointCloudBodyIndices[2] : -1);
|
|
pointCloudVertexShader.SetInt("BodyIndex3", uCount > 3 ? pointCloudBodyIndices[3] : -1);
|
|
pointCloudVertexShader.SetInt("BodyIndex4", uCount > 4 ? pointCloudBodyIndices[4] : -1);
|
|
|
|
//Debug.Log("All: " + (pointCloudPlayerIndices[0] == -1 ? 1 : 0) + ", Ind: " +
|
|
// (uCount > 0 ? pointCloudBodyIndices[0] : -1) + " " +
|
|
// (uCount > 1 ? pointCloudBodyIndices[1] : -1) + " " +
|
|
// (uCount > 2 ? pointCloudBodyIndices[2] : -1) + " " +
|
|
// (uCount > 3 ? pointCloudBodyIndices[3] : -1) + " " +
|
|
// (uCount > 4 ? pointCloudBodyIndices[4] : -1));
|
|
}
|
|
|
|
KinectInterop.SetComputeShaderInt2(pointCloudVertexShader, "PointCloudRes", pointCloudVertexRes.x, pointCloudVertexRes.y);
|
|
KinectInterop.SetComputeShaderInt2(pointCloudVertexShader, "DepthRes", sensorData.depthImageWidth, sensorData.depthImageHeight);
|
|
KinectInterop.SetComputeShaderFloat2(pointCloudVertexShader, "SpaceScale", sensorData.sensorSpaceScale.x, sensorData.sensorSpaceScale.y);
|
|
pointCloudVertexShader.SetInt("MinDepth", (int)(minDepthDistance * 1000f));
|
|
pointCloudVertexShader.SetInt("MaxDepth", (int)(maxDepthDistance * 1000f));
|
|
pointCloudVertexShader.SetBuffer(pointCloudVertexKernel, "SpaceTable", pointCloudSpaceBuffer);
|
|
pointCloudVertexShader.SetBuffer(pointCloudVertexKernel, "DepthMap", pointCloudDepthBuffer);
|
|
pointCloudVertexShader.SetBuffer(pointCloudVertexKernel, "ColorToDepthMap", pointCloudCoordBuffer);
|
|
pointCloudVertexShader.SetTexture(pointCloudVertexKernel, "PointCloudVertexTex", pointCloudVertexRT);
|
|
pointCloudVertexShader.Dispatch(pointCloudVertexKernel, pointCloudVertexRes.x / 8, pointCloudVertexRes.y / 8, 1);
|
|
|
|
if (pointCloudVertexTexture != null)
|
|
{
|
|
Graphics.Blit(pointCloudVertexRT, pointCloudVertexTexture);
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
|
|
// creates the point-cloud color shader and its respective buffers, as needed
|
|
protected override bool CreatePointCloudColorShader(KinectInterop.SensorData sensorData)
|
|
{
|
|
if (pointCloudResolution != PointCloudResolution.DepthCameraResolution)
|
|
{
|
|
return base.CreatePointCloudColorShader(sensorData);
|
|
}
|
|
|
|
// for K2 depth camera resolution only
|
|
if (pointCloudColorRT == null)
|
|
{
|
|
pointCloudColorRT = new RenderTexture(sensorData.depthImageWidth, sensorData.depthImageHeight, 0, RenderTextureFormat.ARGB32);
|
|
pointCloudColorRT.enableRandomWrite = true;
|
|
pointCloudColorRT.Create();
|
|
}
|
|
|
|
if (pointCloudColorShader == null)
|
|
{
|
|
pointCloudColorShader = Resources.Load("PointCloudColorShaderK2") as ComputeShader;
|
|
pointCloudColorKernel = pointCloudColorShader != null ? pointCloudColorShader.FindKernel("BakeColorTex") : -1;
|
|
}
|
|
|
|
if (pointCloudCoordBuffer == null)
|
|
{
|
|
int coordBufferLength = sensorData.depthImageWidth * sensorData.depthImageHeight * 2; // Vector2 = 2 x float
|
|
pointCloudCoordBuffer = new ComputeBuffer(coordBufferLength, sizeof(float));
|
|
}
|
|
|
|
if (depthCamColorCoordFrame == null)
|
|
{
|
|
depthCamColorCoordFrame = new Vector2[sensorData.depthImageWidth * sensorData.depthImageHeight];
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
// updates the point-cloud color shader with the actual data
|
|
protected override bool UpdatePointCloudColorShader(KinectInterop.SensorData sensorData)
|
|
{
|
|
if (pointCloudResolution != PointCloudResolution.DepthCameraResolution)
|
|
{
|
|
return base.UpdatePointCloudColorShader(sensorData);
|
|
}
|
|
|
|
// for K2 depth camera resolution only
|
|
if (pointCloudColorShader != null && pointCloudCoordBuffer != null && sensorData.colorImageTexture != null && pointCloudColorRT != null &&
|
|
sensorData.lastDepthCamColorFrameTime != lastDepthCamColorFrameTime)
|
|
{
|
|
sensorData.lastDepthCamColorFrameTime = lastDepthCamColorFrameTime;
|
|
|
|
lock (depthCamColorFrameLock)
|
|
{
|
|
int coordBufferLength = sensorData.depthImageWidth * sensorData.depthImageHeight * 2;
|
|
KinectInterop.SetComputeBufferData(pointCloudCoordBuffer, depthCamColorCoordFrame, coordBufferLength, sizeof(float));
|
|
}
|
|
|
|
KinectInterop.SetComputeShaderInt2(pointCloudColorShader, "DepthRes", sensorData.depthImageWidth, sensorData.depthImageHeight);
|
|
pointCloudColorShader.SetBuffer(pointCloudColorKernel, "DepthToColorMap", pointCloudCoordBuffer);
|
|
pointCloudColorShader.SetTexture(pointCloudColorKernel, "ColorTex", sensorData.colorImageTexture);
|
|
pointCloudColorShader.SetTexture(pointCloudColorKernel, "PointCloudColorTex", pointCloudColorRT);
|
|
pointCloudColorShader.Dispatch(pointCloudColorKernel, sensorData.depthImageWidth / 8, sensorData.depthImageHeight / 8, 1);
|
|
|
|
if(pointCloudColorTexture != null)
|
|
{
|
|
Graphics.Blit(pointCloudColorRT, pointCloudColorTexture);
|
|
}
|
|
|
|
if (sensorData.depthCamColorImageTexture != null)
|
|
{
|
|
Texture2D depth2colorTex2d = (Texture2D)sensorData.depthCamColorImageTexture;
|
|
KinectInterop.RenderTex2Tex2D(pointCloudColorTexture, ref depth2colorTex2d);
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
|
|
// creates the color-depth shader and its respective buffers, as needed
|
|
protected override bool CreateColorDepthShader(KinectInterop.SensorData sensorData)
|
|
{
|
|
if (colorDepthShader == null)
|
|
{
|
|
colorDepthShader = Resources.Load("ColorDepthShaderK2") as ComputeShader;
|
|
colorDepthKernel = colorDepthShader != null ? colorDepthShader.FindKernel("BakeColorDepth") : -1;
|
|
}
|
|
|
|
if (pointCloudDepthBuffer == null)
|
|
{
|
|
int bufferLength = sensorData.depthImageWidth * sensorData.depthImageHeight / 2;
|
|
pointCloudDepthBuffer = new ComputeBuffer(bufferLength, sizeof(uint));
|
|
}
|
|
|
|
if (pointCloudCoordBuffer == null)
|
|
{
|
|
int bufferLength = sensorData.colorImageWidth * sensorData.colorImageHeight * 2; // Vector2 = 2 x float
|
|
pointCloudCoordBuffer = new ComputeBuffer(bufferLength, sizeof(float));
|
|
}
|
|
|
|
if (colorCamDepthCoordFrame == null)
|
|
{
|
|
colorCamDepthCoordFrame = new Vector2[sensorData.colorImageWidth * sensorData.colorImageHeight];
|
|
}
|
|
|
|
if (sensorData.colorDepthTexture == null)
|
|
{
|
|
sensorData.colorDepthTexture = new RenderTexture(sensorData.colorImageWidth, sensorData.colorImageHeight, 0, RenderTextureFormat.ARGB32);
|
|
//sensorData.colorDepthTexture.enableRandomWrite = true;
|
|
sensorData.colorDepthTexture.Create();
|
|
}
|
|
|
|
colorDepthShaderInited = true;
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
// updates the color-depth shader with the actual data
|
|
protected override bool UpdateColorDepthShader(KinectInterop.SensorData sensorData)
|
|
{
|
|
// for K2 depth camera resolution only
|
|
if (colorDepthShader != null && pointCloudDepthBuffer != null && pointCloudCoordBuffer != null && colorCamDepthCoordFrame != null)
|
|
{
|
|
if (/**sensorData.usedColorDepthBufferTime == sensorData.lastColorDepthBufferTime && */
|
|
sensorData.lastColorDepthBufferTime != lastColorCamDepthFrameTime)
|
|
{
|
|
sensorData.lastColorDepthBufferTime = lastColorCamDepthFrameTime;
|
|
|
|
if (sensorData.colorImageTexture != null)
|
|
{
|
|
Graphics.Blit(sensorData.colorImageTexture, sensorData.colorDepthTexture);
|
|
}
|
|
|
|
KinectInterop.SetComputeBufferData(pointCloudDepthBuffer, sensorData.depthImage, sensorData.depthImage.Length >> 1, sizeof(uint));
|
|
|
|
lock (colorCamDepthFrameLock)
|
|
{
|
|
int bufferLength = sensorData.colorImageWidth * sensorData.colorImageHeight * 2;
|
|
KinectInterop.SetComputeBufferData(pointCloudCoordBuffer, colorCamDepthCoordFrame, bufferLength, sizeof(float));
|
|
}
|
|
|
|
if(sensorData.colorDepthBuffer != null)
|
|
{
|
|
KinectInterop.SetComputeShaderInt2(colorDepthShader, "_ColorRes", sensorData.colorImageWidth, sensorData.colorImageHeight);
|
|
KinectInterop.SetComputeShaderInt2(colorDepthShader, "_DepthRes", sensorData.depthImageWidth, sensorData.depthImageHeight);
|
|
|
|
colorDepthShader.SetBuffer(colorDepthKernel, "_DepthMap", pointCloudDepthBuffer);
|
|
colorDepthShader.SetBuffer(colorDepthKernel, "_Color2DepthMap", pointCloudCoordBuffer);
|
|
//colorDepthShader.SetTexture(colorDepthKernel, "_ColorTex", sensorData.colorImageTexture);
|
|
colorDepthShader.SetBuffer(colorDepthKernel, "_ColorDepthMap", sensorData.colorDepthBuffer);
|
|
colorDepthShader.Dispatch(colorDepthKernel, sensorData.colorImageWidth / 8, sensorData.colorImageHeight / 8, 1);
|
|
|
|
if (sensorData.colorCamDepthImage != null)
|
|
{
|
|
sensorData.colorDepthBuffer.GetData(sensorData.colorCamDepthImage);
|
|
sensorData.lastColorCamDepthFrameTime = lastColorCamDepthFrameTime;
|
|
}
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
|
|
// creates the color-infrared shader and its respective buffers, as needed
|
|
protected override bool CreateColorInfraredShader(KinectInterop.SensorData sensorData)
|
|
{
|
|
if (colorInfraredShader == null)
|
|
{
|
|
colorInfraredShader = Resources.Load("ColorInfraredShaderK2") as ComputeShader;
|
|
colorInfraredKernel = colorInfraredShader != null ? colorInfraredShader.FindKernel("BakeColorInfrared") : -1;
|
|
}
|
|
|
|
if (pointCloudInfraredBuffer == null)
|
|
{
|
|
int bufferLength = sensorData.depthImageWidth * sensorData.depthImageHeight / 2;
|
|
pointCloudInfraredBuffer = new ComputeBuffer(bufferLength, sizeof(uint));
|
|
}
|
|
|
|
if (pointCloudCoordBuffer == null)
|
|
{
|
|
int bufferLength = sensorData.colorImageWidth * sensorData.colorImageHeight * 2; // Vector2 = 2 x float
|
|
pointCloudCoordBuffer = new ComputeBuffer(bufferLength, sizeof(float));
|
|
}
|
|
|
|
if (colorCamDepthCoordFrame == null)
|
|
{
|
|
colorCamDepthCoordFrame = new Vector2[sensorData.colorImageWidth * sensorData.colorImageHeight];
|
|
}
|
|
|
|
if (sensorData.colorInfraredTexture == null)
|
|
{
|
|
sensorData.colorInfraredTexture = new RenderTexture(sensorData.colorImageWidth, sensorData.colorImageHeight, 0, RenderTextureFormat.ARGB32);
|
|
//sensorData.colorInfraredTexture.enableRandomWrite = true;
|
|
sensorData.colorInfraredTexture.Create();
|
|
}
|
|
|
|
Shader infraredImageShader = Shader.Find("Kinect/InfraredImageShader");
|
|
if (infraredImageShader)
|
|
{
|
|
sensorData.colorInfraredMaterial = new Material(infraredImageShader);
|
|
}
|
|
|
|
colorInfraredShaderInited = true;
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
// updates the color-infrared shader with the actual data
|
|
protected override bool UpdateColorInfraredShader(KinectInterop.SensorData sensorData)
|
|
{
|
|
// for K2 depth camera resolution only
|
|
if (colorInfraredShader != null && pointCloudInfraredBuffer != null && pointCloudCoordBuffer != null && colorCamDepthCoordFrame != null)
|
|
{
|
|
if (/**sensorData.usedColorInfraredBufferTime == sensorData.lastColorInfraredBufferTime && */
|
|
sensorData.lastColorInfraredBufferTime != lastColorCamInfraredFrameTime)
|
|
{
|
|
sensorData.lastColorInfraredBufferTime = lastColorCamInfraredFrameTime;
|
|
//Debug.Log("LastColorCamInfraredFrameTime: " + lastColorCamInfraredFrameTime);
|
|
|
|
//if (sensorData.colorImageTexture != null)
|
|
//{
|
|
// Graphics.Blit(sensorData.colorImageTexture, sensorData.colorInfraredTexture);
|
|
//}
|
|
|
|
KinectInterop.SetComputeBufferData(pointCloudInfraredBuffer, sensorData.infraredImage, sensorData.infraredImage.Length >> 1, sizeof(uint));
|
|
|
|
lock (colorCamDepthFrameLock)
|
|
{
|
|
int bufferLength = sensorData.colorImageWidth * sensorData.colorImageHeight * 2;
|
|
KinectInterop.SetComputeBufferData(pointCloudCoordBuffer, colorCamDepthCoordFrame, bufferLength, sizeof(float));
|
|
}
|
|
|
|
if (sensorData.colorInfraredBuffer != null)
|
|
{
|
|
KinectInterop.SetComputeShaderInt2(colorInfraredShader, "_ColorRes", sensorData.colorImageWidth, sensorData.colorImageHeight);
|
|
KinectInterop.SetComputeShaderInt2(colorInfraredShader, "_DepthRes", sensorData.depthImageWidth, sensorData.depthImageHeight);
|
|
|
|
colorInfraredShader.SetBuffer(colorInfraredKernel, "_InfraredMap", pointCloudInfraredBuffer);
|
|
colorInfraredShader.SetBuffer(colorInfraredKernel, "_Color2DepthMap", pointCloudCoordBuffer);
|
|
//colorInfraredShader.SetTexture(colorInfraredKernel, "_ColorTex", sensorData.colorImageTexture);
|
|
colorInfraredShader.SetBuffer(colorInfraredKernel, "_ColorInfraredMap", sensorData.colorInfraredBuffer);
|
|
colorInfraredShader.Dispatch(colorInfraredKernel, sensorData.colorImageWidth / 8, sensorData.colorImageHeight / 8, 1);
|
|
|
|
if (sensorData.colorCamInfraredImage != null)
|
|
{
|
|
sensorData.colorInfraredBuffer.GetData(sensorData.colorCamInfraredImage);
|
|
sensorData.lastColorCamInfraredFrameTime = lastColorCamInfraredFrameTime;
|
|
}
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
|
|
// creates the color-depth shader and its respective buffers, as needed
|
|
protected override bool CreateColorBodyIndexShader(KinectInterop.SensorData sensorData)
|
|
{
|
|
if (colorBodyIndexShader == null)
|
|
{
|
|
colorBodyIndexShader = Resources.Load("ColorBodyIndexShaderK2") as ComputeShader;
|
|
colorBodyIndexKernel = colorBodyIndexShader != null ? colorBodyIndexShader.FindKernel("BakeColorBodyIndex") : -1;
|
|
}
|
|
|
|
if (bodyIndexBuffer == null)
|
|
{
|
|
int bufferLength = sensorData.depthImageWidth * sensorData.depthImageHeight / 4;
|
|
bodyIndexBuffer = new ComputeBuffer(bufferLength, sizeof(uint));
|
|
}
|
|
|
|
if (pointCloudCoordBuffer == null)
|
|
{
|
|
int bufferLength = sensorData.colorImageWidth * sensorData.colorImageHeight * 2; // Vector2 = 2 x float
|
|
pointCloudCoordBuffer = new ComputeBuffer(bufferLength, sizeof(float));
|
|
}
|
|
|
|
if (colorCamDepthCoordFrame == null)
|
|
{
|
|
colorCamDepthCoordFrame = new Vector2[sensorData.colorImageWidth * sensorData.colorImageHeight];
|
|
}
|
|
|
|
if (sensorData.colorBodyIndexTexture == null)
|
|
{
|
|
sensorData.colorBodyIndexTexture = new RenderTexture(sensorData.colorImageWidth, sensorData.colorImageHeight, 0, RenderTextureFormat.ARGB32);
|
|
//sensorData.colorBodyIndexTexture.enableRandomWrite = true;
|
|
sensorData.colorBodyIndexTexture.Create();
|
|
}
|
|
|
|
colorBodyIndexShaderInited = true;
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
// updates the color body index shader with the actual data
|
|
protected override bool UpdateColorBodyIndexShader(KinectInterop.SensorData sensorData)
|
|
{
|
|
// for K2 depth camera resolution only
|
|
if (colorBodyIndexShader != null && bodyIndexBuffer != null && pointCloudCoordBuffer != null && colorCamDepthCoordFrame != null)
|
|
{
|
|
if (/**sensorData.usedColorBodyIndexBufferTime == sensorData.lastColorBodyIndexBufferTime && */
|
|
sensorData.bodyIndexImage != null && sensorData.lastColorBodyIndexBufferTime != lastColorCamDepthFrameTime)
|
|
{
|
|
sensorData.lastColorBodyIndexBufferTime = lastColorCamDepthFrameTime;
|
|
|
|
if (sensorData.colorImageTexture != null)
|
|
{
|
|
Graphics.Blit(sensorData.colorImageTexture, sensorData.colorBodyIndexTexture);
|
|
}
|
|
|
|
KinectInterop.SetComputeBufferData(bodyIndexBuffer, sensorData.bodyIndexImage, sensorData.bodyIndexImage.Length >> 2, sizeof(uint));
|
|
|
|
lock (colorCamDepthFrameLock)
|
|
{
|
|
int bufferLength = sensorData.colorImageWidth * sensorData.colorImageHeight * 2;
|
|
KinectInterop.SetComputeBufferData(pointCloudCoordBuffer, colorCamDepthCoordFrame, bufferLength, sizeof(float));
|
|
}
|
|
|
|
if(sensorData.colorBodyIndexBuffer != null)
|
|
{
|
|
KinectInterop.SetComputeShaderInt2(colorBodyIndexShader, "_ColorRes", sensorData.colorImageWidth, sensorData.colorImageHeight);
|
|
KinectInterop.SetComputeShaderInt2(colorBodyIndexShader, "_DepthRes", sensorData.depthImageWidth, sensorData.depthImageHeight);
|
|
|
|
colorBodyIndexShader.SetBuffer(colorBodyIndexKernel, "_BodyIndexMap", bodyIndexBuffer);
|
|
colorBodyIndexShader.SetBuffer(colorBodyIndexKernel, "_Color2DepthMap", pointCloudCoordBuffer);
|
|
//colorBodyIndexShader.SetTexture(colorBodyIndexKernel, "_ColorTex", sensorData.colorImageTexture);
|
|
colorBodyIndexShader.SetBuffer(colorBodyIndexKernel, "_ColorBodyIndexMap", sensorData.colorBodyIndexBuffer);
|
|
colorBodyIndexShader.Dispatch(colorBodyIndexKernel, sensorData.colorImageWidth / 8, sensorData.colorImageHeight / 8, 1);
|
|
|
|
if (sensorData.colorCamBodyIndexImage != null)
|
|
{
|
|
sensorData.colorBodyIndexBuffer.GetData(sensorData.colorCamBodyIndexImage);
|
|
sensorData.lastColorCamBodyIndexFrameTime = lastColorCamDepthFrameTime;
|
|
}
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
|
|
public override Vector3 MapDepthPointToSpaceCoords(KinectInterop.SensorData sensorData, Vector2 depthPos, ushort depthVal)
|
|
{
|
|
Vector3 vPoint = Vector3.zero;
|
|
|
|
if (coordMapper != null && depthPos != Vector2.zero)
|
|
{
|
|
DepthSpacePoint depthPoint = new DepthSpacePoint();
|
|
depthPoint.X = depthPos.x;
|
|
depthPoint.Y = depthPos.y;
|
|
|
|
DepthSpacePoint[] depthPoints = new DepthSpacePoint[1];
|
|
depthPoints[0] = depthPoint;
|
|
|
|
ushort[] depthVals = new ushort[1];
|
|
depthVals[0] = depthVal;
|
|
|
|
CameraSpacePoint[] camPoints = new CameraSpacePoint[1];
|
|
coordMapper.MapDepthPointsToCameraSpace(depthPoints, depthVals, camPoints);
|
|
|
|
CameraSpacePoint camPoint = camPoints[0];
|
|
vPoint.x = camPoint.X;
|
|
vPoint.y = camPoint.Y;
|
|
vPoint.z = camPoint.Z;
|
|
}
|
|
|
|
return vPoint;
|
|
}
|
|
|
|
public override Vector2 MapSpacePointToDepthCoords(KinectInterop.SensorData sensorData, Vector3 spacePos)
|
|
{
|
|
Vector2 vPoint = Vector2.zero;
|
|
|
|
if (coordMapper != null)
|
|
{
|
|
CameraSpacePoint camPoint = new CameraSpacePoint();
|
|
camPoint.X = spacePos.x;
|
|
camPoint.Y = spacePos.y;
|
|
camPoint.Z = spacePos.z;
|
|
|
|
CameraSpacePoint[] camPoints = new CameraSpacePoint[1];
|
|
camPoints[0] = camPoint;
|
|
|
|
DepthSpacePoint[] depthPoints = new DepthSpacePoint[1];
|
|
coordMapper.MapCameraPointsToDepthSpace(camPoints, depthPoints);
|
|
|
|
DepthSpacePoint depthPoint = depthPoints[0];
|
|
|
|
if (depthPoint.X >= 0 && depthPoint.X < sensorData.depthImageWidth &&
|
|
depthPoint.Y >= 0 && depthPoint.Y < sensorData.depthImageHeight)
|
|
{
|
|
vPoint.x = depthPoint.X;
|
|
vPoint.y = depthPoint.Y;
|
|
}
|
|
}
|
|
|
|
return vPoint;
|
|
}
|
|
|
|
public override Vector2 MapDepthPointToColorCoords(KinectInterop.SensorData sensorData, Vector2 depthPos, ushort depthVal)
|
|
{
|
|
Vector2 vPoint = Vector2.zero;
|
|
|
|
if (coordMapper != null && depthPos != Vector2.zero)
|
|
{
|
|
DepthSpacePoint depthPoint = new DepthSpacePoint();
|
|
depthPoint.X = depthPos.x;
|
|
depthPoint.Y = depthPos.y;
|
|
|
|
DepthSpacePoint[] depthPoints = new DepthSpacePoint[1];
|
|
depthPoints[0] = depthPoint;
|
|
|
|
ushort[] depthVals = new ushort[1];
|
|
depthVals[0] = depthVal;
|
|
|
|
ColorSpacePoint[] colPoints = new ColorSpacePoint[1];
|
|
coordMapper.MapDepthPointsToColorSpace(depthPoints, depthVals, colPoints);
|
|
|
|
ColorSpacePoint colPoint = colPoints[0];
|
|
vPoint.x = colPoint.X;
|
|
vPoint.y = colPoint.Y;
|
|
}
|
|
|
|
return vPoint;
|
|
}
|
|
|
|
//public override bool MapDepthFrameToSpaceCoords(KinectInterop.SensorData sensorData, ref Vector3[] vSpaceCoords)
|
|
//{
|
|
// if (coordMapper != null && sensorData.depthImage != null)
|
|
// {
|
|
// var pDepthData = GCHandle.Alloc(sensorData.depthImage, GCHandleType.Pinned);
|
|
// var pSpaceCoordsData = GCHandle.Alloc(vSpaceCoords, GCHandleType.Pinned);
|
|
|
|
// coordMapper.MapDepthFrameToCameraSpaceUsingIntPtr(
|
|
// pDepthData.AddrOfPinnedObject(),
|
|
// sensorData.depthImage.Length * sizeof(ushort),
|
|
// pSpaceCoordsData.AddrOfPinnedObject(),
|
|
// (uint)vSpaceCoords.Length);
|
|
|
|
// pSpaceCoordsData.Free();
|
|
// pDepthData.Free();
|
|
|
|
// return true;
|
|
// }
|
|
|
|
// return false;
|
|
//}
|
|
|
|
//public override bool MapDepthFrameToColorCoords(KinectInterop.SensorData sensorData, ref Vector2[] vColorCoords)
|
|
//{
|
|
// if (coordMapper != null && sensorData.colorImageTexture != null && sensorData.depthImage != null)
|
|
// {
|
|
// var pDepthData = GCHandle.Alloc(sensorData.depthImage, GCHandleType.Pinned);
|
|
// var pColorCoordsData = GCHandle.Alloc(vColorCoords, GCHandleType.Pinned);
|
|
|
|
// coordMapper.MapDepthFrameToColorSpaceUsingIntPtr(
|
|
// pDepthData.AddrOfPinnedObject(),
|
|
// sensorData.depthImage.Length * sizeof(ushort),
|
|
// pColorCoordsData.AddrOfPinnedObject(),
|
|
// (uint)vColorCoords.Length);
|
|
|
|
// pColorCoordsData.Free();
|
|
// pDepthData.Free();
|
|
|
|
// return true;
|
|
// }
|
|
|
|
// return false;
|
|
//}
|
|
|
|
//public override bool MapColorFrameToDepthCoords(KinectInterop.SensorData sensorData, ref Vector2[] vDepthCoords)
|
|
//{
|
|
// if (coordMapper != null && sensorData.colorImageTexture != null && sensorData.depthImage != null)
|
|
// {
|
|
// var pDepthData = GCHandle.Alloc(sensorData.depthImage, GCHandleType.Pinned);
|
|
// var pDepthCoordsData = GCHandle.Alloc(vDepthCoords, GCHandleType.Pinned);
|
|
|
|
// coordMapper.MapColorFrameToDepthSpaceUsingIntPtr(
|
|
// pDepthData.AddrOfPinnedObject(),
|
|
// (uint)sensorData.depthImage.Length * sizeof(ushort),
|
|
// pDepthCoordsData.AddrOfPinnedObject(),
|
|
// (uint)vDepthCoords.Length);
|
|
|
|
// pDepthCoordsData.Free();
|
|
// pDepthData.Free();
|
|
|
|
// return true;
|
|
// }
|
|
|
|
// return false;
|
|
//}
|
|
|
|
|
|
// enables or disables color camera depth frame processing
|
|
public override void EnableColorCameraDepthFrame(KinectInterop.SensorData sensorData, bool isEnable)
|
|
{
|
|
base.EnableColorCameraDepthFrame(sensorData, isEnable);
|
|
|
|
if (isEnable && sensorData.colorDepthBuffer == null)
|
|
{
|
|
if (sensorData.colorImageWidth > 0 && sensorData.colorImageHeight > 0)
|
|
{
|
|
int bufferLength = sensorData.colorImageWidth * sensorData.colorImageHeight / 2;
|
|
sensorData.colorDepthBuffer = new ComputeBuffer(bufferLength, sizeof(uint));
|
|
}
|
|
}
|
|
else if (!isEnable && sensorData.colorDepthBuffer != null)
|
|
{
|
|
sensorData.colorDepthBuffer.Release();
|
|
sensorData.colorDepthBuffer.Dispose();
|
|
sensorData.colorDepthBuffer = null;
|
|
}
|
|
}
|
|
|
|
// enables or disables color camera body-index frame processing
|
|
public override void EnableColorCameraBodyIndexFrame(KinectInterop.SensorData sensorData, bool isEnable)
|
|
{
|
|
base.EnableColorCameraBodyIndexFrame(sensorData, isEnable);
|
|
|
|
if (isEnable && sensorData.colorBodyIndexBuffer == null)
|
|
{
|
|
if (sensorData.colorImageWidth > 0 && sensorData.colorImageHeight > 0)
|
|
{
|
|
int bufferLength = sensorData.colorImageWidth * sensorData.colorImageHeight / 4;
|
|
sensorData.colorBodyIndexBuffer = new ComputeBuffer(bufferLength, sizeof(uint));
|
|
}
|
|
}
|
|
else if (!isEnable && sensorData.colorBodyIndexBuffer != null)
|
|
{
|
|
sensorData.colorBodyIndexBuffer.Release();
|
|
sensorData.colorBodyIndexBuffer.Dispose();
|
|
sensorData.colorBodyIndexBuffer = null;
|
|
}
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
#endif
|