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.
3689 lines
167 KiB
3689 lines
167 KiB
using System;
|
|
using System.Collections;
|
|
using System.Collections.Generic;
|
|
using UnityEngine;
|
|
using static com.rfilkov.kinect.KinectInterop;
|
|
using Windows.Kinect;
|
|
|
|
namespace com.rfilkov.kinect
|
|
{
|
|
/// <summary>
|
|
/// DepthSensorBase is the base class of all sensor interface implementations.
|
|
/// </summary>
|
|
public abstract class DepthSensorBase : MonoBehaviour, DepthSensorInterface
|
|
{
|
|
// whether to get and process all possible sensor frames (true), or get new frames after the previous ones have been moved to sensor-data (false)
|
|
protected bool getAllSensorFrames = false;
|
|
|
|
// max depth distance in mm, used for initializing data arrays and compute buffers
|
|
public const int MAX_DEPTH_DISTANCE_MM = 10000;
|
|
|
|
[Tooltip("Device streaming mode, in means of connected sensor, recording or disabled.")]
|
|
public KinectInterop.DeviceStreamingMode deviceStreamingMode = KinectInterop.DeviceStreamingMode.ConnectedSensor;
|
|
|
|
[Tooltip("Index of the depth sensor in the list of currently connected sensors.")]
|
|
public int deviceIndex = 0;
|
|
|
|
[Tooltip("Path to the recording file, if the streaming mode is PlayRecording.")]
|
|
public string recordingFile = string.Empty;
|
|
|
|
//[Tooltip("Sensor position in space.")]
|
|
//public Vector3 devicePosition = new Vector3(0f, 1f, 0f);
|
|
|
|
//[Tooltip("Sensor rotation in space.")]
|
|
//public Vector3 deviceRotation = new Vector3(0f, 0f, 0f);
|
|
|
|
//[Tooltip("Whether the body tracking for this sensor is enabled or not.")]
|
|
//internal bool bodyTrackingEnabled = false;
|
|
|
|
[Tooltip("Minimum distance in meters, used for creating the depth-related images.")]
|
|
[Range(0f, 10f)]
|
|
public float minDepthDistance = 0.5f;
|
|
|
|
[Tooltip("Maximum distance in meters, used for creating the depth-related images.")]
|
|
[Range(0f, 10f)]
|
|
public float maxDepthDistance = 10f;
|
|
|
|
[Tooltip("Resolution of the generated point-cloud textures.")]
|
|
public PointCloudResolution pointCloudResolution = PointCloudResolution.DepthCameraResolution;
|
|
public enum PointCloudResolution : int { DepthCameraResolution = 0, ColorCameraResolution = 1 }
|
|
|
|
[Tooltip("Render texture, used for point-cloud vertex mapping. The texture resolution should match the depth or color image resolution.")]
|
|
public RenderTexture pointCloudVertexTexture = null;
|
|
|
|
[Tooltip("Render texture, used for point-cloud color mapping. The texture resolution should match the depth or color image resolution.")]
|
|
public RenderTexture pointCloudColorTexture = null;
|
|
|
|
[Tooltip("List of comma-separated player indices to be included in the point cloud. Use -1 for all players, or empty list for full point cloud.")]
|
|
public string pointCloudPlayerList = string.Empty;
|
|
|
|
|
|
// initial parameters
|
|
[HideInInspector]
|
|
public KinectInterop.FrameSource frameSourceFlags;
|
|
protected bool isSyncDepthAndColor = false;
|
|
protected bool isSyncBodyAndDepth = false;
|
|
protected bool consoleLogMessages = false;
|
|
|
|
// sensor device-id & platform type
|
|
protected string sensorDeviceId = null;
|
|
protected KinectInterop.DepthSensorPlatform sensorPlatform = KinectInterop.DepthSensorPlatform.None;
|
|
|
|
// initial pose parameters
|
|
protected Vector3 initialPosePosition = Vector3.zero;
|
|
protected Quaternion initialPoseRotation = Quaternion.identity;
|
|
protected Matrix4x4 matTransformPose = Matrix4x4.identity;
|
|
protected Matrix4x4 matLocalPose = Matrix4x4.identity;
|
|
|
|
// frame numbers
|
|
//protected ulong colorFrameNumber = 0;
|
|
//protected ulong depthFrameNumber = 0;
|
|
//protected ulong infraredFrameNumber = 0;
|
|
//protected ulong poseFrameNumber = 0;
|
|
|
|
// raw color data
|
|
protected byte[] rawColorImage = null;
|
|
protected ulong rawColorTimestamp = 0;
|
|
protected ulong currentColorTimestamp = 0;
|
|
public object colorFrameLock = new object();
|
|
|
|
// raw depth data
|
|
protected ushort[] rawDepthImage = null;
|
|
protected ulong rawDepthTimestamp = 0;
|
|
protected ulong currentDepthTimestamp = 0;
|
|
public object depthFrameLock = new object();
|
|
|
|
// raw infrared data
|
|
protected ushort[] rawInfraredImage = null;
|
|
protected ulong rawInfraredTimestamp = 0;
|
|
protected ulong currentInfraredTimestamp = 0;
|
|
public object infraredFrameLock = new object();
|
|
|
|
// raw pose data
|
|
protected Vector3 rawPosePosition = Vector3.zero;
|
|
protected Quaternion rawPoseRotation = Quaternion.identity;
|
|
protected ulong rawPoseTimestamp = 0;
|
|
protected ulong currentPoseTimestamp = 0;
|
|
public object poseFrameLock = new object();
|
|
|
|
// sensor pose data
|
|
protected Vector3 sensorPosePosition;
|
|
protected Quaternion sensorPoseRotation;
|
|
|
|
[HideInInspector]
|
|
public Matrix4x4 depth2colorCamMat = Matrix4x4.identity;
|
|
|
|
protected Vector3 sensorRotOffset = Vector3.zero;
|
|
protected bool sensorRotFlipZ = false;
|
|
protected bool sensorRotIgnoreY = false;
|
|
[HideInInspector]
|
|
public float sensorRotValueY = 0f;
|
|
|
|
// body tracker
|
|
protected bool bIgnoreZCoordinates = false;
|
|
protected bool bIgnoreInferredJoints = false;
|
|
|
|
// raw body data
|
|
protected byte[] rawBodyIndexImage = null;
|
|
protected uint trackedBodiesCount = 0;
|
|
protected List<KinectInterop.BodyData> alTrackedBodies = null;
|
|
protected ulong rawBodyTimestamp = 0;
|
|
protected ulong rawBodyIndexTimestamp = 0;
|
|
protected ulong currentBodyTimestamp = 0;
|
|
protected ulong currentBodyIndexTimestamp = 0;
|
|
public object bodyTrackerLock = new object();
|
|
|
|
|
|
// depth image data
|
|
protected int[] depthHistBufferData = null;
|
|
protected int[] equalHistBufferData = null;
|
|
protected int depthHistTotalPoints = 0;
|
|
//protected ulong lastDepthImageTimestamp = 0;
|
|
//protected object depthImageDataLock = new object();
|
|
|
|
// infrared image data
|
|
protected float minInfraredValue = 0f;
|
|
protected float maxInfraredValue = 0f;
|
|
|
|
// body image data
|
|
protected int[] depthBodyBufferData = null;
|
|
protected int[] equalBodyBufferData = null;
|
|
protected int bodyHistTotalPoints = 0;
|
|
//protected ulong lastBodyImageTimestamp = 0;
|
|
//protected object bodyImageDataLock = new object();
|
|
|
|
// last updated depth coord-frame time
|
|
protected ulong lastDepthCoordFrameTime = 0;
|
|
|
|
// point cloud vertex shader
|
|
protected ComputeShader pointCloudVertexShader = null;
|
|
protected int pointCloudVertexKernel = -1;
|
|
|
|
protected Vector2Int pointCloudVertexRes = Vector2Int.zero;
|
|
protected int[] pointCloudPlayerIndices = null;
|
|
protected int[] pointCloudBodyIndices = null;
|
|
|
|
protected RenderTexture pointCloudVertexRT = null;
|
|
protected ComputeBuffer pointCloudSpaceBuffer = null;
|
|
protected ComputeBuffer pointCloudDepthBuffer = null;
|
|
protected ComputeBuffer pointCloudInfraredBuffer = null;
|
|
protected ComputeBuffer pointCloudBodyIndexBuffer = null;
|
|
|
|
// point cloud color shader
|
|
protected ComputeShader pointCloudColorShader = null;
|
|
protected int pointCloudColorKernel = -1;
|
|
protected Vector2Int pointCloudColorRes = Vector2Int.zero;
|
|
protected RenderTexture pointCloudColorRT = null;
|
|
protected ComputeBuffer pointCloudCoordBuffer = null;
|
|
protected Texture pointCloudAlignedColorTex = null;
|
|
|
|
//// depth2space coords frame
|
|
//protected Vector3[] depth2SpaceCoordFrame = null;
|
|
//protected ulong lastDepth2SpaceFrameTime = 0;
|
|
//protected object depth2SpaceFrameLock = new object();
|
|
|
|
// space tables
|
|
protected Vector3[] depth2SpaceTable = null;
|
|
protected int depth2SpaceWidth = 0, depth2SpaceHeight = 0;
|
|
protected Vector3[] color2SpaceTable = null;
|
|
protected int color2SpaceWidth = 0, color2SpaceHeight = 0;
|
|
//protected ushort[] lastDepthDataBuf = null;
|
|
|
|
// depth cam color frame
|
|
protected byte[] depthCamColorDataFrame = null;
|
|
protected Vector2[] depthCamColorCoordFrame = null;
|
|
protected ulong lastDepthCamColorFrameTime = 0;
|
|
protected object depthCamColorFrameLock = new object();
|
|
|
|
// color cam depth frame
|
|
protected ushort[] colorCamDepthDataFrame = null;
|
|
protected Vector2[] colorCamDepthCoordFrame = null;
|
|
protected ulong lastColorCamDepthFrameTime = 0;
|
|
protected object colorCamDepthFrameLock = new object();
|
|
|
|
// color cam body index frame
|
|
protected byte[] colorCamBodyIndexFrame = null;
|
|
protected ulong lastColorCamBodyIndexFrameTime = 0;
|
|
protected object colorCamBodyIndexFrameLock = new object();
|
|
|
|
// color cam infrared frame
|
|
protected ushort[] colorCamInfraredDataFrame = null;
|
|
protected ulong lastColorCamInfraredFrameTime = 0;
|
|
protected object colorCamInfraredFrameLock = new object();
|
|
|
|
// color cam depth shader
|
|
protected ComputeShader colorDepthShader = null;
|
|
protected int colorDepthKernel = -1;
|
|
protected bool colorDepthShaderInited = false;
|
|
|
|
// color cam infrared shader
|
|
protected ComputeShader colorInfraredShader = null;
|
|
protected int colorInfraredKernel = -1;
|
|
protected bool colorInfraredShaderInited = false;
|
|
|
|
// color cam body index shader
|
|
protected ComputeShader colorBodyIndexShader = null;
|
|
protected int colorBodyIndexKernel = -1;
|
|
protected bool colorBodyIndexShaderInited = false;
|
|
protected ComputeBuffer bodyIndexBuffer = null;
|
|
|
|
// user body merger
|
|
protected KinectUserBodyMerger userBodyMerger = null;
|
|
protected bool userBodyMergerInited = false;
|
|
|
|
// joint position filter
|
|
public JointPositionsFilter jointPositionFilter = null;
|
|
public BodySpinFilter bodySpinFilter = null;
|
|
|
|
|
|
// base depth sensor settings
|
|
[System.Serializable]
|
|
public class BaseSensorSettings
|
|
{
|
|
public int deviceStreamingMode;
|
|
public int deviceIndex;
|
|
public string recordingFile = string.Empty;
|
|
public float minDistance = 0.5f;
|
|
public float maxDistance = 10f;
|
|
}
|
|
|
|
|
|
protected virtual void Awake()
|
|
{
|
|
// init raw sensor pose
|
|
rawPosePosition = Vector3.zero;
|
|
rawPoseRotation = Quaternion.identity;
|
|
rawPoseTimestamp = (ulong)DateTime.Now.Ticks;
|
|
//Debug.Log($"{this.GetType()} - rawPosePos: {rawPosePosition}, rawPoseRot: {rawPoseRotation.eulerAngles}, ts: {rawPoseTimestamp}");
|
|
|
|
sensorPosePosition = transform.position;
|
|
sensorPoseRotation = transform.rotation;
|
|
|
|
// initial pose params
|
|
initialPosePosition = transform.position;
|
|
initialPoseRotation = transform.rotation;
|
|
|
|
matTransformPose.SetTRS(initialPosePosition, initialPoseRotation, Vector3.one);
|
|
}
|
|
|
|
|
|
|
|
public abstract KinectInterop.DepthSensorPlatform GetSensorPlatform();
|
|
|
|
//public virtual bool InitSensorInterface(bool bCopyLibs, ref bool bNeedRestart)
|
|
//{
|
|
// bNeedRestart = false;
|
|
// return true;
|
|
//}
|
|
|
|
//public virtual void FreeSensorInterface(bool bDeleteLibs)
|
|
//{
|
|
//}
|
|
|
|
public virtual string GetSensorDeviceId()
|
|
{
|
|
return sensorDeviceId;
|
|
}
|
|
|
|
|
|
public virtual System.Type GetSensorSettingsType()
|
|
{
|
|
return typeof(BaseSensorSettings);
|
|
}
|
|
|
|
|
|
public virtual BaseSensorSettings GetSensorSettings(BaseSensorSettings settings)
|
|
{
|
|
if (settings == null)
|
|
{
|
|
settings = new BaseSensorSettings();
|
|
}
|
|
|
|
settings.deviceStreamingMode = (int)deviceStreamingMode;
|
|
settings.deviceIndex = deviceIndex;
|
|
settings.recordingFile = recordingFile;
|
|
settings.minDistance = minDepthDistance;
|
|
settings.maxDistance = maxDepthDistance;
|
|
|
|
return settings;
|
|
}
|
|
|
|
|
|
public virtual void SetSensorSettings(BaseSensorSettings settings)
|
|
{
|
|
if (settings == null)
|
|
return;
|
|
|
|
deviceStreamingMode = (KinectInterop.DeviceStreamingMode)settings.deviceStreamingMode;
|
|
deviceIndex = settings.deviceIndex;
|
|
recordingFile = settings.recordingFile;
|
|
minDepthDistance = settings.minDistance;
|
|
maxDepthDistance = settings.maxDistance;
|
|
}
|
|
|
|
|
|
public abstract List<KinectInterop.SensorDeviceInfo> GetAvailableSensors();
|
|
|
|
public virtual KinectInterop.SensorData OpenSensor(KinectManager kinectManager, KinectInterop.FrameSource dwFlags, bool bSyncDepthAndColor, bool bSyncBodyAndDepth)
|
|
{
|
|
// save the parameters for later
|
|
frameSourceFlags = dwFlags;
|
|
isSyncDepthAndColor = bSyncDepthAndColor && ((dwFlags & KinectInterop.FrameSource.TypeColor) != 0) && ((dwFlags & KinectInterop.FrameSource.TypeDepth) != 0);
|
|
isSyncBodyAndDepth = bSyncBodyAndDepth && ((dwFlags & (KinectInterop.FrameSource.TypeBody | KinectInterop.FrameSource.TypeBodyIndex)) != 0) && ((dwFlags & KinectInterop.FrameSource.TypeDepth) != 0);
|
|
|
|
consoleLogMessages = kinectManager ? kinectManager.consoleLogMessages : false;
|
|
|
|
if (kinectManager && kinectManager.jointPositionSmoothing != SmoothingType.None)
|
|
{
|
|
// joint position filter
|
|
jointPositionFilter = new JointPositionsFilter();
|
|
jointPositionFilter.Init(kinectManager.jointPositionSmoothing);
|
|
}
|
|
|
|
if (kinectManager && kinectManager.bodySpinFilter != BodySpinType.None)
|
|
{
|
|
// body spin filter
|
|
bodySpinFilter = new BodySpinFilter(kinectManager.bodySpinFilter);
|
|
}
|
|
|
|
if ((dwFlags & KinectInterop.FrameSource.TypePose) != 0)
|
|
{
|
|
// clear sensor pose
|
|
SetSensorToWorldMatrix(Vector3.zero, Quaternion.identity, true);
|
|
}
|
|
|
|
return null;
|
|
}
|
|
|
|
|
|
public virtual void CloseSensor(KinectInterop.SensorData sensorData)
|
|
{
|
|
// clear basic timestamps
|
|
rawColorTimestamp = rawDepthTimestamp = rawInfraredTimestamp = 0;
|
|
rawBodyTimestamp = rawBodyIndexTimestamp = 0;
|
|
rawPoseTimestamp = 0;
|
|
|
|
// stop body tracking, if needed
|
|
StopBodyTracking(sensorData);
|
|
|
|
// dispose coord mapping shaders
|
|
DisposePointCloudVertexShader(sensorData);
|
|
DisposePointCloudColorShader(sensorData);
|
|
|
|
DisposeColorDepthShader(sensorData);
|
|
DisposeColorInfraredShader(sensorData);
|
|
DisposeColorBodyIndexShader(sensorData);
|
|
|
|
DisposeDepthTexShader(sensorData);
|
|
DisposeInfraredTexShader(sensorData);
|
|
}
|
|
|
|
|
|
public virtual void EnablePoseStream(KinectInterop.SensorData sensorData, bool bEnable)
|
|
{
|
|
if (bEnable)
|
|
{
|
|
frameSourceFlags |= KinectInterop.FrameSource.TypePose;
|
|
|
|
// clear sensor pose
|
|
SetSensorToWorldMatrix(Vector3.zero, Quaternion.identity, true);
|
|
}
|
|
else
|
|
{
|
|
frameSourceFlags &= ~KinectInterop.FrameSource.TypePose;
|
|
}
|
|
}
|
|
|
|
|
|
public virtual bool EnableSensorSync(KinectInterop.SensorData sensorData, bool bEnable)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
public virtual bool IsSensorMaster()
|
|
{
|
|
return false;
|
|
}
|
|
|
|
public virtual bool IsSensorFrameSynched(ulong frameTime, ulong masterTime)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
|
|
public virtual float GetMinInfraredValue()
|
|
{
|
|
return minInfraredValue;
|
|
}
|
|
|
|
public virtual float GetMaxInfraredValue()
|
|
{
|
|
return maxInfraredValue;
|
|
}
|
|
|
|
public virtual void SetMinMaxInfraredValues(float minValue, float maxValue)
|
|
{
|
|
minInfraredValue = minValue;
|
|
maxInfraredValue = maxValue;
|
|
}
|
|
|
|
|
|
public virtual bool IsSensorDataValid()
|
|
{
|
|
return true;
|
|
}
|
|
|
|
public virtual float GetBodyTrackerOrientationAngle()
|
|
{
|
|
return 0f;
|
|
}
|
|
|
|
public virtual void InitSensorData(KinectInterop.SensorData sensorData, KinectManager kinectManager)
|
|
{
|
|
//if (sensorData.depthImage != null)
|
|
//{
|
|
// depthImageBufferData = new int[sensorData.depthImage.Length];
|
|
//}
|
|
|
|
// depth image data
|
|
if (kinectManager.getDepthFrames == KinectManager.DepthTextureType.DepthTexture)
|
|
{
|
|
depthHistBufferData = new int[MAX_DEPTH_DISTANCE_MM + 1];
|
|
equalHistBufferData = new int[MAX_DEPTH_DISTANCE_MM + 1];
|
|
//sensorData.depthHistBufferData = new int[equalHistBufferData.Length];
|
|
}
|
|
else
|
|
{
|
|
depthHistBufferData = null;
|
|
equalHistBufferData = null;
|
|
//sensorData.depthHistBufferData = null;
|
|
}
|
|
|
|
// body image data
|
|
if (kinectManager.getBodyFrames == KinectManager.BodyTextureType.UserTexture)
|
|
{
|
|
depthBodyBufferData = new int[MAX_DEPTH_DISTANCE_MM + 1];
|
|
equalBodyBufferData = new int[MAX_DEPTH_DISTANCE_MM + 1];
|
|
//sensorData.bodyHistBufferData = new int[equalBodyBufferData.Length];
|
|
}
|
|
else
|
|
{
|
|
depthBodyBufferData = null;
|
|
equalBodyBufferData = null;
|
|
//sensorData.bodyHistBufferData = null;
|
|
}
|
|
|
|
lock (bodyTrackerLock)
|
|
{
|
|
// save the needed KM settings
|
|
bIgnoreZCoordinates = kinectManager.ignoreZCoordinates;
|
|
bIgnoreInferredJoints = kinectManager.ignoreInferredJoints;
|
|
}
|
|
}
|
|
|
|
public virtual void PollSensorFrames(KinectInterop.SensorData sensorData)
|
|
{
|
|
}
|
|
|
|
|
|
public virtual void PollCoordTransformFrames(KinectInterop.SensorData sensorData)
|
|
{
|
|
}
|
|
|
|
|
|
public virtual void PollSensorFrameTimes(KinectInterop.SensorData sensorData)
|
|
{
|
|
//// depth-image data
|
|
//if (lastDepthImageTimestamp != rawDepthTimestamp && rawDepthImage != null && depthHistBufferData != null)
|
|
//{
|
|
// lock (depthImageDataLock)
|
|
// {
|
|
// Array.Clear(depthHistBufferData, 0, depthHistBufferData.Length);
|
|
// Array.Clear(equalHistBufferData, 0, equalHistBufferData.Length);
|
|
// histDataTotalPoints = 0;
|
|
|
|
// //int depthMinDistance = (int)(minDistance * 1000f);
|
|
// //int depthMaxDistance = (int)(maxDistance * 1000f);
|
|
|
|
// for (int i = 0; i < rawDepthImage.Length; i++)
|
|
// {
|
|
// int depth = rawDepthImage[i];
|
|
// int limDepth = (depth <= MAX_DEPTH_DISTANCE_MM) ? depth : 0;
|
|
|
|
// if (limDepth > 0)
|
|
// {
|
|
// depthHistBufferData[limDepth]++;
|
|
// histDataTotalPoints++;
|
|
// }
|
|
// }
|
|
|
|
// equalHistBufferData[0] = depthHistBufferData[0];
|
|
// for (int i = 1; i < depthHistBufferData.Length; i++)
|
|
// {
|
|
// equalHistBufferData[i] = equalHistBufferData[i - 1] + depthHistBufferData[i];
|
|
// }
|
|
|
|
// // make depth 0 equal to the max-depth
|
|
// equalHistBufferData[0] = equalHistBufferData[equalHistBufferData.Length - 1];
|
|
|
|
// lastDepthImageTimestamp = rawDepthTimestamp;
|
|
// //Debug.Log("lastDepthImageTimestamp: " + lastDepthImageTimestamp);
|
|
// }
|
|
//}
|
|
|
|
//// body-image data
|
|
//if (lastBodyImageTimestamp != rawBodyIndexTimestamp && rawDepthImage != null && depthBodyBufferData != null)
|
|
//{
|
|
// lock (bodyImageDataLock)
|
|
// {
|
|
// Array.Clear(depthBodyBufferData, 0, depthBodyBufferData.Length);
|
|
// Array.Clear(equalBodyBufferData, 0, equalBodyBufferData.Length);
|
|
// histBodyTotalPoints = 0;
|
|
|
|
// int depthMinDistance = (int)(minDistance * 1000f);
|
|
// int depthMaxDistance = (int)(maxDistance * 1000f);
|
|
|
|
// for (int i = 0; i < rawDepthImage.Length; i++)
|
|
// {
|
|
// int depth = rawDepthImage[i];
|
|
// //int limDepth = (depth <= MAX_DEPTH_DISTANCE_MM) ? depth : 0;
|
|
// int limDepth = (depth >= depthMinDistance && depth <= depthMaxDistance) ? depth : 0;
|
|
|
|
// if (/**rawBodyIndexImage[i] != 255 &&*/ limDepth > 0)
|
|
// {
|
|
// depthBodyBufferData[limDepth]++;
|
|
// histBodyTotalPoints++;
|
|
// }
|
|
// }
|
|
|
|
// if(histBodyTotalPoints > 0)
|
|
// {
|
|
// equalBodyBufferData[0] = depthBodyBufferData[0];
|
|
// for (int i = 1; i < depthBodyBufferData.Length; i++)
|
|
// {
|
|
// equalBodyBufferData[i] = equalBodyBufferData[i - 1] + depthBodyBufferData[i];
|
|
// }
|
|
// }
|
|
|
|
// lastBodyImageTimestamp = rawBodyIndexTimestamp;
|
|
// //Debug.Log("lastBodyImageTimestamp: " + lastBodyImageTimestamp);
|
|
// }
|
|
//}
|
|
|
|
// ...
|
|
|
|
|
|
// set the frame timestamps
|
|
if (currentColorTimestamp != rawColorTimestamp)
|
|
{
|
|
// new color frame
|
|
currentColorTimestamp = rawColorTimestamp;
|
|
}
|
|
|
|
if (currentDepthTimestamp != rawDepthTimestamp)
|
|
{
|
|
// new depth frame
|
|
currentDepthTimestamp = rawDepthTimestamp;
|
|
}
|
|
|
|
if (currentInfraredTimestamp != rawInfraredTimestamp)
|
|
{
|
|
// new depth frame
|
|
currentInfraredTimestamp = rawInfraredTimestamp;
|
|
}
|
|
|
|
if (currentPoseTimestamp != rawPoseTimestamp)
|
|
{
|
|
// new pose frame
|
|
currentPoseTimestamp = rawPoseTimestamp;
|
|
}
|
|
|
|
if (currentBodyTimestamp != rawBodyTimestamp)
|
|
{
|
|
// new body frame
|
|
currentBodyTimestamp = rawBodyTimestamp;
|
|
}
|
|
|
|
if (currentBodyIndexTimestamp != rawBodyIndexTimestamp)
|
|
{
|
|
// new body index frame
|
|
currentBodyIndexTimestamp = rawBodyIndexTimestamp;
|
|
}
|
|
}
|
|
|
|
|
|
public virtual bool UpdateSensorData(KinectInterop.SensorData sensorData, KinectManager kinectManager, bool isPlayMode)
|
|
{
|
|
// color frame
|
|
if (rawColorImage != null && sensorData.lastColorFrameTime != currentColorTimestamp && !isPlayMode)
|
|
{
|
|
lock (colorFrameLock)
|
|
{
|
|
Texture2D colorImageTex2D = sensorData.colorImageTexture as Texture2D;
|
|
if (colorImageTex2D != null)
|
|
{
|
|
colorImageTex2D.LoadRawTextureData(rawColorImage);
|
|
colorImageTex2D.Apply();
|
|
}
|
|
|
|
sensorData.lastColorFrameTime = currentColorTimestamp;
|
|
//Debug.Log("D" + deviceIndex + " UpdateColorTimestamp: " + currentColorTimestamp + ", Now: " + DateTime.Now.ToString("HH:mm:ss.fff"));
|
|
}
|
|
}
|
|
|
|
// depth frame
|
|
if (rawDepthImage != null && sensorData.lastDepthFrameTime != currentDepthTimestamp && !isPlayMode)
|
|
{
|
|
lock (depthFrameLock)
|
|
{
|
|
// depth image
|
|
if (sensorData.depthImage != null)
|
|
{
|
|
//Buffer.BlockCopy(rawDepthImage, 0, sensorData.depthImage, 0, rawDepthImage.Length * sizeof(ushort));
|
|
KinectInterop.CopyBytes(rawDepthImage, sizeof(ushort), sensorData.depthImage, sizeof(ushort));
|
|
}
|
|
|
|
sensorData.lastDepthFrameTime = currentDepthTimestamp;
|
|
//Debug.Log("D" + deviceIndex + " UpdateDepthTimestamp: " + currentDepthTimestamp + ", Now: " + DateTime.Now.ToString("HH:mm:ss.fff"));
|
|
}
|
|
}
|
|
|
|
//// depth hist frame
|
|
//if (equalHistBufferData != null && sensorData.lastDepthHistTime != lastDepthImageTimestamp && !isPlayMode)
|
|
//{
|
|
// lock (depthImageDataLock)
|
|
// {
|
|
// if (sensorData.depthHistBufferData != null)
|
|
// {
|
|
// KinectInterop.CopyBytes(equalHistBufferData, sizeof(int), sensorData.depthHistBufferData, sizeof(int));
|
|
// }
|
|
|
|
// sensorData.depthHistTotalPoints = histDataTotalPoints;
|
|
// sensorData.lastDepthHistTime = lastDepthImageTimestamp;
|
|
// //Debug.Log("D" + deviceIndex + " UpdateDepthHistTimestamp: " + lastDepthImageTimestamp);
|
|
// }
|
|
//}
|
|
|
|
// infrared frame
|
|
if (rawInfraredImage != null && sensorData.lastInfraredFrameTime != currentInfraredTimestamp && !isPlayMode)
|
|
{
|
|
lock (infraredFrameLock)
|
|
{
|
|
if (sensorData.infraredImage != null)
|
|
{
|
|
//Buffer.BlockCopy(rawInfraredImage, 0, sensorData.infraredImage, 0, rawInfraredImage.Length * sizeof(ushort));
|
|
KinectInterop.CopyBytes(rawInfraredImage, sizeof(ushort), sensorData.infraredImage, sizeof(ushort));
|
|
}
|
|
|
|
sensorData.lastInfraredFrameTime = currentInfraredTimestamp;
|
|
//Debug.Log("D" + deviceIndex + " UpdateInfraredTimestamp: " + currentInfraredTimestamp + ", Now: " + DateTime.Now.ToString("HH:mm:ss.fff"));
|
|
}
|
|
}
|
|
|
|
// save the current pose frame time
|
|
ulong lastSensorPoseFrameTime = sensorData.lastSensorPoseFrameTime;
|
|
|
|
// pose frame
|
|
if (sensorData.lastSensorPoseFrameTime != currentPoseTimestamp && !isPlayMode)
|
|
{
|
|
lock (poseFrameLock)
|
|
{
|
|
Quaternion localPoseRot = rawPoseRotation;
|
|
if (sensorRotIgnoreY)
|
|
{
|
|
Vector3 localPoseRotEuler = localPoseRot.eulerAngles;
|
|
localPoseRotEuler.y = sensorRotValueY;
|
|
localPoseRot = Quaternion.Euler(localPoseRotEuler);
|
|
}
|
|
|
|
Quaternion corrPoseRotation = Quaternion.Euler(sensorRotOffset) * localPoseRot;
|
|
if(sensorRotFlipZ)
|
|
{
|
|
Vector3 corrPoseRotEuler = corrPoseRotation.eulerAngles;
|
|
corrPoseRotEuler.z = -corrPoseRotEuler.z;
|
|
corrPoseRotation = Quaternion.Euler(corrPoseRotEuler);
|
|
}
|
|
|
|
matLocalPose.SetTRS(rawPosePosition, corrPoseRotation, Vector3.one);
|
|
Matrix4x4 matTransform = matTransformPose * matLocalPose;
|
|
|
|
sensorPosePosition = matTransform.GetColumn(3);
|
|
sensorPoseRotation = matTransform.rotation;
|
|
|
|
sensorData.sensorPosePosition = sensorPosePosition;
|
|
sensorData.sensorPoseRotation = sensorPoseRotation;
|
|
|
|
sensorData.lastSensorPoseFrameTime = currentPoseTimestamp;
|
|
//Debug.Log("D" + deviceIndex + " UpdatePoseTimestamp: " + currentPoseTimestamp + ", Now: " + DateTime.Now.ToString("HH:mm:ss.fff"));
|
|
}
|
|
}
|
|
|
|
// check if the pose data has changed
|
|
if (lastSensorPoseFrameTime != sensorData.lastSensorPoseFrameTime)
|
|
{
|
|
ApplySensorPoseUpdate(kinectManager);
|
|
}
|
|
|
|
// body frame
|
|
if(sensorData.lastBodyFrameTime != currentBodyTimestamp)
|
|
{
|
|
lock (bodyTrackerLock)
|
|
{
|
|
// number of bodies
|
|
sensorData.trackedBodiesCount = trackedBodiesCount;
|
|
|
|
// create the needed slots
|
|
if (sensorData.alTrackedBodies.Length < trackedBodiesCount)
|
|
{
|
|
//sensorData.alTrackedBodies.Add(new KinectInterop.BodyData((int)KinectInterop.JointType.Count));
|
|
Array.Resize<KinectInterop.BodyData>(ref sensorData.alTrackedBodies, (int)trackedBodiesCount);
|
|
|
|
for (int i = 0; i < trackedBodiesCount; i++)
|
|
{
|
|
sensorData.alTrackedBodies[i] = new KinectInterop.BodyData((int)KinectInterop.JointType.Count);
|
|
}
|
|
}
|
|
|
|
//alTrackedBodies.CopyTo(sensorData.alTrackedBodies);
|
|
for (int i = 0; i < trackedBodiesCount; i++)
|
|
{
|
|
//sensorData.alTrackedBodies[i] = alTrackedBodies[i];
|
|
//KinectInterop.CopyBytes<KinectInterop.BodyData>(alTrackedBodies[i], ref sensorData.alTrackedBodies[i]);
|
|
alTrackedBodies[i].CopyTo(ref sensorData.alTrackedBodies[i]);
|
|
|
|
//KinectInterop.BodyData bodyData = sensorData.alTrackedBodies[i];
|
|
//Debug.Log(" (U)User ID: " + bodyData.liTrackingID + ", body: " + i + ", bi: " + bodyData.iBodyIndex + ", pos: " + bodyData.joint[0].kinectPos + ", rot: " + bodyData.joint[0].normalRotation.eulerAngles);
|
|
}
|
|
|
|
sensorData.lastBodyFrameTime = currentBodyTimestamp;
|
|
//Debug.Log("D" + deviceIndex + " UpdateBodyTimestamp: " + currentBodyTimestamp + ", BodyCount: " + trackedBodiesCount + ", Now: " + DateTime.Now.Ticks /*.ToString("HH:mm:ss.fff")*/);
|
|
}
|
|
}
|
|
|
|
// body index frame
|
|
if (sensorData.lastBodyIndexFrameTime != currentBodyIndexTimestamp)
|
|
{
|
|
lock (bodyTrackerLock)
|
|
{
|
|
// body index image
|
|
if (rawBodyIndexImage != null && sensorData.bodyIndexImage != null)
|
|
{
|
|
KinectInterop.CopyBytes(rawBodyIndexImage, sizeof(byte), sensorData.bodyIndexImage, sizeof(byte));
|
|
}
|
|
|
|
sensorData.lastBodyIndexFrameTime = currentBodyIndexTimestamp;
|
|
//Debug.Log("D" + deviceIndex + " UpdateBodyIndexTimestamp: " + currentBodyIndexTimestamp + ", Now: " + DateTime.Now.ToString("HH:mm:ss.fff"));
|
|
}
|
|
}
|
|
|
|
//// body hist frame
|
|
//if (equalBodyBufferData != null && sensorData.lastBodyHistTime != lastBodyImageTimestamp && !isPlayMode)
|
|
//{
|
|
// lock (bodyImageDataLock)
|
|
// {
|
|
// if (sensorData.bodyHistBufferData != null)
|
|
// {
|
|
// KinectInterop.CopyBytes(equalBodyBufferData, sizeof(int), sensorData.bodyHistBufferData, sizeof(int));
|
|
// }
|
|
|
|
// sensorData.bodyHistTotalPoints = histBodyTotalPoints;
|
|
// sensorData.lastBodyHistTime = lastBodyImageTimestamp;
|
|
// //Debug.Log("D" + deviceIndex + " UpdateBodyHistTimestamp: " + lastBodyImageTimestamp);
|
|
// }
|
|
//}
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
// applies the sensor-pose update to transform position & rotation, if needed
|
|
protected void ApplySensorPoseUpdate(KinectManager kinectManager)
|
|
{
|
|
if (kinectManager.getPoseFrames != KinectManager.PoseUsageType.RawPoseData)
|
|
{
|
|
switch (kinectManager.getPoseFrames)
|
|
{
|
|
case KinectManager.PoseUsageType.DisplayInfo:
|
|
if (kinectManager.statusInfoText != null)
|
|
{
|
|
kinectManager.statusInfoText.text = string.Format("Sensor position: ({0:F2}, {1:F2}, {2:F2}), rotation: {3}",
|
|
sensorPosePosition.x, sensorPosePosition.y, sensorPosePosition.z, sensorPoseRotation.eulerAngles);
|
|
}
|
|
break;
|
|
|
|
case KinectManager.PoseUsageType.UpdateTransform:
|
|
transform.position = sensorPosePosition; // sensorData.sensorPosePosition;
|
|
transform.rotation = sensorPoseRotation; // sensorData.sensorPoseRotation;
|
|
//sensorData.sensorTransformUpdated = true;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
// returns the anchor position of the background raw image
|
|
public virtual Vector2 GetBackgroundImageAnchorPos(KinectInterop.SensorData sensorData)
|
|
{
|
|
return Vector2.zero;
|
|
}
|
|
|
|
|
|
// returns the point cloud texture resolution
|
|
public virtual Vector2Int GetPointCloudTexResolution(KinectInterop.SensorData sensorData)
|
|
{
|
|
Vector2Int texRes = Vector2Int.zero;
|
|
|
|
switch (pointCloudResolution)
|
|
{
|
|
case PointCloudResolution.DepthCameraResolution:
|
|
texRes = new Vector2Int(sensorData.depthImageWidth, sensorData.depthImageHeight);
|
|
break;
|
|
|
|
case PointCloudResolution.ColorCameraResolution:
|
|
texRes = new Vector2Int(sensorData.colorImageWidth, sensorData.colorImageHeight);
|
|
break;
|
|
}
|
|
|
|
if(texRes == Vector2Int.zero)
|
|
{
|
|
throw new Exception("Unsupported point cloud resolution: " + pointCloudResolution + " or the respective image is not available.");
|
|
}
|
|
|
|
return texRes;
|
|
}
|
|
|
|
|
|
// returns the list of point-cloud player indices, or null if the list is empty
|
|
public virtual int[] GetPointCloudPlayerIndices()
|
|
{
|
|
if (string.IsNullOrEmpty(pointCloudPlayerList))
|
|
return null;
|
|
|
|
string[] asPlayerIndices = pointCloudPlayerList.Split(",".ToCharArray());
|
|
int[] aiPlayerIndices = new int[asPlayerIndices.Length];
|
|
|
|
for(int i = 0; i < asPlayerIndices.Length; i++)
|
|
{
|
|
if (!int.TryParse(asPlayerIndices[i].Trim(), out aiPlayerIndices[i]))
|
|
aiPlayerIndices[i] = -1;
|
|
}
|
|
|
|
return aiPlayerIndices;
|
|
}
|
|
|
|
|
|
// creates the point-cloud vertex shader and its respective buffers, as needed
|
|
protected virtual bool CreatePointCloudVertexShader(KinectInterop.SensorData sensorData)
|
|
{
|
|
if (sensorData.depthCamIntr == null || sensorData.depthCamIntr.distType == KinectInterop.DistortionType.None)
|
|
return false;
|
|
|
|
pointCloudVertexRes = GetPointCloudTexResolution(sensorData);
|
|
pointCloudPlayerIndices = GetPointCloudPlayerIndices();
|
|
|
|
if (pointCloudVertexRT == null)
|
|
{
|
|
pointCloudVertexRT = KinectInterop.CreateRenderTexture(pointCloudVertexRT, pointCloudVertexRes.x, pointCloudVertexRes.y, RenderTextureFormat.ARGBHalf);
|
|
//pointCloudVertexRT.enableRandomWrite = true;
|
|
pointCloudVertexRT.Create();
|
|
}
|
|
|
|
if (pointCloudVertexShader == null)
|
|
{
|
|
if(pointCloudPlayerIndices == null)
|
|
pointCloudVertexShader = Resources.Load("PointCloudVertexShaderAll") as ComputeShader;
|
|
else
|
|
pointCloudVertexShader = Resources.Load("PointCloudBodyVertexShaderAll") as ComputeShader;
|
|
|
|
pointCloudVertexKernel = pointCloudVertexShader != null ? pointCloudVertexShader.FindKernel("BakeVertexTex") : -1;
|
|
}
|
|
|
|
if (pointCloudSpaceBuffer == null)
|
|
{
|
|
int spaceBufferLength = pointCloudVertexRes.x * pointCloudVertexRes.y * 3;
|
|
pointCloudSpaceBuffer = new ComputeBuffer(spaceBufferLength, sizeof(float));
|
|
|
|
// depth2space table
|
|
//Debug.Log("Started creating space tables...");
|
|
//float fTimeStart = Time.realtimeSinceStartup;
|
|
|
|
//int depthImageLength = pointCloudVertexRes.x * pointCloudVertexRes.y;
|
|
//Vector3[] depth2SpaceTable = new Vector3[depthImageLength];
|
|
|
|
//for (int dy = 0, di = 0; dy < pointCloudVertexRes.y; dy++)
|
|
//{
|
|
// for (int dx = 0; dx < pointCloudVertexRes.x; dx++, di++)
|
|
// {
|
|
// Vector2 depthPos = new Vector2(dx, dy);
|
|
// depth2SpaceTable[di] = pointCloudResolution == PointCloudResolution.ColorCameraResolution ?
|
|
// MapColorPointToSpaceCoords(sensorData, depthPos, 1000) : MapDepthPointToSpaceCoords(sensorData, depthPos, 1000);
|
|
// }
|
|
//}
|
|
|
|
depth2SpaceTable = pointCloudResolution == PointCloudResolution.ColorCameraResolution ?
|
|
GetColorCameraSpaceTable(sensorData) : GetDepthCameraSpaceTable(sensorData);
|
|
|
|
//// parallelize for gaining time
|
|
//System.Threading.Tasks.Parallel.For(0, pointCloudVertexRes.y, dy =>
|
|
//{
|
|
// int di = dy * pointCloudVertexRes.x;
|
|
|
|
// for (var dx = 0; dx < pointCloudVertexRes.x; dx++, di++)
|
|
// {
|
|
// Vector2 depthPos = new Vector2(dx, dy);
|
|
// depth2SpaceTable[di] = pointCloudResolution == PointCloudResolution.ColorCameraResolution ?
|
|
// MapColorPointToSpaceCoords(sensorData, depthPos, 1000) : MapDepthPointToSpaceCoords(sensorData, depthPos, 1000);
|
|
// }
|
|
//});
|
|
|
|
//Debug.Log("depth2SpaceTable: " + depth2SpaceTable);
|
|
pointCloudSpaceBuffer.SetData(depth2SpaceTable);
|
|
depth2SpaceTable = null;
|
|
//Debug.Log("Finished creating space tables in " + (Time.realtimeSinceStartup - fTimeStart) + "s");
|
|
}
|
|
|
|
if (pointCloudDepthBuffer == null)
|
|
{
|
|
int depthBufferLength = pointCloudVertexRes.x * pointCloudVertexRes.y / 2;
|
|
pointCloudDepthBuffer = new ComputeBuffer(depthBufferLength, sizeof(uint));
|
|
}
|
|
|
|
if (pointCloudResolution == PointCloudResolution.ColorCameraResolution && colorCamDepthDataFrame == null)
|
|
{
|
|
colorCamDepthDataFrame = new ushort[sensorData.colorImageWidth * sensorData.colorImageHeight];
|
|
}
|
|
|
|
if (pointCloudPlayerIndices != null)
|
|
{
|
|
// body index buffer
|
|
int biBufferLength = pointCloudVertexRes.x * pointCloudVertexRes.y / 4;
|
|
|
|
if (pointCloudResolution == PointCloudResolution.ColorCameraResolution && sensorData.colorBodyIndexBuffer == null)
|
|
sensorData.colorBodyIndexBuffer = new ComputeBuffer(biBufferLength, sizeof(uint));
|
|
else if(pointCloudResolution == PointCloudResolution.DepthCameraResolution && pointCloudBodyIndexBuffer == null)
|
|
pointCloudBodyIndexBuffer = new ComputeBuffer(biBufferLength, sizeof(uint));
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
// disposes the point-cloud vertex shader and its respective buffers
|
|
protected virtual void DisposePointCloudVertexShader(KinectInterop.SensorData sensorData)
|
|
{
|
|
if (pointCloudSpaceBuffer != null)
|
|
{
|
|
pointCloudSpaceBuffer.Dispose();
|
|
pointCloudSpaceBuffer = null;
|
|
}
|
|
|
|
if (pointCloudDepthBuffer != null)
|
|
{
|
|
pointCloudDepthBuffer.Dispose();
|
|
pointCloudDepthBuffer = null;
|
|
}
|
|
|
|
if (pointCloudResolution == PointCloudResolution.ColorCameraResolution && sensorData.colorBodyIndexBuffer != null)
|
|
{
|
|
sensorData.colorBodyIndexBuffer.Dispose();
|
|
sensorData.colorBodyIndexBuffer = null;
|
|
}
|
|
|
|
if (pointCloudBodyIndexBuffer != null)
|
|
{
|
|
pointCloudBodyIndexBuffer.Dispose();
|
|
pointCloudBodyIndexBuffer = null;
|
|
}
|
|
|
|
if (pointCloudCoordBuffer != null)
|
|
{
|
|
// K2 color camera resolution
|
|
pointCloudCoordBuffer.Dispose();
|
|
pointCloudCoordBuffer = null;
|
|
}
|
|
|
|
if (pointCloudVertexRT != null)
|
|
{
|
|
pointCloudVertexRT.Release();
|
|
pointCloudVertexRT = null;
|
|
}
|
|
|
|
if (colorCamDepthDataFrame != null)
|
|
{
|
|
colorCamDepthDataFrame = null;
|
|
}
|
|
|
|
if (colorCamDepthCoordFrame != null)
|
|
{
|
|
colorCamDepthCoordFrame = null;
|
|
}
|
|
|
|
if (pointCloudVertexShader != null)
|
|
{
|
|
pointCloudVertexShader = null;
|
|
}
|
|
}
|
|
|
|
|
|
// converts player indices to body indices. Returns true on successful conversion. Returns false, if the playerIndices array is null.
|
|
protected bool GetPointCloudBodyIndices(KinectInterop.SensorData sensorData, KinectManager kinectManager, int[] playerIndices, ref int[] bodyIndices)
|
|
{
|
|
if (playerIndices == null || kinectManager == null || kinectManager.userManager == null)
|
|
return false;
|
|
|
|
if(bodyIndices == null || bodyIndices.Length != playerIndices.Length)
|
|
{
|
|
bodyIndices = new int[playerIndices.Length];
|
|
}
|
|
|
|
if (!userBodyMergerInited && KinectManager.Instance != null && KinectManager.Instance.IsInitialized())
|
|
{
|
|
userBodyMerger = KinectManager.Instance.GetUserBodyMerger();
|
|
userBodyMergerInited = true;
|
|
}
|
|
|
|
for (int i = 0; i < playerIndices.Length; i++)
|
|
{
|
|
int iPlayer = playerIndices[i];
|
|
ulong userId = iPlayer >= 0 && iPlayer < KinectInterop.Constants.MaxBodyCount ? kinectManager.userManager.aUserIndexIds[iPlayer] : 0;
|
|
|
|
if(userBodyMerger != null && userId > 0)
|
|
{
|
|
userId = userBodyMerger.GetSensorTrackingId(sensorData.sensorIndex, userId);
|
|
}
|
|
|
|
bodyIndices[i] = -1;
|
|
for (int b = 0; b < sensorData.trackedBodiesCount; b++)
|
|
{
|
|
if (sensorData.alTrackedBodies[b].bIsTracked && sensorData.alTrackedBodies[b].liTrackingID == userId)
|
|
{
|
|
bodyIndices[i] = sensorData.alTrackedBodies[b].iBodyIndex;
|
|
break;
|
|
}
|
|
}
|
|
|
|
//Debug.Log("playerIndex: " + iPlayer + ", userId: " + userId + ", bodyIndex: " + bodyIndices[i]);
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
// updates the point-cloud vertex shader with the actual data
|
|
protected virtual bool UpdatePointCloudVertexShader(KinectInterop.SensorData sensorData, KinectManager kinectManager)
|
|
{
|
|
if (pointCloudVertexShader != null && pointCloudVertexRT != null &&
|
|
((sensorData.lastDepth2SpaceFrameTime != sensorData.lastDepthFrameTime) ||
|
|
(sensorData.lastDepth2SpaceFrameTime != lastColorCamDepthFrameTime)))
|
|
{
|
|
if (pointCloudResolution == PointCloudResolution.ColorCameraResolution)
|
|
{
|
|
if(colorCamDepthDataFrame != null)
|
|
{
|
|
lock (colorCamDepthFrameLock)
|
|
{
|
|
int bufLength = colorCamDepthDataFrame.Length >> 1;
|
|
if(pointCloudDepthBuffer == null || pointCloudDepthBuffer.count != bufLength)
|
|
{
|
|
pointCloudDepthBuffer = KinectInterop.CreateComputeBuffer(pointCloudDepthBuffer, bufLength, sizeof(uint));
|
|
//Debug.Log("Created pointCloudDepthBuffer with len: " + bufLength);
|
|
}
|
|
|
|
KinectInterop.SetComputeBufferData(pointCloudDepthBuffer, colorCamDepthDataFrame, bufLength, sizeof(uint));
|
|
}
|
|
}
|
|
|
|
sensorData.lastDepth2SpaceFrameTime = lastColorCamDepthFrameTime;
|
|
}
|
|
else
|
|
{
|
|
if(sensorData.depthImage != null)
|
|
{
|
|
int bufLength = sensorData.depthImage.Length >> 1;
|
|
if (pointCloudDepthBuffer == null || pointCloudDepthBuffer.count != bufLength)
|
|
{
|
|
pointCloudDepthBuffer = KinectInterop.CreateComputeBuffer(pointCloudDepthBuffer, bufLength, sizeof(uint));
|
|
//Debug.Log("Created pointCloudDepthBuffer with len: " + bufLength);
|
|
}
|
|
|
|
KinectInterop.SetComputeBufferData(pointCloudDepthBuffer, sensorData.depthImage, bufLength, sizeof(uint));
|
|
}
|
|
|
|
sensorData.lastDepth2SpaceFrameTime = sensorData.lastDepthFrameTime;
|
|
}
|
|
|
|
if (GetPointCloudBodyIndices(sensorData, kinectManager, pointCloudPlayerIndices, ref pointCloudBodyIndices))
|
|
{
|
|
// body index
|
|
if (pointCloudResolution == PointCloudResolution.ColorCameraResolution && sensorData.colorBodyIndexBuffer != null)
|
|
{
|
|
pointCloudVertexShader.SetBuffer(pointCloudVertexKernel, "BodyIndexMap", sensorData.colorBodyIndexBuffer);
|
|
}
|
|
else if (pointCloudResolution == PointCloudResolution.DepthCameraResolution && pointCloudBodyIndexBuffer != null && sensorData.bodyIndexImage != null)
|
|
{
|
|
int bufLength = sensorData.bodyIndexImage.Length >> 2;
|
|
if (pointCloudBodyIndexBuffer == null || pointCloudBodyIndexBuffer.count != bufLength)
|
|
{
|
|
pointCloudBodyIndexBuffer = KinectInterop.CreateComputeBuffer(pointCloudBodyIndexBuffer, bufLength, sizeof(uint));
|
|
//Debug.Log("Created pointCloudBodyIndexBuffer with len: " + bufLength);
|
|
}
|
|
|
|
KinectInterop.SetComputeBufferData(pointCloudBodyIndexBuffer, sensorData.bodyIndexImage, bufLength, 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));
|
|
}
|
|
|
|
if (pointCloudResolution == PointCloudResolution.ColorCameraResolution)
|
|
{
|
|
if (color2SpaceWidth != sensorData.colorImageWidth || color2SpaceHeight != sensorData.colorImageHeight)
|
|
{
|
|
depth2SpaceTable = GetColorCameraSpaceTable(sensorData);
|
|
pointCloudSpaceBuffer.SetData(depth2SpaceTable);
|
|
depth2SpaceTable = null;
|
|
|
|
//Debug.Log("Updated color-cam space table to w: " + color2SpaceWidth + "x" + color2SpaceHeight + ", cw: " + sensorData.colorImageWidth + ", ch: " + sensorData.colorImageHeight);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (depth2SpaceWidth != sensorData.depthImageWidth || depth2SpaceHeight != sensorData.depthImageHeight)
|
|
{
|
|
depth2SpaceTable = GetDepthCameraSpaceTable(sensorData);
|
|
pointCloudSpaceBuffer.SetData(depth2SpaceTable);
|
|
depth2SpaceTable = null;
|
|
|
|
//Debug.Log("Updated depth-cam space table to w: " + depth2SpaceWidth + "x" + depth2SpaceHeight + ", dw: " + sensorData.depthImageWidth + ", dh: " + sensorData.depthImageHeight);
|
|
}
|
|
}
|
|
|
|
pointCloudVertexRes = GetPointCloudTexResolution(sensorData);
|
|
if (pointCloudVertexRT == null || pointCloudVertexRT.width != pointCloudVertexRes.x || pointCloudVertexRT.height != pointCloudVertexRes.y)
|
|
{
|
|
pointCloudVertexRT = KinectInterop.CreateRenderTexture(pointCloudVertexRT, pointCloudVertexRes.x, pointCloudVertexRes.y, RenderTextureFormat.ARGBHalf);
|
|
pointCloudVertexRT.Create();
|
|
}
|
|
|
|
KinectInterop.SetComputeShaderInt2(pointCloudVertexShader, "PointCloudRes", pointCloudVertexRes.x, pointCloudVertexRes.y);
|
|
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.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 virtual bool CreatePointCloudColorShader(KinectInterop.SensorData sensorData)
|
|
{
|
|
//renderDepthAlignedColorTexture.enableRandomWrite = true;
|
|
|
|
//if (pointCloudColorRT == null)
|
|
//{
|
|
// pointCloudColorRT = new RenderTexture(sensorData.depthImageWidth, sensorData.depthImageHeight, 0, RenderTextureFormat.ARGB32);
|
|
// pointCloudColorRT.enableRandomWrite = true;
|
|
// pointCloudColorRT.Create();
|
|
//}
|
|
|
|
pointCloudColorRes = GetPointCloudTexResolution(sensorData);
|
|
|
|
if(pointCloudResolution == PointCloudResolution.DepthCameraResolution)
|
|
{
|
|
if (pointCloudAlignedColorTex == null)
|
|
{
|
|
pointCloudAlignedColorTex = new Texture2D(sensorData.depthImageWidth, sensorData.depthImageHeight, sensorData.colorImageFormat, false);
|
|
}
|
|
|
|
if (depthCamColorDataFrame == null)
|
|
{
|
|
depthCamColorDataFrame = new byte[sensorData.depthImageWidth * sensorData.depthImageHeight * sensorData.colorImageStride];
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
// disposes the point-cloud color shader and its respective buffers
|
|
protected virtual void DisposePointCloudColorShader(KinectInterop.SensorData sensorData)
|
|
{
|
|
if (pointCloudCoordBuffer != null)
|
|
{
|
|
// K2 depth camera resolution
|
|
pointCloudCoordBuffer.Dispose();
|
|
pointCloudCoordBuffer = null;
|
|
}
|
|
|
|
if (pointCloudColorRT)
|
|
{
|
|
pointCloudColorRT.Release();
|
|
pointCloudColorRT = null;
|
|
}
|
|
|
|
if (pointCloudAlignedColorTex != null)
|
|
{
|
|
Destroy(pointCloudAlignedColorTex);
|
|
pointCloudAlignedColorTex = null;
|
|
}
|
|
|
|
if (depthCamColorDataFrame != null)
|
|
{
|
|
depthCamColorDataFrame = null;
|
|
}
|
|
|
|
if (depthCamColorCoordFrame != null)
|
|
{
|
|
depthCamColorCoordFrame = null;
|
|
}
|
|
|
|
if (pointCloudColorShader != null)
|
|
{
|
|
pointCloudColorShader = null;
|
|
}
|
|
}
|
|
|
|
|
|
// updates the point-cloud color shader with the actual data
|
|
protected virtual bool UpdatePointCloudColorShader(KinectInterop.SensorData sensorData)
|
|
{
|
|
Texture texColor = null;
|
|
|
|
if (pointCloudResolution == PointCloudResolution.DepthCameraResolution)
|
|
{
|
|
if (pointCloudAlignedColorTex != null && depthCamColorDataFrame != null && sensorData.lastDepthCamColorFrameTime != lastDepthCamColorFrameTime)
|
|
{
|
|
lock (depthCamColorFrameLock)
|
|
{
|
|
sensorData.lastDepthCamColorFrameTime = lastDepthCamColorFrameTime;
|
|
|
|
((Texture2D)pointCloudAlignedColorTex).LoadRawTextureData(depthCamColorDataFrame);
|
|
((Texture2D)pointCloudAlignedColorTex).Apply();
|
|
|
|
if(sensorData.depthCamColorImageTexture != null)
|
|
{
|
|
Graphics.CopyTexture(pointCloudAlignedColorTex, sensorData.depthCamColorImageTexture);
|
|
}
|
|
}
|
|
|
|
if (pointCloudColorRT != null)
|
|
{
|
|
Graphics.CopyTexture(pointCloudAlignedColorTex, pointCloudColorRT);
|
|
}
|
|
|
|
texColor = pointCloudAlignedColorTex;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
texColor = sensorData.colorImageTexture;
|
|
}
|
|
|
|
if(texColor != null)
|
|
{
|
|
if(pointCloudColorTexture != null)
|
|
{
|
|
Graphics.Blit(texColor, pointCloudColorTexture);
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
|
|
// creates the color-depth shader and its respective buffers, as needed
|
|
protected virtual bool CreateColorDepthShader(KinectInterop.SensorData sensorData)
|
|
{
|
|
if (colorCamDepthDataFrame == null)
|
|
{
|
|
colorCamDepthDataFrame = new ushort[sensorData.colorImageWidth * sensorData.colorImageHeight];
|
|
}
|
|
|
|
if (sensorData.colorDepthTexture == null)
|
|
{
|
|
sensorData.colorDepthTexture = KinectInterop.CreateRenderTexture(sensorData.colorDepthTexture, sensorData.colorImageWidth, sensorData.colorImageHeight, RenderTextureFormat.ARGB32);
|
|
//sensorData.colorDepthTexture.enableRandomWrite = true;
|
|
sensorData.colorDepthTexture.Create();
|
|
}
|
|
|
|
colorDepthShaderInited = true;
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
// disposes the color-depth shader and its respective buffers
|
|
protected virtual void DisposeColorDepthShader(KinectInterop.SensorData sensorData)
|
|
{
|
|
if (colorCamDepthDataFrame != null)
|
|
{
|
|
colorCamDepthDataFrame = null;
|
|
}
|
|
|
|
if (sensorData.colorDepthTexture != null)
|
|
{
|
|
sensorData.colorDepthTexture.Release();
|
|
sensorData.colorDepthTexture = null;
|
|
}
|
|
|
|
if (pointCloudDepthBuffer != null)
|
|
{
|
|
pointCloudDepthBuffer.Dispose();
|
|
pointCloudDepthBuffer = null;
|
|
}
|
|
|
|
if (pointCloudCoordBuffer != null)
|
|
{
|
|
pointCloudCoordBuffer.Dispose();
|
|
pointCloudCoordBuffer = null;
|
|
}
|
|
|
|
if (colorCamDepthCoordFrame != null)
|
|
{
|
|
colorCamDepthCoordFrame = null;
|
|
}
|
|
|
|
if (colorDepthShader != null)
|
|
{
|
|
colorDepthShader = null;
|
|
}
|
|
|
|
colorDepthShaderInited = false;
|
|
}
|
|
|
|
|
|
// updates the color-depth shader with the actual data
|
|
protected virtual bool UpdateColorDepthShader(KinectInterop.SensorData sensorData)
|
|
{
|
|
if (colorCamDepthDataFrame != null)
|
|
{
|
|
if(/**sensorData.usedColorDepthBufferTime == sensorData.lastColorDepthBufferTime && */
|
|
sensorData.lastColorDepthBufferTime != lastColorCamDepthFrameTime)
|
|
{
|
|
lock(colorCamDepthFrameLock)
|
|
{
|
|
sensorData.lastColorDepthBufferTime = lastColorCamDepthFrameTime;
|
|
|
|
if (sensorData.colorImageTexture != null)
|
|
{
|
|
if(sensorData.colorDepthTexture == null || sensorData.colorDepthTexture.width != sensorData.colorImageWidth || sensorData.colorDepthTexture.height != sensorData.colorImageHeight)
|
|
{
|
|
sensorData.colorDepthTexture = KinectInterop.CreateRenderTexture(sensorData.colorDepthTexture, sensorData.colorImageWidth, sensorData.colorImageHeight, RenderTextureFormat.ARGB32);
|
|
sensorData.colorDepthTexture.Create();
|
|
}
|
|
|
|
Graphics.Blit(sensorData.colorImageTexture, sensorData.colorDepthTexture);
|
|
}
|
|
|
|
if(sensorData.colorDepthBuffer != null)
|
|
{
|
|
int bufferLength = sensorData.colorImageWidth * sensorData.colorImageHeight >> 1;
|
|
if(sensorData.colorDepthBuffer.count != bufferLength)
|
|
{
|
|
sensorData.colorDepthBuffer = KinectInterop.CreateComputeBuffer(sensorData.colorDepthBuffer, bufferLength, sizeof(uint));
|
|
}
|
|
|
|
KinectInterop.SetComputeBufferData(sensorData.colorDepthBuffer, colorCamDepthDataFrame, bufferLength, sizeof(uint));
|
|
}
|
|
|
|
if(sensorData.colorCamDepthImage != null)
|
|
{
|
|
if(sensorData.colorCamDepthImage.Length != colorCamDepthDataFrame.Length)
|
|
{
|
|
sensorData.colorCamDepthImage = new ushort[colorCamDepthDataFrame.Length];
|
|
}
|
|
|
|
KinectInterop.CopyBytes(colorCamDepthDataFrame, sizeof(ushort), sensorData.colorCamDepthImage, sizeof(ushort));
|
|
sensorData.lastColorCamDepthFrameTime = lastColorCamDepthFrameTime;
|
|
}
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
|
|
// creates the color-infrared shader and its respective buffers, as needed
|
|
protected virtual bool CreateColorInfraredShader(KinectInterop.SensorData sensorData)
|
|
{
|
|
if (colorCamInfraredDataFrame == null)
|
|
{
|
|
colorCamInfraredDataFrame = new ushort[sensorData.colorImageWidth * sensorData.colorImageHeight];
|
|
}
|
|
|
|
if (sensorData.colorInfraredTexture == null)
|
|
{
|
|
sensorData.colorInfraredTexture = KinectInterop.CreateRenderTexture(sensorData.colorInfraredTexture, sensorData.colorImageWidth, sensorData.colorImageHeight, 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;
|
|
}
|
|
|
|
|
|
// disposes the color-infrared shader and its respective buffers
|
|
protected virtual void DisposeColorInfraredShader(KinectInterop.SensorData sensorData)
|
|
{
|
|
if (colorCamInfraredDataFrame != null)
|
|
{
|
|
colorCamInfraredDataFrame = null;
|
|
}
|
|
|
|
if (sensorData.colorInfraredTexture != null)
|
|
{
|
|
sensorData.colorInfraredTexture.Release();
|
|
sensorData.colorInfraredTexture = null;
|
|
}
|
|
|
|
if(sensorData.colorInfraredMaterial != null)
|
|
{
|
|
sensorData.colorInfraredMaterial = null;
|
|
}
|
|
|
|
if (pointCloudInfraredBuffer != null)
|
|
{
|
|
pointCloudInfraredBuffer.Dispose();
|
|
pointCloudInfraredBuffer = null;
|
|
}
|
|
|
|
if (pointCloudCoordBuffer != null)
|
|
{
|
|
pointCloudCoordBuffer.Dispose();
|
|
pointCloudCoordBuffer = null;
|
|
}
|
|
|
|
if (colorCamDepthCoordFrame != null)
|
|
{
|
|
colorCamDepthCoordFrame = null;
|
|
}
|
|
|
|
if (colorInfraredShader != null)
|
|
{
|
|
colorInfraredShader = null;
|
|
}
|
|
|
|
colorInfraredShaderInited = false;
|
|
}
|
|
|
|
|
|
// updates the color-infrared shader with the actual data
|
|
protected virtual bool UpdateColorInfraredShader(KinectInterop.SensorData sensorData)
|
|
{
|
|
if (colorCamInfraredDataFrame != null)
|
|
{
|
|
if (/**sensorData.usedColorInfraredBufferTime == sensorData.lastColorInfraredBufferTime && */
|
|
sensorData.lastColorInfraredBufferTime != lastColorCamInfraredFrameTime)
|
|
{
|
|
lock (colorCamInfraredFrameLock)
|
|
{
|
|
sensorData.lastColorInfraredBufferTime = lastColorCamInfraredFrameTime;
|
|
|
|
//if (sensorData.colorImageTexture != null)
|
|
//{
|
|
// Graphics.Blit(sensorData.colorImageTexture, sensorData.colorInfraredTexture);
|
|
//}
|
|
|
|
if (sensorData.colorInfraredBuffer != null)
|
|
{
|
|
int bufferLength = sensorData.colorImageWidth * sensorData.colorImageHeight >> 1;
|
|
if(sensorData.colorInfraredBuffer.count != bufferLength)
|
|
{
|
|
sensorData.colorInfraredBuffer = KinectInterop.CreateComputeBuffer(sensorData.colorInfraredBuffer, bufferLength, sizeof(uint));
|
|
}
|
|
|
|
KinectInterop.SetComputeBufferData(sensorData.colorInfraredBuffer, colorCamInfraredDataFrame, bufferLength, sizeof(uint));
|
|
}
|
|
|
|
if (sensorData.colorCamInfraredImage != null)
|
|
{
|
|
if(sensorData.colorCamInfraredImage.Length != colorCamInfraredDataFrame.Length)
|
|
{
|
|
sensorData.colorCamInfraredImage = new ushort[colorCamInfraredDataFrame.Length];
|
|
}
|
|
|
|
KinectInterop.CopyBytes(colorCamInfraredDataFrame, sizeof(ushort), sensorData.colorCamInfraredImage, sizeof(ushort));
|
|
sensorData.lastColorCamInfraredFrameTime = lastColorCamInfraredFrameTime;
|
|
}
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
|
|
// creates the color-camera body index shader and its respective buffers, as needed
|
|
protected virtual bool CreateColorBodyIndexShader(KinectInterop.SensorData sensorData)
|
|
{
|
|
if (colorCamBodyIndexFrame == null)
|
|
{
|
|
colorCamBodyIndexFrame = new byte[sensorData.colorImageWidth * sensorData.colorImageHeight];
|
|
}
|
|
|
|
if (sensorData.colorBodyIndexTexture == null)
|
|
{
|
|
sensorData.colorBodyIndexTexture = KinectInterop.CreateRenderTexture(sensorData.colorBodyIndexTexture, sensorData.colorImageWidth, sensorData.colorImageHeight, RenderTextureFormat.ARGB32);
|
|
//sensorData.colorBodyIndexTexture.enableRandomWrite = true;
|
|
sensorData.colorBodyIndexTexture.Create();
|
|
}
|
|
|
|
colorBodyIndexShaderInited = true;
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
// disposes the color-camera body index shader and its respective buffers
|
|
protected virtual void DisposeColorBodyIndexShader(KinectInterop.SensorData sensorData)
|
|
{
|
|
if (colorCamBodyIndexFrame != null)
|
|
{
|
|
colorCamBodyIndexFrame = null;
|
|
}
|
|
|
|
if (sensorData.colorBodyIndexTexture != null)
|
|
{
|
|
sensorData.colorBodyIndexTexture.Release();
|
|
sensorData.colorBodyIndexTexture = null;
|
|
}
|
|
|
|
if (bodyIndexBuffer != null)
|
|
{
|
|
bodyIndexBuffer.Dispose();
|
|
bodyIndexBuffer = null;
|
|
}
|
|
|
|
if (pointCloudCoordBuffer != null)
|
|
{
|
|
pointCloudCoordBuffer.Dispose();
|
|
pointCloudCoordBuffer = null;
|
|
}
|
|
|
|
if (colorCamDepthCoordFrame != null)
|
|
{
|
|
colorCamDepthCoordFrame = null;
|
|
}
|
|
|
|
if (colorBodyIndexShader != null)
|
|
{
|
|
colorBodyIndexShader = null;
|
|
}
|
|
|
|
colorBodyIndexShaderInited = false;
|
|
}
|
|
|
|
|
|
// updates the color-camera body index shader with the actual data
|
|
protected virtual bool UpdateColorBodyIndexShader(KinectInterop.SensorData sensorData)
|
|
{
|
|
if (colorCamBodyIndexFrame != null)
|
|
{
|
|
if (/**sensorData.usedColorBodyIndexBufferTime == sensorData.lastColorBodyIndexBufferTime && */
|
|
sensorData.lastColorBodyIndexBufferTime != lastColorCamBodyIndexFrameTime)
|
|
{
|
|
lock (colorCamBodyIndexFrameLock)
|
|
{
|
|
sensorData.lastColorBodyIndexBufferTime = lastColorCamBodyIndexFrameTime;
|
|
|
|
if (sensorData.colorImageTexture != null)
|
|
{
|
|
if(sensorData.colorBodyIndexTexture == null || sensorData.colorBodyIndexTexture.width != sensorData.colorImageWidth || sensorData.colorBodyIndexTexture.height != sensorData.colorImageHeight)
|
|
{
|
|
sensorData.colorBodyIndexTexture = KinectInterop.CreateRenderTexture(sensorData.colorBodyIndexTexture, sensorData.colorImageWidth, sensorData.colorImageHeight, RenderTextureFormat.ARGB32);
|
|
sensorData.colorBodyIndexTexture.Create();
|
|
}
|
|
|
|
Graphics.Blit(sensorData.colorImageTexture, sensorData.colorBodyIndexTexture);
|
|
}
|
|
|
|
if(sensorData.colorBodyIndexBuffer != null)
|
|
{
|
|
int bufferLength = sensorData.colorImageWidth * sensorData.colorImageHeight >> 2;
|
|
if(sensorData.colorBodyIndexBuffer.count != bufferLength)
|
|
{
|
|
sensorData.colorBodyIndexBuffer = KinectInterop.CreateComputeBuffer(sensorData.colorBodyIndexBuffer, bufferLength, sizeof(uint));
|
|
}
|
|
|
|
KinectInterop.SetComputeBufferData(sensorData.colorBodyIndexBuffer, colorCamBodyIndexFrame, bufferLength, sizeof(uint));
|
|
}
|
|
|
|
if (sensorData.colorCamBodyIndexImage != null)
|
|
{
|
|
if(sensorData.colorCamBodyIndexImage.Length != colorCamBodyIndexFrame.Length)
|
|
{
|
|
sensorData.colorCamBodyIndexImage = new byte[colorCamBodyIndexFrame.Length];
|
|
}
|
|
|
|
KinectInterop.CopyBytes(colorCamBodyIndexFrame, sizeof(byte), sensorData.colorCamBodyIndexImage, sizeof(byte));
|
|
sensorData.lastColorCamBodyIndexFrameTime = lastColorCamBodyIndexFrameTime;
|
|
}
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
return false;
|
|
}
|
|
|
|
|
|
// creates the depth-tex shader and its respective buffers, as needed
|
|
protected virtual bool CreateDepthTexShader(KinectInterop.SensorData sensorData)
|
|
{
|
|
Shader depthTexShader = Shader.Find("Kinect/DepthTexShader");
|
|
if (depthTexShader != null)
|
|
{
|
|
sensorData.depthTexMaterial = new Material(depthTexShader);
|
|
|
|
if (sensorData.depthImageBuffer == null)
|
|
{
|
|
int depthBufferLength = sensorData.depthImageWidth * sensorData.depthImageHeight / 2;
|
|
sensorData.depthImageBuffer = KinectInterop.CreateComputeBuffer(sensorData.depthImageBuffer, depthBufferLength, sizeof(uint));
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
// disposes the depth-tex shader and its respective buffers
|
|
protected virtual void DisposeDepthTexShader(KinectInterop.SensorData sensorData)
|
|
{
|
|
if (sensorData.depthTexTexture != null)
|
|
{
|
|
sensorData.depthTexTexture.Release();
|
|
sensorData.depthTexTexture = null;
|
|
}
|
|
|
|
if (sensorData.depthImageBuffer != null)
|
|
{
|
|
sensorData.depthImageBuffer.Dispose();
|
|
sensorData.depthImageBuffer = null;
|
|
}
|
|
|
|
sensorData.depthTexMaterial = null;
|
|
}
|
|
|
|
|
|
// creates the infrared-tex shader and its respective buffers, as needed
|
|
protected virtual bool CreateInfraredTexShader(KinectInterop.SensorData sensorData)
|
|
{
|
|
Shader infraredTexShader = Shader.Find("Kinect/DepthTexShader");
|
|
if (infraredTexShader != null)
|
|
{
|
|
sensorData.infraredTexMaterial = new Material(infraredTexShader);
|
|
|
|
if (sensorData.infraredImageBuffer == null)
|
|
{
|
|
int depthBufferLength = sensorData.depthImageWidth * sensorData.depthImageHeight / 2;
|
|
sensorData.infraredImageBuffer = KinectInterop.CreateComputeBuffer(sensorData.infraredImageBuffer, depthBufferLength, sizeof(uint));
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
// disposes the infrared-tex shader and its respective buffers
|
|
protected virtual void DisposeInfraredTexShader(KinectInterop.SensorData sensorData)
|
|
{
|
|
if (sensorData.infraredTexTexture != null)
|
|
{
|
|
sensorData.infraredTexTexture.Release();
|
|
sensorData.infraredTexTexture = null;
|
|
}
|
|
|
|
if (sensorData.infraredImageBuffer != null)
|
|
{
|
|
sensorData.infraredImageBuffer.Dispose();
|
|
sensorData.infraredImageBuffer = null;
|
|
}
|
|
|
|
sensorData.infraredTexMaterial = null;
|
|
}
|
|
|
|
|
|
// updates transformed frame textures, if needed
|
|
public virtual bool UpdateTransformedFrameTextures(KinectInterop.SensorData sensorData, KinectManager kinectManager)
|
|
{
|
|
// depth cam space frame
|
|
if (pointCloudVertexTexture != null)
|
|
{
|
|
if (pointCloudVertexShader != null || CreatePointCloudVertexShader(sensorData))
|
|
{
|
|
UpdatePointCloudVertexShader(sensorData, kinectManager);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (pointCloudVertexShader != null)
|
|
{
|
|
DisposePointCloudVertexShader(sensorData);
|
|
}
|
|
}
|
|
|
|
// depth cam color frame
|
|
if (pointCloudColorTexture != null || sensorData.depthCamColorImageTexture != null)
|
|
{
|
|
if (pointCloudColorShader != null || pointCloudAlignedColorTex != null || CreatePointCloudColorShader(sensorData))
|
|
{
|
|
UpdatePointCloudColorShader(sensorData);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (pointCloudColorShader != null || pointCloudAlignedColorTex != null)
|
|
{
|
|
DisposePointCloudColorShader(sensorData);
|
|
}
|
|
}
|
|
|
|
// color cam depth frame
|
|
if (sensorData.colorDepthBuffer != null || sensorData.colorCamDepthImage != null)
|
|
{
|
|
if(colorDepthShaderInited || CreateColorDepthShader(sensorData))
|
|
{
|
|
UpdateColorDepthShader(sensorData);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if(colorDepthShaderInited)
|
|
{
|
|
DisposeColorDepthShader(sensorData);
|
|
}
|
|
}
|
|
|
|
// color cam infrared frame
|
|
if (sensorData.colorInfraredBuffer != null || sensorData.colorCamInfraredImage != null)
|
|
{
|
|
if (colorInfraredShaderInited || CreateColorInfraredShader(sensorData))
|
|
{
|
|
UpdateColorInfraredShader(sensorData);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (colorInfraredShaderInited)
|
|
{
|
|
DisposeColorInfraredShader(sensorData);
|
|
}
|
|
}
|
|
|
|
// color cam body index frame
|
|
if (sensorData.colorBodyIndexBuffer != null || sensorData.colorCamBodyIndexImage != null)
|
|
{
|
|
if (colorBodyIndexShaderInited || CreateColorBodyIndexShader(sensorData))
|
|
{
|
|
UpdateColorBodyIndexShader(sensorData);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (colorBodyIndexShaderInited)
|
|
{
|
|
DisposeColorBodyIndexShader(sensorData);
|
|
}
|
|
}
|
|
|
|
// depth-tex
|
|
if (sensorData.depthTexTexture != null)
|
|
{
|
|
if (sensorData.depthTexMaterial != null || CreateDepthTexShader(sensorData))
|
|
{
|
|
//UpdateDepthTexShader(sensorData); // code moved to UpdateSensorTextures()
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (sensorData.depthTexMaterial != null)
|
|
{
|
|
DisposeDepthTexShader(sensorData);
|
|
}
|
|
}
|
|
|
|
// infrared-tex
|
|
if (sensorData.infraredTexTexture != null)
|
|
{
|
|
if (sensorData.infraredTexMaterial != null || CreateInfraredTexShader(sensorData))
|
|
{
|
|
//UpdateInfraredTexShader(sensorData); // code moved to UpdateSensorTextures()
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (sensorData.infraredTexMaterial != null)
|
|
{
|
|
DisposeInfraredTexShader(sensorData);
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
|
|
public virtual bool UpdateSensorTextures(KinectInterop.SensorData sensorData, KinectManager kinectManager, ulong prevDepthFrameTime, ulong prevIrFrameTime)
|
|
{
|
|
// check if the depth data has changed
|
|
if (prevDepthFrameTime != sensorData.lastDepthFrameTime || prevIrFrameTime != sensorData.lastInfraredFrameTime)
|
|
{
|
|
// depth texture
|
|
if (sensorData.depthImageTexture != null && sensorData.depthImageMaterial != null &&
|
|
sensorData.depthImage != null && sensorData.lastDepthImageTime != sensorData.lastDepthFrameTime)
|
|
{
|
|
UpdateSensorTexDepthImg(sensorData, kinectManager);
|
|
|
|
sensorData.lastDepthImageTime = sensorData.lastDepthFrameTime;
|
|
//Debug.Log("D" + deviceIndex + " DepthTextureTimestamp: " + sensorData.lastDepthImageTime);
|
|
}
|
|
|
|
// infrared texture
|
|
if (((sensorData.infraredImageTexture != null && sensorData.infraredImageMaterial != null) || (sensorData.colorInfraredTexture != null && sensorData.colorInfraredMaterial != null)) &&
|
|
sensorData.infraredImage != null && sensorData.lastInfraredImageTime != sensorData.lastInfraredFrameTime)
|
|
{
|
|
UpdateSensorTexInfraredImg(sensorData, kinectManager);
|
|
|
|
sensorData.lastInfraredImageTime = sensorData.lastInfraredFrameTime;
|
|
//Debug.Log("D" + deviceIndex + " InfraredTextureTimestamp: " + sensorData.lastInfraredImageTime);
|
|
}
|
|
|
|
// user texture & body texture
|
|
if (sensorData.bodyImageTexture != null && sensorData.bodyImageMaterial != null &&
|
|
sensorData.lastBodyImageTime != sensorData.lastBodyIndexFrameTime)
|
|
{
|
|
UpdateSensorTexBodyIndexImg(sensorData, kinectManager);
|
|
|
|
sensorData.lastBodyImageTime = sensorData.lastBodyIndexFrameTime;
|
|
//Debug.Log("D" + deviceIndex + " BodyTextureTimestamp: " + sensorData.lastBodyImageTime);
|
|
}
|
|
|
|
// depth-hsv
|
|
if (sensorData.depthTexMaterial != null && sensorData.depthImage != null &&
|
|
sensorData.lastDepthTexTime != sensorData.lastDepthFrameTime)
|
|
{
|
|
UpdateSensorTexDepthHsv(sensorData, kinectManager);
|
|
|
|
sensorData.lastDepthTexTime = sensorData.lastDepthFrameTime;
|
|
//Debug.Log("D" + deviceIndex + " DepthTexTimestamp: " + sensorData.lastDepthTexTime);
|
|
}
|
|
|
|
// infrared-hsv
|
|
if (sensorData.infraredTexMaterial != null && sensorData.infraredImage != null &&
|
|
sensorData.lastInfraredTexTime != sensorData.lastInfraredFrameTime)
|
|
{
|
|
UpdateSensorTexInfraredHsv(sensorData, kinectManager);
|
|
|
|
sensorData.lastInfraredTexTime = sensorData.lastInfraredFrameTime;
|
|
//Debug.Log("D" + deviceIndex + " InfraredTexTimestamp: " + sensorData.lastInfraredTexTime);
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
// updates sensor depth image
|
|
private void UpdateSensorTexDepthImg(KinectInterop.SensorData sensorData, KinectManager kinectManager)
|
|
{
|
|
// depth-image hist data
|
|
Array.Clear(depthHistBufferData, 0, depthHistBufferData.Length);
|
|
Array.Clear(equalHistBufferData, 0, equalHistBufferData.Length);
|
|
depthHistTotalPoints = 0;
|
|
|
|
//int depthMinDistance = (int)(minDistance * 1000f);
|
|
//int depthMaxDistance = (int)(maxDistance * 1000f);
|
|
|
|
int frameLen = sensorData.depthImage.Length;
|
|
for (int i = 0; i < frameLen; i++)
|
|
{
|
|
int depth = sensorData.depthImage[i];
|
|
int limDepth = (depth <= MAX_DEPTH_DISTANCE_MM) ? depth : 0;
|
|
|
|
if (limDepth > 0)
|
|
{
|
|
depthHistBufferData[limDepth]++;
|
|
depthHistTotalPoints++;
|
|
}
|
|
}
|
|
|
|
equalHistBufferData[0] = depthHistBufferData[0];
|
|
for (int i = 1; i < depthHistBufferData.Length; i++)
|
|
{
|
|
equalHistBufferData[i] = equalHistBufferData[i - 1] + depthHistBufferData[i];
|
|
}
|
|
|
|
// make depth 0 equal to the max-depth
|
|
equalHistBufferData[0] = equalHistBufferData[equalHistBufferData.Length - 1];
|
|
|
|
if (sensorData.depthImageBuffer != null && sensorData.depthImage != null)
|
|
{
|
|
int depthBufferLength = sensorData.depthImageWidth * sensorData.depthImageHeight / 2;
|
|
KinectInterop.SetComputeBufferData(sensorData.depthImageBuffer, sensorData.depthImage, depthBufferLength, sizeof(uint));
|
|
}
|
|
|
|
if (sensorData.depthHistBuffer != null)
|
|
{
|
|
//sensorData.depthHistBuffer.SetData(equalHistBufferData);
|
|
KinectInterop.SetComputeBufferData(sensorData.depthHistBuffer, equalHistBufferData, equalHistBufferData.Length, sizeof(int));
|
|
}
|
|
|
|
sensorData.depthImageMaterial.SetInt("_TexResX", sensorData.depthImageWidth);
|
|
sensorData.depthImageMaterial.SetInt("_TexResY", sensorData.depthImageHeight);
|
|
sensorData.depthImageMaterial.SetInt("_MinDepth", (int)(minDepthDistance * 1000f));
|
|
sensorData.depthImageMaterial.SetInt("_MaxDepth", (int)(maxDepthDistance * 1000f));
|
|
sensorData.depthImageMaterial.SetInt("_TotalPoints", depthHistTotalPoints);
|
|
sensorData.depthImageMaterial.SetBuffer("_DepthMap", sensorData.depthImageBuffer);
|
|
sensorData.depthImageMaterial.SetBuffer("_HistMap", sensorData.depthHistBuffer);
|
|
|
|
Graphics.Blit(null, sensorData.depthImageTexture, sensorData.depthImageMaterial);
|
|
//Debug.Log(" UpdateSensorTexDepthImg: " + depthHistTotalPoints + " pts");
|
|
}
|
|
|
|
// updates sensor infrared image
|
|
private void UpdateSensorTexInfraredImg(KinectInterop.SensorData sensorData, KinectManager kinectManager)
|
|
{
|
|
if(sensorData.infraredImageTexture != null && sensorData.infraredImageMaterial != null)
|
|
{
|
|
if (sensorData.infraredImageBuffer != null && sensorData.infraredImage != null)
|
|
{
|
|
int infraredBufferLength = sensorData.depthImageWidth * sensorData.depthImageHeight / 2;
|
|
KinectInterop.SetComputeBufferData(sensorData.infraredImageBuffer, sensorData.infraredImage, infraredBufferLength, sizeof(uint));
|
|
}
|
|
|
|
sensorData.infraredImageMaterial.SetInt("_TexResX", sensorData.depthImageWidth);
|
|
sensorData.infraredImageMaterial.SetInt("_TexResY", sensorData.depthImageHeight);
|
|
sensorData.infraredImageMaterial.SetFloat("_MinValue", minInfraredValue);
|
|
sensorData.infraredImageMaterial.SetFloat("_MaxValue", maxInfraredValue);
|
|
sensorData.infraredImageMaterial.SetBuffer("_InfraredMap", sensorData.infraredImageBuffer);
|
|
|
|
Graphics.Blit(null, sensorData.infraredImageTexture, sensorData.infraredImageMaterial);
|
|
}
|
|
|
|
if (sensorData.colorInfraredBuffer != null && sensorData.colorInfraredMaterial != null)
|
|
{
|
|
sensorData.colorInfraredMaterial.SetInt("_TexResX", sensorData.colorImageWidth);
|
|
sensorData.colorInfraredMaterial.SetInt("_TexResY", sensorData.colorImageHeight);
|
|
sensorData.colorInfraredMaterial.SetFloat("_MinValue", minInfraredValue);
|
|
sensorData.colorInfraredMaterial.SetFloat("_MaxValue", maxInfraredValue);
|
|
sensorData.colorInfraredMaterial.SetBuffer("_InfraredMap", sensorData.colorInfraredBuffer);
|
|
|
|
Graphics.Blit(null, sensorData.colorInfraredTexture, sensorData.colorInfraredMaterial);
|
|
}
|
|
}
|
|
|
|
// updates sensor body-index image
|
|
private void UpdateSensorTexBodyIndexImg(KinectInterop.SensorData sensorData, KinectManager kinectManager)
|
|
{
|
|
if (kinectManager.getBodyFrames == KinectManager.BodyTextureType.UserTexture)
|
|
{
|
|
// body-image hist data
|
|
Array.Clear(depthBodyBufferData, 0, depthBodyBufferData.Length);
|
|
Array.Clear(equalBodyBufferData, 0, equalBodyBufferData.Length);
|
|
bodyHistTotalPoints = 0;
|
|
|
|
int depthMinDistance = (int)(minDepthDistance * 1000f);
|
|
int depthMaxDistance = (int)(maxDepthDistance * 1000f);
|
|
|
|
int frameLen = sensorData.depthImage.Length;
|
|
for (int i = 0; i < frameLen; i++)
|
|
{
|
|
int depth = sensorData.depthImage[i];
|
|
//int limDepth = (depth <= MAX_DEPTH_DISTANCE_MM) ? depth : 0;
|
|
int limDepth = (depth >= depthMinDistance && depth <= depthMaxDistance) ? depth : 0;
|
|
|
|
if (/**rawBodyIndexImage[i] != 255 &&*/ limDepth > 0)
|
|
{
|
|
depthBodyBufferData[limDepth]++;
|
|
bodyHistTotalPoints++;
|
|
}
|
|
}
|
|
|
|
if (bodyHistTotalPoints > 0)
|
|
{
|
|
equalBodyBufferData[0] = depthBodyBufferData[0];
|
|
for (int i = 1; i < depthBodyBufferData.Length; i++)
|
|
{
|
|
equalBodyBufferData[i] = equalBodyBufferData[i - 1] + depthBodyBufferData[i];
|
|
}
|
|
}
|
|
|
|
if (sensorData.bodyHistBuffer != null)
|
|
{
|
|
//sensorData.depthHistBuffer.SetData(equalBodyBufferData);
|
|
KinectInterop.SetComputeBufferData(sensorData.bodyHistBuffer, equalBodyBufferData, equalBodyBufferData.Length, sizeof(int));
|
|
}
|
|
}
|
|
|
|
if (sensorData.bodyIndexBuffer != null && sensorData.bodyIndexImage != null)
|
|
{
|
|
int bodyIndexBufferLength = sensorData.depthImageWidth * sensorData.depthImageHeight / 4;
|
|
KinectInterop.SetComputeBufferData(sensorData.bodyIndexBuffer, sensorData.bodyIndexImage, bodyIndexBufferLength, sizeof(uint));
|
|
}
|
|
|
|
float minDist = minDepthDistance; // kinectManager.minUserDistance != 0f ? kinectManager.minUserDistance : minDepthDistance;
|
|
float maxDist = maxDepthDistance; // kinectManager.maxUserDistance != 0f ? kinectManager.maxUserDistance : maxDepthDistance;
|
|
|
|
sensorData.bodyImageMaterial.SetInt("_TexResX", sensorData.depthImageWidth);
|
|
sensorData.bodyImageMaterial.SetInt("_TexResY", sensorData.depthImageHeight);
|
|
sensorData.bodyImageMaterial.SetInt("_MinDepth", (int)(minDist * 1000f));
|
|
sensorData.bodyImageMaterial.SetInt("_MaxDepth", (int)(maxDist * 1000f));
|
|
|
|
sensorData.bodyImageMaterial.SetBuffer("_BodyIndexMap", sensorData.bodyIndexBuffer);
|
|
|
|
if (kinectManager.getBodyFrames == KinectManager.BodyTextureType.UserTexture)
|
|
{
|
|
if (sensorData.depthImageBuffer != null && sensorData.depthImage != null)
|
|
{
|
|
int depthBufferLength = sensorData.depthImageWidth * sensorData.depthImageHeight / 2;
|
|
KinectInterop.SetComputeBufferData(sensorData.depthImageBuffer, sensorData.depthImage, depthBufferLength, sizeof(uint));
|
|
}
|
|
|
|
sensorData.bodyImageMaterial.SetBuffer("_DepthMap", sensorData.depthImageBuffer);
|
|
sensorData.bodyImageMaterial.SetBuffer("_HistMap", sensorData.bodyHistBuffer);
|
|
sensorData.bodyImageMaterial.SetInt("_TotalPoints", bodyHistTotalPoints);
|
|
//sensorData.bodyImageMaterial.SetInt("_FirstUserIndex", kinectManager.GetBodyIndexByUserId(kinectManager.userManager.liPrimaryUserId));
|
|
|
|
Color[] bodyIndexColors = kinectManager.GetBodyIndexColors();
|
|
sensorData.bodyImageMaterial.SetColorArray("_BodyIndexColors", bodyIndexColors);
|
|
}
|
|
|
|
Graphics.Blit(null, sensorData.bodyImageTexture, sensorData.bodyImageMaterial);
|
|
}
|
|
|
|
// updates sensor depth-hsv image
|
|
private void UpdateSensorTexDepthHsv(KinectInterop.SensorData sensorData, KinectManager kinectManager)
|
|
{
|
|
if (sensorData.depthImageBuffer != null && sensorData.depthImage != null)
|
|
{
|
|
int depthBufferLength = sensorData.depthImageWidth * sensorData.depthImageHeight / 2;
|
|
KinectInterop.SetComputeBufferData(sensorData.depthImageBuffer, sensorData.depthImage, depthBufferLength, sizeof(uint));
|
|
}
|
|
|
|
sensorData.depthTexMaterial.SetBuffer("_DepthMap", sensorData.depthImageBuffer);
|
|
sensorData.depthTexMaterial.SetInt("_TexResX", sensorData.depthImageWidth);
|
|
sensorData.depthTexMaterial.SetInt("_TexResY", sensorData.depthImageHeight);
|
|
sensorData.depthTexMaterial.SetInt("_MinDepth", (int)(minDepthDistance * 1000f));
|
|
sensorData.depthTexMaterial.SetInt("_MaxDepth", (int)(maxDepthDistance * 1000f));
|
|
|
|
Graphics.Blit(null, sensorData.depthTexTexture, sensorData.depthTexMaterial);
|
|
}
|
|
|
|
// updates sensor infrared-hsv image
|
|
private void UpdateSensorTexInfraredHsv(KinectInterop.SensorData sensorData, KinectManager kinectManager)
|
|
{
|
|
if (sensorData.infraredImageBuffer != null && sensorData.infraredImage != null)
|
|
{
|
|
int depthBufferLength = sensorData.depthImageWidth * sensorData.depthImageHeight / 2;
|
|
KinectInterop.SetComputeBufferData(sensorData.infraredImageBuffer, sensorData.infraredImage, depthBufferLength, sizeof(uint));
|
|
}
|
|
|
|
sensorData.infraredTexMaterial.SetBuffer("_DepthMap", sensorData.infraredImageBuffer);
|
|
sensorData.infraredTexMaterial.SetInt("_TexResX", sensorData.depthImageWidth);
|
|
sensorData.infraredTexMaterial.SetInt("_TexResY", sensorData.depthImageHeight);
|
|
sensorData.infraredTexMaterial.SetInt("_MinDepth", (int)minInfraredValue);
|
|
sensorData.infraredTexMaterial.SetInt("_MaxDepth", (int)maxInfraredValue);
|
|
|
|
Graphics.Blit(null, sensorData.infraredTexTexture, sensorData.infraredTexMaterial);
|
|
}
|
|
|
|
|
|
// returns depth-to-color-camera matrix
|
|
public virtual Matrix4x4 GetDepthToColorCameraMatrix()
|
|
{
|
|
return depth2colorCamMat;
|
|
}
|
|
|
|
|
|
// returns sensor-to-world matrix
|
|
public virtual Matrix4x4 GetSensorToWorldMatrix()
|
|
{
|
|
Matrix4x4 mSensor = Matrix4x4.identity;
|
|
mSensor.SetTRS(sensorPosePosition, sensorPoseRotation, Vector3.one);
|
|
|
|
return mSensor;
|
|
}
|
|
|
|
|
|
// sets sensor-to-world matrix
|
|
public virtual void SetSensorToWorldMatrix(Matrix4x4 mSensor2World, bool isUpdateTransform)
|
|
{
|
|
Vector3 sensorWorldPosition = mSensor2World.GetColumn(3);
|
|
Quaternion sensorWorldRotation = mSensor2World.rotation;
|
|
|
|
SetSensorToWorldMatrix(sensorWorldPosition, sensorWorldRotation, isUpdateTransform);
|
|
}
|
|
|
|
// sets sensor-to-world matrix
|
|
public virtual void SetSensorToWorldMatrix(Vector3 sensorWorldPosition, Quaternion sensorWorldRotation, bool isUpdateTransform)
|
|
{
|
|
initialPosePosition = sensorPosePosition = sensorWorldPosition;
|
|
initialPoseRotation = sensorPoseRotation = sensorWorldRotation;
|
|
matTransformPose.SetTRS(initialPosePosition, initialPoseRotation, Vector3.one);
|
|
|
|
if (isUpdateTransform)
|
|
{
|
|
transform.position = sensorPosePosition;
|
|
transform.rotation = sensorPoseRotation;
|
|
}
|
|
}
|
|
|
|
|
|
// returns sensor rotation, properly adjusted for body tracking
|
|
protected Quaternion GetSensorRotationNotZFlipped(bool bInverted)
|
|
{
|
|
Vector3 sensorRotEuler = sensorPoseRotation.eulerAngles;
|
|
|
|
if (sensorRotFlipZ)
|
|
{
|
|
sensorRotEuler.z = -sensorRotEuler.z;
|
|
}
|
|
|
|
Quaternion sensorRot = Quaternion.Euler(sensorRotEuler);
|
|
return bInverted ? Quaternion.Inverse(sensorRot) : sensorRot;
|
|
}
|
|
|
|
|
|
// returns sensor transform. Please note transform updates depend on the getPoseFrames-KM setting.
|
|
public virtual Transform GetSensorTransform()
|
|
{
|
|
return transform;
|
|
}
|
|
|
|
|
|
// unprojects plane point into the space
|
|
public virtual Vector3 UnprojectPoint(KinectInterop.CameraIntrinsics intr, Vector2 pixel, float depth)
|
|
{
|
|
return Vector3.zero;
|
|
}
|
|
|
|
|
|
// projects space point onto a plane
|
|
public virtual Vector2 ProjectPoint(KinectInterop.CameraIntrinsics intr, Vector3 point)
|
|
{
|
|
return Vector2.zero;
|
|
}
|
|
|
|
|
|
// transforms a point from one space to another
|
|
public virtual Vector3 TransformPoint(KinectInterop.CameraExtrinsics extr, Vector3 point)
|
|
{
|
|
return Vector3.zero;
|
|
}
|
|
|
|
|
|
public virtual Vector3[] GetDepthCameraSpaceTable(KinectInterop.SensorData sensorData)
|
|
{
|
|
if (sensorData == null || sensorPlatform == KinectInterop.DepthSensorPlatform.None || string.IsNullOrEmpty(sensorDeviceId))
|
|
return null;
|
|
|
|
if (depth2SpaceTable == null || depth2SpaceWidth != sensorData.depthImageWidth || depth2SpaceHeight != sensorData.depthImageHeight)
|
|
{
|
|
depth2SpaceTable = KinectInterop.LoadSpaceTable(sensorPlatform, sensorDeviceId, "depth", sensorData.depthImageWidth, sensorData.depthImageHeight);
|
|
}
|
|
|
|
depth2SpaceWidth = sensorData.depthImageWidth;
|
|
depth2SpaceHeight = sensorData.depthImageHeight;
|
|
|
|
// depth2space table
|
|
int depthImageLength = sensorData.depthImageWidth * sensorData.depthImageHeight;
|
|
if (depth2SpaceTable == null || depth2SpaceTable.Length != depthImageLength)
|
|
{
|
|
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);
|
|
}
|
|
}
|
|
|
|
KinectInterop.SaveSpaceTable(depth2SpaceTable, sensorPlatform, sensorDeviceId, "depth", sensorData.depthImageWidth, sensorData.depthImageHeight);
|
|
}
|
|
|
|
return depth2SpaceTable;
|
|
}
|
|
|
|
|
|
public virtual Vector3[] GetColorCameraSpaceTable(KinectInterop.SensorData sensorData)
|
|
{
|
|
if (sensorData == null || sensorPlatform == KinectInterop.DepthSensorPlatform.None || string.IsNullOrEmpty(sensorDeviceId))
|
|
return null;
|
|
|
|
if(color2SpaceTable == null || color2SpaceWidth != sensorData.colorImageWidth || color2SpaceHeight != sensorData.colorImageHeight)
|
|
{
|
|
color2SpaceTable = KinectInterop.LoadSpaceTable(sensorPlatform, sensorDeviceId, "color", sensorData.colorImageWidth, sensorData.colorImageHeight);
|
|
}
|
|
|
|
color2SpaceWidth = sensorData.colorImageWidth;
|
|
color2SpaceHeight = sensorData.colorImageHeight;
|
|
|
|
// color2space
|
|
int colorImageLength = sensorData.colorImageWidth * sensorData.colorImageHeight;
|
|
if (color2SpaceTable == null || color2SpaceTable.Length != colorImageLength)
|
|
{
|
|
color2SpaceTable = new Vector3[colorImageLength];
|
|
|
|
for (int cy = 0, ci = 0; cy < sensorData.colorImageHeight; cy++)
|
|
{
|
|
for (int cx = 0; cx < sensorData.colorImageWidth; cx++, ci++)
|
|
{
|
|
Vector2 colorPos = new Vector2(cx, cy);
|
|
color2SpaceTable[ci] = MapColorPointToSpaceCoords(sensorData, colorPos, 1000);
|
|
}
|
|
}
|
|
|
|
KinectInterop.SaveSpaceTable(color2SpaceTable, sensorPlatform, sensorDeviceId, "color", sensorData.colorImageWidth, sensorData.colorImageHeight);
|
|
}
|
|
|
|
return color2SpaceTable;
|
|
}
|
|
|
|
|
|
public virtual Vector3 MapDepthPointToSpaceCoords(KinectInterop.SensorData sensorData, Vector2 depthPos, ushort depthVal)
|
|
{
|
|
if (sensorData.depthCamIntr != null)
|
|
{
|
|
return UnprojectPoint(sensorData.depthCamIntr, depthPos, (float)depthVal * 0.001f);
|
|
}
|
|
|
|
return Vector3.zero;
|
|
}
|
|
|
|
|
|
public virtual Vector2 MapSpacePointToDepthCoords(KinectInterop.SensorData sensorData, Vector3 spacePos)
|
|
{
|
|
if (sensorData.depthCamIntr != null)
|
|
{
|
|
return ProjectPoint(sensorData.depthCamIntr, spacePos);
|
|
}
|
|
|
|
return Vector2.zero;
|
|
}
|
|
|
|
|
|
public virtual Vector3 MapColorPointToSpaceCoords(KinectInterop.SensorData sensorData, Vector2 colorPos, ushort depthVal)
|
|
{
|
|
if (sensorData.colorCamIntr != null)
|
|
{
|
|
return UnprojectPoint(sensorData.colorCamIntr, colorPos, (float)depthVal * 0.001f);
|
|
}
|
|
|
|
return Vector3.zero;
|
|
}
|
|
|
|
|
|
public virtual Vector2 MapSpacePointToColorCoords(KinectInterop.SensorData sensorData, Vector3 spacePos)
|
|
{
|
|
if (sensorData.colorCamIntr != null)
|
|
{
|
|
return ProjectPoint(sensorData.colorCamIntr, spacePos);
|
|
}
|
|
|
|
return Vector2.zero;
|
|
}
|
|
|
|
|
|
public virtual Vector2 MapDepthPointToColorCoords(KinectInterop.SensorData sensorData, Vector2 depthPos, ushort depthVal)
|
|
{
|
|
if (sensorData.depthCamIntr != null && sensorData.colorCamIntr != null && sensorData.depth2ColorExtr != null)
|
|
{
|
|
Vector3 depthSpacePos = UnprojectPoint(sensorData.depthCamIntr, depthPos, (float)depthVal * 0.001f);
|
|
Vector3 colorSpacePos = TransformPoint(sensorData.depth2ColorExtr, depthSpacePos);
|
|
Vector2 colorPos = ProjectPoint(sensorData.colorCamIntr, colorSpacePos);
|
|
|
|
return colorPos;
|
|
}
|
|
|
|
return Vector2.zero;
|
|
}
|
|
|
|
|
|
public virtual Vector2 MapColorPointToDepthCoords(KinectInterop.SensorData sensorData, Vector2 colorPos, int minDist, int maxDist)
|
|
{
|
|
if (sensorData.depthCamIntr != null && sensorData.colorCamIntr != null && sensorData.color2DepthExtr != null && sensorData.depthImage != null)
|
|
{
|
|
Vector3 colorSpacePos1 = UnprojectPoint(sensorData.colorCamIntr, colorPos, 1f);
|
|
|
|
if(minDist <= 0)
|
|
minDist = (int)(minDepthDistance * 1000f);
|
|
if(maxDist <= 0)
|
|
maxDist = (int)(maxDepthDistance * 1000f);
|
|
|
|
int depthImageW = sensorData.depthImageWidth;
|
|
int depthImageL = sensorData.depthImage.Length;
|
|
|
|
Vector2 depthPos = Vector2.zero;
|
|
bool bFound = false;
|
|
|
|
for (int d = minDist; d < maxDist; d++)
|
|
{
|
|
Vector3 colorSpacePos = colorSpacePos1 * (d * 0.001f);
|
|
Vector3 depthSpacePos = TransformPoint(sensorData.color2DepthExtr, colorSpacePos);
|
|
depthPos = ProjectPoint(sensorData.depthCamIntr, depthSpacePos);
|
|
|
|
int di = (int)(depthPos.y + 0.5f) * depthImageW + (int)(depthPos.x + 0.5f);
|
|
|
|
if (di >= 0 && di < depthImageL)
|
|
{
|
|
int z = sensorData.depthImage[di];
|
|
if ((z != 0) && (z <= d))
|
|
{
|
|
bFound = true;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
return bFound ? depthPos : Vector2.zero;
|
|
}
|
|
|
|
return Vector2.zero;
|
|
}
|
|
|
|
|
|
//public virtual bool MapDepthFrameToSpaceCoords(KinectInterop.SensorData sensorData, ref Vector3[] vSpaceCoords)
|
|
//{
|
|
// if (vSpaceCoords == null)
|
|
// {
|
|
// vSpaceCoords = new Vector3[sensorData.depthImageWidth * sensorData.depthImageHeight];
|
|
// }
|
|
|
|
// if (InitCoordMapperSpaceTables(sensorData, true, false))
|
|
// {
|
|
// for (int dy = 0, di = 0; dy < sensorData.depthImageHeight; dy++)
|
|
// {
|
|
// for (int dx = 0; dx < sensorData.depthImageWidth; dx++, di++)
|
|
// {
|
|
// if (sensorData.depthImage[di] != 0)
|
|
// {
|
|
// float depthVal = (float)sensorData.depthImage[di] / 1000f;
|
|
// vSpaceCoords[di] = depth2SpaceTable[di] * depthVal;
|
|
// }
|
|
// else
|
|
// {
|
|
// vSpaceCoords[di] = Vector3.zero;
|
|
// }
|
|
// }
|
|
// }
|
|
|
|
// return true;
|
|
// }
|
|
|
|
// return false;
|
|
//}
|
|
|
|
|
|
//public virtual bool MapDepthFrameToColorData(KinectInterop.SensorData sensorData, ref byte[] vColorFrameData)
|
|
//{
|
|
// return false;
|
|
//}
|
|
|
|
|
|
//public virtual bool MapColorFrameToDepthData(KinectInterop.SensorData sensorData, ref ushort[] vDepthFrameData)
|
|
//{
|
|
// return false;
|
|
//}
|
|
|
|
|
|
//public virtual bool MapDepthFrameToColorCoords(KinectInterop.SensorData sensorData, ref Vector2[] vColorCoords)
|
|
//{
|
|
// if (sensorData.depthCamIntr != null && sensorData.colorCamIntr != null && sensorData.depth2ColorExtr != null)
|
|
// {
|
|
// int depthImageW = sensorData.depthImageWidth;
|
|
// int depthImageH = sensorData.depthImageHeight;
|
|
|
|
// int mapImageLen = depthImageW * depthImageH;
|
|
// if (vColorCoords == null || vColorCoords.Length != mapImageLen)
|
|
// {
|
|
// vColorCoords = new Vector2[mapImageLen];
|
|
// }
|
|
|
|
// for (int dy = 0, dIndex = 0; dy < depthImageH; dy++)
|
|
// {
|
|
// for (int dx = 0; dx < depthImageW; dx++, dIndex++)
|
|
// {
|
|
// ushort depthVal = sensorData.depthImage[dIndex];
|
|
|
|
// if (depthVal != 0)
|
|
// {
|
|
// Vector2 depthPos = new Vector2(dx, dy);
|
|
|
|
// Vector3 depthSpacePos = UnprojectPoint(sensorData.depthCamIntr, depthPos, (float)depthVal / 1000f);
|
|
// Vector3 colorSpacePos = TransformPoint(sensorData.depth2ColorExtr, depthSpacePos);
|
|
// vColorCoords[dIndex] = ProjectPoint(sensorData.colorCamIntr, colorSpacePos);
|
|
// }
|
|
// else
|
|
// {
|
|
// vColorCoords[dIndex] = new Vector2(float.NegativeInfinity, float.NegativeInfinity);
|
|
// }
|
|
// }
|
|
// }
|
|
|
|
// return true;
|
|
// }
|
|
|
|
// return false;
|
|
//}
|
|
|
|
//public virtual bool MapColorFrameToDepthCoords(KinectInterop.SensorData sensorData, ref Vector2[] vDepthCoords)
|
|
//{
|
|
// if (sensorData.depthCamIntr != null && sensorData.colorCamIntr != null && sensorData.depth2ColorExtr != null)
|
|
// {
|
|
// int depthImageW = sensorData.depthImageWidth;
|
|
// int depthImageH = sensorData.depthImageHeight;
|
|
|
|
// int mapImageLen = sensorData.colorImageWidth * sensorData.colorImageHeight;
|
|
// if (vDepthCoords == null || vDepthCoords.Length != mapImageLen)
|
|
// {
|
|
// vDepthCoords = new Vector2[mapImageLen];
|
|
// }
|
|
|
|
// int colorWidth = sensorData.colorCamIntr.width;
|
|
// int colorHeight = sensorData.colorCamIntr.height;
|
|
|
|
// for (int dy = 0, dIndex = 0; dy < depthImageH; dy++)
|
|
// {
|
|
// for (int dx = 0; dx < depthImageW; dx++, dIndex++)
|
|
// {
|
|
// ushort depthVal = sensorData.depthImage[dIndex];
|
|
|
|
// if (depthVal != 0)
|
|
// {
|
|
// float depth = (float)depthVal / 1000f;
|
|
|
|
// Vector2 depthPos1 = new Vector2(dx - 0.5f, dy - 0.5f);
|
|
// Vector3 depthSpacePos1 = UnprojectPoint(sensorData.depthCamIntr, depthPos1, depth);
|
|
// Vector3 colorSpacePos1 = TransformPoint(sensorData.depth2ColorExtr, depthSpacePos1);
|
|
// Vector2 colorPos1 = ProjectPoint(sensorData.colorCamIntr, colorSpacePos1);
|
|
|
|
// int colorPos1X = Mathf.RoundToInt(colorPos1.x);
|
|
// int colorPos1Y = Mathf.RoundToInt(colorPos1.y);
|
|
|
|
// Vector2 depthPos2 = new Vector2(dx + 0.5f, dy + 0.5f);
|
|
// Vector3 depthSpacePos2 = UnprojectPoint(sensorData.depthCamIntr, depthPos2, depth);
|
|
// Vector3 colorSpacePos2 = TransformPoint(sensorData.depth2ColorExtr, depthSpacePos2);
|
|
// Vector2 colorPos2 = ProjectPoint(sensorData.colorCamIntr, colorSpacePos2);
|
|
|
|
// int colorPos2X = (int)(colorPos2.x + 0.5f);
|
|
// int colorPos2Y = (int)(colorPos2.y + 0.5f);
|
|
|
|
// if (colorPos1X < 0 || colorPos1Y < 0 || colorPos2X >= colorWidth || colorPos2Y >= colorHeight)
|
|
// continue;
|
|
|
|
// // Transfer between the depth pixels and the pixels inside the rectangle on the other image
|
|
// for (int y = colorPos1Y; y <= colorPos2Y; y++)
|
|
// {
|
|
// int cIndex = y * colorWidth + colorPos1X;
|
|
|
|
// for (int x = colorPos1X; x <= colorPos2X; x++, cIndex++)
|
|
// {
|
|
// vDepthCoords[cIndex] = new Vector2(dx, dy);
|
|
// }
|
|
// }
|
|
// }
|
|
// else
|
|
// {
|
|
// //vDepthCoords[cIndex] = new Vector2(float.NegativeInfinity, float.NegativeInfinity);
|
|
// }
|
|
// }
|
|
// }
|
|
|
|
// return true;
|
|
// }
|
|
|
|
// return false;
|
|
//}
|
|
|
|
|
|
// estimates horizontal and vertical FOV
|
|
protected void EstimateFOV(KinectInterop.CameraIntrinsics intr)
|
|
{
|
|
//intr.hFOV = (Mathf.Atan2(intr.ppx + 0.5f, intr.fx) + Mathf.Atan2(intr.width - (intr.ppx + 0.5f), intr.fx)) * 57.2957795f;
|
|
//intr.vFOV = (Mathf.Atan2(intr.ppy + 0.5f, intr.fy) + Mathf.Atan2(intr.height - (intr.ppy + 0.5f), intr.fy)) * 57.2957795f;
|
|
intr.hFOV = 2f * Mathf.Atan2((float)intr.width * 0.5f, intr.fx) * Mathf.Rad2Deg;
|
|
intr.vFOV = 2f * Mathf.Atan2((float)intr.height * 0.5f, intr.fy) * Mathf.Rad2Deg;
|
|
}
|
|
|
|
|
|
// initializes the body-data structures and start the body tracking
|
|
public virtual bool InitBodyTracking(KinectInterop.FrameSource dwFlags, KinectInterop.SensorData sensorData)
|
|
{
|
|
if ((dwFlags & KinectInterop.FrameSource.TypeBodyIndex) != 0)
|
|
{
|
|
rawBodyIndexImage = new byte[sensorData.depthImageWidth * sensorData.depthImageHeight];
|
|
sensorData.bodyIndexImage = new byte[sensorData.depthImageWidth * sensorData.depthImageHeight];
|
|
}
|
|
|
|
if ((dwFlags & KinectInterop.FrameSource.TypeBody) != 0) // check for body stream
|
|
{
|
|
alTrackedBodies = new List<KinectInterop.BodyData>();
|
|
sensorData.alTrackedBodies = new KinectInterop.BodyData[0]; // new List<KinectInterop.BodyData>();
|
|
|
|
trackedBodiesCount = 0;
|
|
sensorData.trackedBodiesCount = 0;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
// stops the body tracker and releases its data
|
|
public virtual void StopBodyTracking(KinectInterop.SensorData sensorData)
|
|
{
|
|
}
|
|
|
|
// estimates additional joints for the given body
|
|
protected virtual void CalcBodySpecialJoints(ref KinectInterop.BodyData bodyData)
|
|
{
|
|
// hand left
|
|
int w = (int)KinectInterop.JointType.WristLeft;
|
|
int h = (int)KinectInterop.JointType.HandLeft;
|
|
|
|
if (bodyData.joint[h].trackingState == KinectInterop.TrackingState.Inferred &&
|
|
bodyData.joint[w].trackingState < KinectInterop.TrackingState.Inferred)
|
|
{
|
|
bodyData.joint[h].trackingState = KinectInterop.TrackingState.NotTracked;
|
|
}
|
|
|
|
int e = (int)KinectInterop.JointType.ElbowLeft;
|
|
//if (bodyData.joint[h].trackingState == KinectInterop.TrackingState.Inferred)
|
|
//{
|
|
// bodyData.joint[h].trackingState = KinectInterop.TrackingState.Tracked;
|
|
//}
|
|
//else
|
|
if (bodyData.joint[h].trackingState == KinectInterop.TrackingState.NotTracked &&
|
|
bodyData.joint[w].trackingState >= KinectInterop.TrackingState.Inferred)
|
|
{
|
|
KinectInterop.JointData jointData = bodyData.joint[h];
|
|
jointData.trackingState = bodyData.joint[w].trackingState;
|
|
jointData.orientation = bodyData.joint[w].orientation;
|
|
|
|
Vector3 posWrist = bodyData.joint[w].kinectPos;
|
|
Vector3 posElbow = bodyData.joint[e].kinectPos;
|
|
jointData.kinectPos = posWrist + (posWrist - posElbow) * 0.42f;
|
|
|
|
posWrist = bodyData.joint[w].position;
|
|
posElbow = bodyData.joint[e].position;
|
|
jointData.position = posWrist + (posWrist - posElbow) * 0.42f;
|
|
|
|
bodyData.joint[h] = jointData;
|
|
}
|
|
|
|
int f = (int)KinectInterop.JointType.HandtipLeft;
|
|
if (bodyData.joint[f].trackingState == KinectInterop.TrackingState.Inferred &&
|
|
bodyData.joint[w].trackingState <= KinectInterop.TrackingState.Inferred)
|
|
{
|
|
bodyData.joint[f].trackingState = KinectInterop.TrackingState.NotTracked;
|
|
}
|
|
|
|
f = (int)KinectInterop.JointType.ThumbLeft;
|
|
if (bodyData.joint[f].trackingState == KinectInterop.TrackingState.Inferred &&
|
|
bodyData.joint[w].trackingState <= KinectInterop.TrackingState.Inferred)
|
|
{
|
|
bodyData.joint[f].trackingState = KinectInterop.TrackingState.NotTracked;
|
|
}
|
|
|
|
// hand right
|
|
w = (int)KinectInterop.JointType.WristRight;
|
|
h = (int)KinectInterop.JointType.HandRight;
|
|
|
|
if (bodyData.joint[h].trackingState == KinectInterop.TrackingState.Inferred &&
|
|
bodyData.joint[w].trackingState < KinectInterop.TrackingState.Inferred)
|
|
{
|
|
bodyData.joint[h].trackingState = KinectInterop.TrackingState.NotTracked;
|
|
}
|
|
|
|
e = (int)KinectInterop.JointType.ElbowRight;
|
|
//if (bodyData.joint[h].trackingState == KinectInterop.TrackingState.Inferred)
|
|
//{
|
|
// bodyData.joint[h].trackingState = KinectInterop.TrackingState.Tracked;
|
|
//}
|
|
//else
|
|
if (bodyData.joint[h].trackingState == KinectInterop.TrackingState.NotTracked &&
|
|
bodyData.joint[w].trackingState >= KinectInterop.TrackingState.Inferred)
|
|
{
|
|
KinectInterop.JointData jointData = bodyData.joint[h];
|
|
jointData.trackingState = bodyData.joint[w].trackingState;
|
|
jointData.orientation = bodyData.joint[w].orientation;
|
|
|
|
Vector3 posWrist = bodyData.joint[w].kinectPos;
|
|
Vector3 posElbow = bodyData.joint[e].kinectPos;
|
|
jointData.kinectPos = posWrist + (posWrist - posElbow) * 0.42f;
|
|
|
|
posWrist = bodyData.joint[w].position;
|
|
posElbow = bodyData.joint[e].position;
|
|
jointData.position = posWrist + (posWrist - posElbow) * 0.42f;
|
|
|
|
bodyData.joint[h] = jointData;
|
|
}
|
|
|
|
f = (int)KinectInterop.JointType.HandtipRight;
|
|
if (bodyData.joint[f].trackingState == KinectInterop.TrackingState.Inferred &&
|
|
bodyData.joint[w].trackingState <= KinectInterop.TrackingState.Inferred)
|
|
{
|
|
bodyData.joint[f].trackingState = KinectInterop.TrackingState.NotTracked;
|
|
}
|
|
|
|
f = (int)KinectInterop.JointType.ThumbRight;
|
|
if (bodyData.joint[f].trackingState == KinectInterop.TrackingState.Inferred &&
|
|
bodyData.joint[w].trackingState <= KinectInterop.TrackingState.Inferred)
|
|
{
|
|
bodyData.joint[f].trackingState = KinectInterop.TrackingState.NotTracked;
|
|
}
|
|
|
|
//// 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;
|
|
//}
|
|
}
|
|
|
|
|
|
//// calculates all bone directions for the given body
|
|
//protected virtual void CalcBodyJointDirs(ref KinectInterop.BodyData bodyData)
|
|
//{
|
|
// if (bodyData.bIsTracked)
|
|
// {
|
|
// for (int j = 0; j < (int)KinectInterop.JointType.Count; j++)
|
|
// {
|
|
// if (j == 0)
|
|
// {
|
|
// bodyData.joint[j].direction = Vector3.zero;
|
|
// }
|
|
// else
|
|
// {
|
|
// int jParent = (int)KinectInterop.GetParentJoint((KinectInterop.JointType)j);
|
|
|
|
// if (bodyData.joint[j].trackingState != KinectInterop.TrackingState.NotTracked &&
|
|
// bodyData.joint[jParent].trackingState != KinectInterop.TrackingState.NotTracked)
|
|
// {
|
|
// bodyData.joint[j].direction = (bodyData.joint[j].position - bodyData.joint[jParent].position); //.normalized;
|
|
// }
|
|
// }
|
|
// }
|
|
// }
|
|
//}
|
|
|
|
// calculates all joint orientations for the given body
|
|
protected virtual void CalcBodyJointOrients(ref KinectInterop.BodyData bodyData)
|
|
{
|
|
if(bIgnoreZCoordinates)
|
|
{
|
|
DoCalcBodyJointOrients2D(ref bodyData, bIgnoreInferredJoints);
|
|
}
|
|
else
|
|
{
|
|
DoCalcBodyJointOrients(ref bodyData, bIgnoreInferredJoints);
|
|
}
|
|
}
|
|
|
|
// calculates all joint orientations for the given body in 2D
|
|
public static void DoCalcBodyJointOrients2D(ref KinectInterop.BodyData bodyData, bool bIgnoreInferredJoints)
|
|
{
|
|
int jointCount = bodyData.joint.Length;
|
|
for (int j = 0; j < jointCount; j++)
|
|
{
|
|
int joint = j;
|
|
|
|
KinectInterop.JointData jointData = bodyData.joint[joint];
|
|
bool bJointValid = bIgnoreInferredJoints ? jointData.trackingState == KinectInterop.TrackingState.Tracked : jointData.trackingState != KinectInterop.TrackingState.NotTracked;
|
|
|
|
if (bJointValid)
|
|
{
|
|
int nextJoint = (int)KinectInterop.GetNextJoint((KinectInterop.JointType)joint);
|
|
if (nextJoint != joint && joint != (int)KinectInterop.JointType.AnkleLeft && joint != (int)KinectInterop.JointType.AnkleRight &&
|
|
joint != (int)KinectInterop.JointType.Nose && joint != (int)KinectInterop.JointType.WristLeft && joint != (int)KinectInterop.JointType.WristRight)
|
|
{
|
|
KinectInterop.JointData nextJointData = bodyData.joint[nextJoint];
|
|
bool bNextJointValid = bIgnoreInferredJoints ? nextJointData.trackingState == KinectInterop.TrackingState.Tracked : nextJointData.trackingState != KinectInterop.TrackingState.NotTracked;
|
|
|
|
Vector3 baseDir = KinectJointBaseDir[nextJoint];
|
|
Vector3 jointDir = nextJointData.direction.normalized;
|
|
jointDir = new Vector3(jointDir.x, jointDir.y, 0f).normalized;
|
|
|
|
Quaternion jointOrientNormal = jointData.normalRotation;
|
|
if (bNextJointValid)
|
|
{
|
|
jointOrientNormal = Quaternion.FromToRotation(baseDir, jointDir);
|
|
|
|
if (joint == (int)KinectInterop.JointType.Head)
|
|
{
|
|
if (bodyData.joint[(int)KinectInterop.JointType.EarRight].trackingState != KinectInterop.TrackingState.NotTracked &&
|
|
bodyData.joint[(int)KinectInterop.JointType.EarLeft].trackingState != KinectInterop.TrackingState.NotTracked)
|
|
{
|
|
Vector3 posREar = bodyData.joint[(int)KinectInterop.JointType.EarRight].position;
|
|
Vector3 posLEar = bodyData.joint[(int)KinectInterop.JointType.EarLeft].position;
|
|
|
|
Vector3 earsDir = posREar - posLEar;
|
|
earsDir = new Vector3(earsDir.x, earsDir.y, 0f).normalized;
|
|
jointOrientNormal = Quaternion.FromToRotation(Vector3.right, earsDir);
|
|
|
|
//Debug.Log($"head - ears: {earsDir}, rot: {jointOrientNormal.eulerAngles}");
|
|
}
|
|
}
|
|
|
|
jointData.normalRotation = jointOrientNormal;
|
|
}
|
|
|
|
Vector3 mirroredAngles = jointData.normalRotation.eulerAngles;
|
|
mirroredAngles.y = -mirroredAngles.y;
|
|
mirroredAngles.z = -mirroredAngles.z;
|
|
|
|
jointData.mirroredRotation = Quaternion.Euler(mirroredAngles);
|
|
}
|
|
else
|
|
{
|
|
// get the orientation of the parent joint
|
|
int prevJoint = (int)KinectInterop.GetParentJoint((KinectInterop.JointType)joint);
|
|
if (prevJoint != joint)
|
|
{
|
|
jointData.normalRotation = bodyData.joint[prevJoint].normalRotation;
|
|
jointData.mirroredRotation = bodyData.joint[prevJoint].mirroredRotation;
|
|
}
|
|
else
|
|
{
|
|
jointData.normalRotation = Quaternion.identity;
|
|
jointData.mirroredRotation = Quaternion.identity;
|
|
}
|
|
}
|
|
|
|
bodyData.joint[joint] = jointData;
|
|
}
|
|
|
|
if (joint == (int)KinectInterop.JointType.Pelvis)
|
|
{
|
|
bodyData.normalRotation = jointData.normalRotation;
|
|
bodyData.mirroredRotation = jointData.mirroredRotation;
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
// calculates all joint orientations for the given body (static method)
|
|
public static void DoCalcBodyJointOrients(ref KinectInterop.BodyData bodyData, bool bIgnoreInferredJoints)
|
|
{
|
|
int jointCount = bodyData.joint.Length;
|
|
|
|
Vector3 posRShoulder = bodyData.joint[(int)KinectInterop.JointType.ShoulderRight].position;
|
|
Vector3 posLShoulder = bodyData.joint[(int)KinectInterop.JointType.ShoulderLeft].position;
|
|
Vector3 shouldersDirection = posRShoulder - posLShoulder;
|
|
shouldersDirection -= Vector3.Project(shouldersDirection, Vector3.up);
|
|
|
|
Vector3 posRHip = bodyData.joint[(int)KinectInterop.JointType.HipRight].position;
|
|
Vector3 posLHip = bodyData.joint[(int)KinectInterop.JointType.HipLeft].position;
|
|
Vector3 hipsDirection = posRHip - posLHip;
|
|
hipsDirection -= Vector3.Project(hipsDirection, Vector3.up);
|
|
|
|
for (int j = 0; j < jointCount; j++)
|
|
{
|
|
int joint = j;
|
|
|
|
KinectInterop.JointData jointData = bodyData.joint[joint];
|
|
bool bJointValid = bIgnoreInferredJoints ? jointData.trackingState == KinectInterop.TrackingState.Tracked : jointData.trackingState != KinectInterop.TrackingState.NotTracked;
|
|
|
|
if (bJointValid)
|
|
{
|
|
int nextJoint = (int)KinectInterop.GetNextJoint((KinectInterop.JointType)joint);
|
|
if (nextJoint != joint && nextJoint >= 0 && nextJoint < jointCount)
|
|
{
|
|
KinectInterop.JointData nextJointData = bodyData.joint[nextJoint];
|
|
bool bNextJointValid = bIgnoreInferredJoints ? nextJointData.trackingState == KinectInterop.TrackingState.Tracked : nextJointData.trackingState != KinectInterop.TrackingState.NotTracked;
|
|
|
|
Vector3 baseDir = KinectJointBaseDir[nextJoint];
|
|
Vector3 jointDir = nextJointData.direction.normalized;
|
|
jointDir = new Vector3(jointDir.x, jointDir.y, -jointDir.z).normalized;
|
|
|
|
Quaternion jointOrientNormal = jointData.normalRotation;
|
|
if (bNextJointValid)
|
|
{
|
|
jointOrientNormal = Quaternion.FromToRotation(baseDir, jointDir);
|
|
}
|
|
|
|
//if(joint == 0)
|
|
//{
|
|
// Debug.Log($"pelvis: pos: {jointData.position}\ndir: {jointDir}, base: {baseDir}");
|
|
//}
|
|
|
|
if ((joint == (int)KinectInterop.JointType.ShoulderLeft) ||
|
|
(joint == (int)KinectInterop.JointType.ElbowLeft) ||
|
|
(joint == (int)KinectInterop.JointType.WristLeft) ||
|
|
(joint == (int)KinectInterop.JointType.HandLeft))
|
|
{
|
|
if (bNextJointValid && jointData.direction != Vector3.zero && jointDir != Vector3.zero)
|
|
{
|
|
Vector3 parJointDir = jointData.direction.normalized;
|
|
parJointDir = new Vector3(parJointDir.x, parJointDir.y, -parJointDir.z).normalized;
|
|
|
|
if (joint == (int)KinectInterop.JointType.WristLeft)
|
|
{
|
|
// for wrist, take the finger direction into account, too
|
|
int fingerJoint = (int)KinectInterop.GetNextJoint((KinectInterop.JointType)nextJoint);
|
|
|
|
if (fingerJoint != joint && fingerJoint >= 0 && fingerJoint < jointCount)
|
|
{
|
|
KinectInterop.JointData fingerData = bodyData.joint[fingerJoint];
|
|
if (fingerData.trackingState != KinectInterop.TrackingState.NotTracked)
|
|
{
|
|
jointDir = (nextJointData.direction + fingerData.direction).normalized;
|
|
jointDir = new Vector3(jointDir.x, jointDir.y, -jointDir.z).normalized;
|
|
}
|
|
}
|
|
}
|
|
|
|
float parDotJoint = Vector3.Dot(parJointDir, jointDir);
|
|
//if (joint == (int)KinectInterop.JointType.ElbowLeft)
|
|
// Debug.Log ((KinectInterop.JointType)joint + ": " + parDotJoint);
|
|
|
|
if ((parDotJoint >= 0.001f && parDotJoint <= 0.999f) || (parDotJoint >= -0.999f && parDotJoint <= -0.001f))
|
|
{
|
|
if (joint != (int)KinectInterop.JointType.ShoulderLeft && parJointDir != Vector3.zero)
|
|
{
|
|
Vector3 upDir = -Vector3.Cross(-parJointDir, jointDir).normalized;
|
|
Vector3 fwdDir = Vector3.Cross(-jointDir, upDir).normalized;
|
|
if (fwdDir.sqrMagnitude >= 0.5f && upDir.sqrMagnitude >= 0.5f) // avoid zero-length directions
|
|
jointOrientNormal = Quaternion.LookRotation(fwdDir, upDir);
|
|
//if(joint == (int)KinectInterop.JointType.ElbowLeft)
|
|
// Debug.Log((KinectInterop.JointType)joint + " - F:" + fwdDir + ", U:" + upDir + ", P: " + parJointDir + ", J: " + jointDir);
|
|
}
|
|
else
|
|
{
|
|
// shoulder left
|
|
Vector3 fwdDir = Vector3.zero, upDir = Vector3.zero;
|
|
KinectInterop.JointData shCenterData = bodyData.joint[(int)KinectInterop.JointType.ClavicleLeft];
|
|
|
|
Vector3 spineDir = shCenterData.direction;
|
|
spineDir = new Vector3(spineDir.x, spineDir.y, -spineDir.z).normalized;
|
|
|
|
fwdDir = Vector3.Cross(-jointDir, spineDir).normalized;
|
|
float fwdDotJoint = Vector3.Dot(fwdDir, Vector3.forward);
|
|
//Debug.Log("fwdDotJointL: " + fwdDotJoint);
|
|
|
|
if (fwdDotJoint >= 0f && bodyData.joint[(int)KinectInterop.JointType.ElbowLeft].trackingState == KinectInterop.TrackingState.Tracked &&
|
|
bodyData.joint[(int)KinectInterop.JointType.WristLeft].trackingState == KinectInterop.TrackingState.Tracked)
|
|
{
|
|
// get up-dir from elbow
|
|
Vector3 elbowDir = bodyData.joint[(int)KinectInterop.JointType.WristLeft].direction;
|
|
elbowDir = new Vector3(elbowDir.x, elbowDir.y, -elbowDir.z).normalized;
|
|
|
|
upDir = -Vector3.Cross(-jointDir, elbowDir).normalized;
|
|
fwdDir = Vector3.Cross(-jointDir, upDir).normalized;
|
|
//Debug.Log((KinectInterop.JointType)joint + "*E - U:" + upDir + ", F:" + fwdDir + ", J: " + jointDir + ", E: " + elbowDir);
|
|
}
|
|
else
|
|
{
|
|
// get fwd-dir from spine
|
|
upDir = Vector3.Cross(fwdDir, -jointDir).normalized;
|
|
//Debug.Log((KinectInterop.JointType)joint + "*S - F:" + fwdDir + ", U:" + upDir + ", S:" + spineDir + ", J:" + jointDir);
|
|
}
|
|
|
|
if (fwdDir.sqrMagnitude >= 0.5f && upDir.sqrMagnitude >= 0.5f) // avoid zero-length directions
|
|
jointOrientNormal = Quaternion.LookRotation(fwdDir, upDir);
|
|
}
|
|
|
|
jointData.normalRotation = jointOrientNormal;
|
|
}
|
|
else if (joint != (int)KinectInterop.JointType.ShoulderLeft) // to prevent upper-arm ticks
|
|
{
|
|
// no angle between bones - use the parent's rotation
|
|
KinectInterop.JointType parJoint = KinectInterop.GetParentJoint((KinectInterop.JointType)joint);
|
|
KinectInterop.JointData parJointData = bodyData.joint[(int)parJoint];
|
|
|
|
jointData.normalRotation = parJointData.normalRotation;
|
|
}
|
|
}
|
|
|
|
// allowedHandRotations = All (left wrist/hand)
|
|
if (joint == (int)KinectInterop.JointType.WristLeft || joint == (int)KinectInterop.JointType.HandLeft)
|
|
{
|
|
KinectInterop.JointData thumbData = bodyData.joint[(int)KinectInterop.JointType.ThumbLeft];
|
|
|
|
int prevJoint = (int)KinectInterop.GetParentJoint((KinectInterop.JointType)joint);
|
|
KinectInterop.JointData prevJointData = bodyData.joint[prevJoint];
|
|
|
|
if (thumbData.trackingState != KinectInterop.TrackingState.NotTracked &&
|
|
prevJointData.trackingState != KinectInterop.TrackingState.NotTracked)
|
|
{
|
|
Vector3 rightDir = -jointDir;
|
|
Vector3 fwdDir = thumbData.direction.normalized;
|
|
fwdDir = new Vector3(fwdDir.x, fwdDir.y, -fwdDir.z).normalized;
|
|
|
|
if (joint == (int)KinectInterop.JointType.HandLeft)
|
|
{
|
|
Vector3 prevBaseDir = -Vector3.left; // - KinectInterop.JointBaseDir[prevJoint];
|
|
Vector3 prevOrthoDir = new Vector3(prevBaseDir.y, prevBaseDir.z, prevBaseDir.x);
|
|
fwdDir = prevJointData.normalRotation * prevOrthoDir;
|
|
//rightDir -= Vector3.Project(rightDir, fwdDir);
|
|
}
|
|
|
|
if (rightDir != Vector3.zero && fwdDir != Vector3.zero)
|
|
{
|
|
Vector3 upDir = Vector3.Cross(fwdDir, rightDir).normalized;
|
|
fwdDir = Vector3.Cross(rightDir, upDir).normalized;
|
|
|
|
//jointData.normalRotation = Quaternion.LookRotation(fwdDir, upDir);
|
|
Quaternion jointOrientThumb = Quaternion.LookRotation(fwdDir, upDir);
|
|
jointOrientNormal = (joint == (int)KinectInterop.JointType.WristLeft) ?
|
|
Quaternion.RotateTowards(prevJointData.normalRotation, jointOrientThumb, 80f) : jointOrientThumb;
|
|
|
|
jointData.normalRotation = jointOrientNormal;
|
|
}
|
|
}
|
|
|
|
//bRotated = true;
|
|
}
|
|
|
|
if (joint == (int)KinectInterop.JointType.WristLeft || joint == (int)KinectInterop.JointType.HandLeft)
|
|
{
|
|
// limit wrist and hand twist
|
|
int prevJoint = (int)KinectInterop.GetParentJoint((KinectInterop.JointType)joint);
|
|
KinectInterop.JointData prevJointData = bodyData.joint[prevJoint];
|
|
|
|
if (prevJointData.trackingState != KinectInterop.TrackingState.NotTracked)
|
|
{
|
|
jointData.normalRotation = Quaternion.RotateTowards(prevJointData.normalRotation, jointData.normalRotation, 70f);
|
|
}
|
|
}
|
|
|
|
}
|
|
else if ((joint == (int)KinectInterop.JointType.ShoulderRight) ||
|
|
(joint == (int)KinectInterop.JointType.ElbowRight) ||
|
|
(joint == (int)KinectInterop.JointType.WristRight) ||
|
|
(joint == (int)KinectInterop.JointType.HandRight))
|
|
{
|
|
if (bNextJointValid && jointData.direction != Vector3.zero && jointDir != Vector3.zero)
|
|
{
|
|
Vector3 parJointDir = jointData.direction.normalized;
|
|
parJointDir = new Vector3(parJointDir.x, parJointDir.y, -parJointDir.z).normalized;
|
|
|
|
if (joint == (int)KinectInterop.JointType.WristRight)
|
|
{
|
|
// for wrist, take the finger direction into account, too
|
|
int fingerJoint = (int)KinectInterop.GetNextJoint((KinectInterop.JointType)nextJoint);
|
|
|
|
if (fingerJoint != joint && fingerJoint >= 0 && fingerJoint < jointCount)
|
|
{
|
|
KinectInterop.JointData fingerData = bodyData.joint[fingerJoint];
|
|
if (fingerData.trackingState != KinectInterop.TrackingState.NotTracked)
|
|
{
|
|
jointDir = (nextJointData.direction + fingerData.direction).normalized;
|
|
jointDir = new Vector3(jointDir.x, jointDir.y, -jointDir.z).normalized;
|
|
}
|
|
}
|
|
}
|
|
|
|
float parDotJoint = Vector3.Dot(parJointDir, jointDir);
|
|
//if (joint == (int)KinectInterop.JointType.ElbowRight)
|
|
// Debug.Log ((KinectInterop.JointType)joint + ": " + parDotJoint);
|
|
|
|
if ((parDotJoint >= 0.001f && parDotJoint <= 0.999f) || (parDotJoint >= -0.999f && parDotJoint <= -0.001f))
|
|
{
|
|
if (joint != (int)KinectInterop.JointType.ShoulderRight && parJointDir != Vector3.zero)
|
|
{
|
|
Vector3 upDir = -Vector3.Cross(parJointDir, jointDir).normalized;
|
|
Vector3 fwdDir = Vector3.Cross(jointDir, upDir).normalized;
|
|
if (fwdDir.sqrMagnitude >= 0.5f && upDir.sqrMagnitude >= 0.5f) // avoid zero-length directions
|
|
jointOrientNormal = Quaternion.LookRotation(fwdDir, upDir);
|
|
//if(joint == (int)KinectInterop.JointType.ElbowRight)
|
|
// Debug.Log((KinectInterop.JointType)joint + " - F:" + fwdDir + ", U:" + upDir + ", P: " + parJointDir + ", J: " + jointDir);
|
|
}
|
|
else
|
|
{
|
|
// shoulder right
|
|
Vector3 fwdDir = Vector3.zero, upDir = Vector3.zero;
|
|
KinectInterop.JointData shCenterData = bodyData.joint[(int)KinectInterop.JointType.ClavicleRight];
|
|
|
|
Vector3 spineDir = shCenterData.direction.normalized;
|
|
spineDir = new Vector3(spineDir.x, spineDir.y, -spineDir.z).normalized;
|
|
|
|
fwdDir = Vector3.Cross(jointDir, spineDir).normalized;
|
|
float fwdDotJoint = Vector3.Dot(fwdDir, Vector3.forward);
|
|
//Debug.Log("fwdDotJointR: " + fwdDotJoint);
|
|
|
|
if (fwdDotJoint >= 0f && bodyData.joint[(int)KinectInterop.JointType.ElbowRight].trackingState == KinectInterop.TrackingState.Tracked &&
|
|
bodyData.joint[(int)KinectInterop.JointType.WristRight].trackingState == KinectInterop.TrackingState.Tracked)
|
|
{
|
|
// get up-dir from elbow
|
|
Vector3 elbowDir = bodyData.joint[(int)KinectInterop.JointType.WristRight].direction;
|
|
elbowDir = new Vector3(elbowDir.x, elbowDir.y, -elbowDir.z).normalized;
|
|
|
|
upDir = -Vector3.Cross(jointDir, elbowDir).normalized;
|
|
fwdDir = Vector3.Cross(jointDir, upDir).normalized;
|
|
//Debug.Log((KinectInterop.JointType)joint + "*E - U:" + upDir + ", F:" + fwdDir + ", J: " + jointDir + ", E: " + elbowDir);
|
|
}
|
|
else
|
|
{
|
|
// get fwd-dir from spine
|
|
upDir = Vector3.Cross(fwdDir, jointDir).normalized;
|
|
//Debug.Log((KinectInterop.JointType)joint + " - F:" + fwdDir + ", U:" + upDir + ", S:" + spineDir + ", J:" + jointDir);
|
|
}
|
|
|
|
if (fwdDir.sqrMagnitude >= 0.5f && upDir.sqrMagnitude >= 0.5f) // avoid zero-length directions
|
|
jointOrientNormal = Quaternion.LookRotation(fwdDir, upDir);
|
|
}
|
|
|
|
jointData.normalRotation = jointOrientNormal;
|
|
}
|
|
else if (joint != (int)KinectInterop.JointType.ShoulderRight) // to prevent upper-arm ticks
|
|
{
|
|
// no angle between bones - use the parent's rotation
|
|
KinectInterop.JointType parJoint = KinectInterop.GetParentJoint((KinectInterop.JointType)joint);
|
|
KinectInterop.JointData parJointData = bodyData.joint[(int)parJoint];
|
|
|
|
jointData.normalRotation = parJointData.normalRotation;
|
|
}
|
|
}
|
|
|
|
// allowedHandRotations = All (right wrist/hand)
|
|
if (joint == (int)KinectInterop.JointType.WristRight || joint == (int)KinectInterop.JointType.HandRight)
|
|
{
|
|
KinectInterop.JointData thumbData = bodyData.joint[(int)KinectInterop.JointType.ThumbRight];
|
|
|
|
int prevJoint = (int)KinectInterop.GetParentJoint((KinectInterop.JointType)joint);
|
|
KinectInterop.JointData prevJointData = bodyData.joint[prevJoint];
|
|
|
|
if (thumbData.trackingState != KinectInterop.TrackingState.NotTracked &&
|
|
prevJointData.trackingState != KinectInterop.TrackingState.NotTracked)
|
|
{
|
|
Vector3 rightDir = jointDir;
|
|
Vector3 fwdDir = thumbData.direction.normalized;
|
|
fwdDir = new Vector3(fwdDir.x, fwdDir.y, -fwdDir.z).normalized;
|
|
|
|
if (joint == (int)KinectInterop.JointType.HandRight)
|
|
{
|
|
Vector3 prevBaseDir = Vector3.right; // KinectInterop.JointBaseDir[prevJoint];
|
|
Vector3 prevOrthoDir = new Vector3(prevBaseDir.y, prevBaseDir.z, prevBaseDir.x);
|
|
fwdDir = prevJointData.normalRotation * prevOrthoDir;
|
|
//rightDir -= Vector3.Project(rightDir, fwdDir);
|
|
}
|
|
|
|
if (rightDir != Vector3.zero && fwdDir != Vector3.zero)
|
|
{
|
|
Vector3 upDir = Vector3.Cross(fwdDir, rightDir).normalized;
|
|
fwdDir = Vector3.Cross(rightDir, upDir).normalized;
|
|
|
|
Quaternion jointOrientThumb = Quaternion.LookRotation(fwdDir, upDir);
|
|
jointOrientNormal = (joint == (int)KinectInterop.JointType.WristRight) ?
|
|
Quaternion.RotateTowards(prevJointData.normalRotation, jointOrientThumb, 80f) : jointOrientThumb;
|
|
|
|
jointData.normalRotation = jointOrientNormal;
|
|
}
|
|
}
|
|
|
|
//bRotated = true;
|
|
}
|
|
|
|
if (joint == (int)KinectInterop.JointType.WristRight || joint == (int)KinectInterop.JointType.HandRight)
|
|
{
|
|
// limit wrist and hand twist
|
|
int prevJoint = (int)KinectInterop.GetParentJoint((KinectInterop.JointType)joint);
|
|
KinectInterop.JointData prevJointData = bodyData.joint[prevJoint];
|
|
|
|
if (prevJointData.trackingState != KinectInterop.TrackingState.NotTracked)
|
|
{
|
|
jointData.normalRotation = Quaternion.RotateTowards(prevJointData.normalRotation, jointData.normalRotation, 70f);
|
|
}
|
|
}
|
|
|
|
}
|
|
else if (joint == (int)KinectInterop.JointType.ClavicleLeft)
|
|
{
|
|
Vector3 parJointDir = jointData.direction.normalized;
|
|
parJointDir = new Vector3(parJointDir.x, parJointDir.y, -parJointDir.z).normalized;
|
|
|
|
Vector3 fwdDir = Vector3.Cross(-jointDir, parJointDir).normalized;
|
|
Vector3 upDir = Vector3.Cross(fwdDir, -jointDir).normalized;
|
|
if (fwdDir.sqrMagnitude >= 0.5f && upDir.sqrMagnitude >= 0.5f) // avoid zero-length directions
|
|
jointOrientNormal = Quaternion.LookRotation(fwdDir, upDir);
|
|
|
|
jointData.normalRotation = jointOrientNormal;
|
|
}
|
|
else if (joint == (int)KinectInterop.JointType.ClavicleRight)
|
|
{
|
|
Vector3 parJointDir = jointData.direction.normalized;
|
|
parJointDir = new Vector3(parJointDir.x, parJointDir.y, -parJointDir.z).normalized;
|
|
|
|
Vector3 fwdDir = Vector3.Cross(jointDir, parJointDir).normalized;
|
|
Vector3 upDir = Vector3.Cross(fwdDir, jointDir).normalized;
|
|
if (fwdDir.sqrMagnitude >= 0.5f && upDir.sqrMagnitude >= 0.5f) // avoid zero-length directions
|
|
jointOrientNormal = Quaternion.LookRotation(fwdDir, upDir);
|
|
|
|
jointData.normalRotation = jointOrientNormal;
|
|
}
|
|
else
|
|
{
|
|
jointData.normalRotation = jointOrientNormal;
|
|
}
|
|
|
|
if ((joint == (int)KinectInterop.JointType.SpineNaval) ||
|
|
(joint == (int)KinectInterop.JointType.SpineChest) ||
|
|
(joint == (int)KinectInterop.JointType.Neck))
|
|
{
|
|
if(bNextJointValid && jointDir != Vector3.zero)
|
|
{
|
|
Vector3 baseDir2 = Vector3.right;
|
|
Vector3 jointDir2 = shouldersDirection;
|
|
jointDir2.z = -jointDir2.z;
|
|
|
|
jointData.normalRotation *= Quaternion.FromToRotation(baseDir2, jointDir2);
|
|
}
|
|
}
|
|
else if ((joint == (int)KinectInterop.JointType.Pelvis) ||
|
|
(joint == (int)KinectInterop.JointType.HipLeft) || (joint == (int)KinectInterop.JointType.HipRight) ||
|
|
(joint == (int)KinectInterop.JointType.KneeLeft) || (joint == (int)KinectInterop.JointType.KneeRight))
|
|
{
|
|
if (bNextJointValid && jointDir != Vector3.zero)
|
|
{
|
|
Vector3 baseDir2 = Vector3.right;
|
|
Vector3 jointDir2 = hipsDirection;
|
|
jointDir2.z = -jointDir2.z;
|
|
|
|
jointData.normalRotation *= Quaternion.FromToRotation(baseDir2, jointDir2);
|
|
}
|
|
}
|
|
else if((joint == (int)KinectInterop.JointType.AnkleLeft) || (joint == (int)KinectInterop.JointType.AnkleRight))
|
|
{
|
|
if (bNextJointValid && jointDir != Vector3.zero)
|
|
{
|
|
Vector3 ankleDir = jointData.direction;
|
|
Vector3 footDir = nextJointData.direction;
|
|
|
|
Vector3 footProj = Vector3.Project(footDir, ankleDir);
|
|
Vector3 anklePos = jointData.position + footProj;
|
|
|
|
jointDir = nextJointData.position - anklePos;
|
|
jointDir = new Vector3(jointDir.x, jointDir.y, -jointDir.z).normalized;
|
|
|
|
Vector3 parJointDir = jointData.direction.normalized;
|
|
parJointDir = new Vector3(parJointDir.x, parJointDir.y, -parJointDir.z).normalized;
|
|
|
|
jointOrientNormal = Quaternion.LookRotation(jointDir, -parJointDir);
|
|
jointData.normalRotation = jointOrientNormal;
|
|
}
|
|
}
|
|
else if(joint == (int)KinectInterop.JointType.Head)
|
|
{
|
|
if (bNextJointValid && jointDir != Vector3.zero &&
|
|
bodyData.joint[(int)KinectInterop.JointType.EarRight].trackingState != KinectInterop.TrackingState.NotTracked &&
|
|
bodyData.joint[(int)KinectInterop.JointType.EarLeft].trackingState != KinectInterop.TrackingState.NotTracked)
|
|
{
|
|
Vector3 posREar = bodyData.joint[(int)KinectInterop.JointType.EarRight].position;
|
|
Vector3 posLEar = bodyData.joint[(int)KinectInterop.JointType.EarLeft].position;
|
|
|
|
Vector3 earsDir = posREar - posLEar;
|
|
earsDir = new Vector3(earsDir.x, earsDir.y, -earsDir.z).normalized;
|
|
|
|
Vector3 headUpDir = -Vector3.Cross(earsDir, jointDir);
|
|
jointOrientNormal = Quaternion.LookRotation(jointDir, headUpDir);
|
|
jointOrientNormal *= Quaternion.Euler(20f, 0, 0); // turn head 20 deg down
|
|
|
|
jointData.normalRotation = jointOrientNormal;
|
|
}
|
|
}
|
|
|
|
Vector3 mirroredAngles = jointData.normalRotation.eulerAngles;
|
|
mirroredAngles.y = -mirroredAngles.y;
|
|
mirroredAngles.z = -mirroredAngles.z;
|
|
|
|
jointData.mirroredRotation = Quaternion.Euler(mirroredAngles);
|
|
}
|
|
else
|
|
{
|
|
// get the orientation of the parent joint
|
|
int prevJoint = (int)KinectInterop.GetParentJoint((KinectInterop.JointType)joint);
|
|
if (prevJoint != joint && prevJoint >= 0 && prevJoint < jointCount)
|
|
{
|
|
jointData.normalRotation = bodyData.joint[prevJoint].normalRotation;
|
|
jointData.mirroredRotation = bodyData.joint[prevJoint].mirroredRotation;
|
|
}
|
|
else
|
|
{
|
|
jointData.normalRotation = Quaternion.identity;
|
|
jointData.mirroredRotation = Quaternion.identity;
|
|
}
|
|
}
|
|
|
|
bodyData.joint[joint] = jointData;
|
|
}
|
|
else
|
|
{
|
|
// joint is not tracked
|
|
}
|
|
|
|
if (joint == (int)KinectInterop.JointType.Pelvis)
|
|
{
|
|
bodyData.normalRotation = jointData.normalRotation;
|
|
bodyData.mirroredRotation = jointData.mirroredRotation;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
protected static readonly Vector3[] KinectJointBaseDir =
|
|
{
|
|
Vector3.zero,
|
|
Vector3.up,
|
|
Vector3.up,
|
|
Vector3.up,
|
|
Vector3.up, // (Vector3.up + Vector3.forward).normalized,
|
|
|
|
Vector3.left,
|
|
Vector3.left,
|
|
Vector3.left,
|
|
Vector3.left,
|
|
Vector3.left,
|
|
|
|
Vector3.right,
|
|
Vector3.right,
|
|
Vector3.right,
|
|
Vector3.right,
|
|
Vector3.right,
|
|
|
|
Vector3.down,
|
|
Vector3.down,
|
|
Vector3.down,
|
|
Vector3.forward,
|
|
|
|
Vector3.down,
|
|
Vector3.down,
|
|
Vector3.down,
|
|
Vector3.forward,
|
|
|
|
Vector3.forward,
|
|
Vector3.left,
|
|
Vector3.left,
|
|
Vector3.right,
|
|
Vector3.right,
|
|
|
|
Vector3.left,
|
|
Vector3.forward,
|
|
Vector3.right,
|
|
Vector3.forward,
|
|
};
|
|
|
|
// returns the sensor-data for network exchange
|
|
public KinectInterop.NetSensorData GetNetSensorData(KinectInterop.SensorData sensorData)
|
|
{
|
|
if (sensorData == null)
|
|
return null;
|
|
|
|
KinectInterop.NetSensorData netSensorData = new KinectInterop.NetSensorData();
|
|
|
|
netSensorData.sensorType = (int)sensorData.sensorInterface.GetSensorPlatform();
|
|
netSensorData.sensorId = sensorDeviceId;
|
|
|
|
netSensorData.colorImageWidth = sensorData.colorImageWidth;
|
|
netSensorData.colorImageHeight = sensorData.colorImageHeight;
|
|
|
|
netSensorData.depthImageWidth = sensorData.depthImageWidth;
|
|
netSensorData.depthImageHeight = sensorData.depthImageHeight;
|
|
|
|
netSensorData.depthCamIntr = sensorData.depthCamIntr;
|
|
netSensorData.colorCamIntr = sensorData.colorCamIntr;
|
|
netSensorData.depth2ColorExtr = sensorData.depth2ColorExtr;
|
|
netSensorData.color2DepthExtr = sensorData.color2DepthExtr;
|
|
|
|
netSensorData.colorImageScale = sensorData.colorImageScale;
|
|
netSensorData.depthImageScale = sensorData.depthImageScale;
|
|
netSensorData.infraredImageScale = sensorData.infraredImageScale;
|
|
netSensorData.sensorSpaceScale = sensorData.sensorSpaceScale;
|
|
netSensorData.unitToMeterFactor = sensorData.unitToMeterFactor;
|
|
|
|
netSensorData.sensorPosition = sensorPosePosition;
|
|
netSensorData.sensorRotation = sensorPoseRotation.eulerAngles;
|
|
|
|
netSensorData.sensorRotOffset = sensorRotOffset;
|
|
netSensorData.sensorRotFlipZ = sensorRotFlipZ;
|
|
netSensorData.sensorRotIgnoreY = sensorRotIgnoreY;
|
|
|
|
return netSensorData;
|
|
}
|
|
|
|
// sets the local sensor data from the network exchange data
|
|
public void SetNetSensorData(KinectInterop.NetSensorData netSensorData, KinectInterop.SensorData sensorData, KinectManager kinectManager)
|
|
{
|
|
if (netSensorData == null || sensorData == null)
|
|
return;
|
|
|
|
sensorDeviceId = netSensorData.sensorId;
|
|
sensorPlatform = (KinectInterop.DepthSensorPlatform)netSensorData.sensorType;
|
|
//Debug.Log("NetSensor is " + sensorPlatform + ", id: " + netSensorData.sensorId);
|
|
|
|
sensorData.colorImageWidth = netSensorData.colorImageWidth;
|
|
sensorData.colorImageHeight = netSensorData.colorImageHeight;
|
|
|
|
sensorData.depthImageWidth = netSensorData.depthImageWidth;
|
|
sensorData.depthImageHeight = netSensorData.depthImageHeight;
|
|
|
|
sensorData.depthCamIntr = netSensorData.depthCamIntr;
|
|
sensorData.colorCamIntr = netSensorData.colorCamIntr;
|
|
sensorData.depth2ColorExtr = netSensorData.depth2ColorExtr;
|
|
sensorData.color2DepthExtr = netSensorData.color2DepthExtr;
|
|
|
|
sensorData.colorImageScale = netSensorData.colorImageScale;
|
|
sensorData.depthImageScale = netSensorData.depthImageScale;
|
|
sensorData.infraredImageScale = netSensorData.infraredImageScale;
|
|
sensorData.sensorSpaceScale = netSensorData.sensorSpaceScale;
|
|
sensorData.unitToMeterFactor = netSensorData.unitToMeterFactor;
|
|
|
|
if (kinectManager != null && kinectManager.getPoseFrames != KinectManager.PoseUsageType.None)
|
|
{
|
|
lock (poseFrameLock)
|
|
{
|
|
transform.position = sensorData.sensorPosePosition = sensorPosePosition = netSensorData.sensorPosition;
|
|
transform.rotation = sensorData.sensorPoseRotation = sensorPoseRotation = Quaternion.Euler(netSensorData.sensorRotation);
|
|
}
|
|
}
|
|
|
|
sensorRotOffset = netSensorData.sensorRotOffset;
|
|
sensorRotFlipZ = netSensorData.sensorRotFlipZ;
|
|
sensorRotIgnoreY = netSensorData.sensorRotIgnoreY;
|
|
}
|
|
|
|
// returns the sensor pose data for network exchange
|
|
public KinectInterop.NetPoseData GetSensorNetPoseData(KinectInterop.SensorData sensorData)
|
|
{
|
|
if (sensorData == null)
|
|
return null;
|
|
|
|
KinectInterop.NetPoseData netPoseData = new KinectInterop.NetPoseData();
|
|
|
|
netPoseData.sensorPosition = sensorPosePosition;
|
|
netPoseData.sensorRotation = sensorPoseRotation.eulerAngles;
|
|
netPoseData.sensorPoseTime = currentPoseTimestamp;
|
|
|
|
return netPoseData;
|
|
}
|
|
|
|
// sets the local sensor pose data from the network exchange data
|
|
public void SetSensorNetPoseData(KinectInterop.NetPoseData netPoseData, KinectInterop.SensorData sensorData, KinectManager kinectManager)
|
|
{
|
|
if (netPoseData == null || sensorData == null)
|
|
return;
|
|
|
|
lock (poseFrameLock)
|
|
{
|
|
sensorData.sensorPosePosition = sensorPosePosition = netPoseData.sensorPosition;
|
|
sensorData.sensorPoseRotation = sensorPoseRotation = Quaternion.Euler(netPoseData.sensorRotation);
|
|
sensorData.lastSensorPoseFrameTime = currentPoseTimestamp = rawPoseTimestamp = netPoseData.sensorPoseTime;
|
|
}
|
|
|
|
// apply sensor pose update to the transform, if needed
|
|
//ApplySensorPoseUpdate(kinectManager);
|
|
}
|
|
|
|
// enables or disables depth camera color frame processing
|
|
public virtual void EnableDepthCameraColorFrame(KinectInterop.SensorData sensorData, bool isEnable)
|
|
{
|
|
if (isEnable && pointCloudColorTexture == null)
|
|
{
|
|
pointCloudColorTexture = KinectInterop.CreateRenderTexture(pointCloudColorTexture,
|
|
sensorData.depthImageWidth, sensorData.depthImageHeight, RenderTextureFormat.ARGB32);
|
|
sensorData.depthCamColorImageTexture = new Texture2D(sensorData.depthImageWidth, sensorData.depthImageHeight, sensorData.colorImageFormat, false);
|
|
}
|
|
else if(!isEnable && pointCloudColorTexture != null)
|
|
{
|
|
pointCloudColorTexture.Release();
|
|
pointCloudColorTexture = null;
|
|
|
|
sensorData.depthCamColorImageTexture = null;
|
|
}
|
|
}
|
|
|
|
// returns the latest depth camera color frame texture along with the last frame time
|
|
public virtual Texture GetDepthCameraColorFrameTexture(KinectInterop.SensorData sensorData, ref Texture2D copyToTex2D, ref ulong frameTime)
|
|
{
|
|
if (sensorData.depthCamColorImageTexture == null)
|
|
return null;
|
|
|
|
lock (depthCamColorFrameLock)
|
|
{
|
|
if(copyToTex2D != null && sensorData.lastDepthCamColorFrameTime != frameTime)
|
|
{
|
|
//KinectInterop.RenderTex2Tex2D(sensorData.depthCamColorImageTexture, ref copyToTex2D);
|
|
Graphics.CopyTexture(sensorData.depthCamColorImageTexture, copyToTex2D);
|
|
}
|
|
|
|
frameTime = sensorData.lastDepthCamColorFrameTime;
|
|
}
|
|
|
|
return sensorData.depthCamColorImageTexture;
|
|
}
|
|
|
|
// enables or disables color camera depth frame processing
|
|
public virtual void EnableColorCameraDepthFrame(KinectInterop.SensorData sensorData, bool isEnable)
|
|
{
|
|
if (isEnable && colorCamDepthDataFrame == null)
|
|
{
|
|
colorCamDepthDataFrame = new ushort[sensorData.colorImageWidth * sensorData.colorImageHeight];
|
|
sensorData.colorCamDepthImage = new ushort[sensorData.colorImageWidth * sensorData.colorImageHeight];
|
|
}
|
|
else if(!isEnable && colorCamDepthDataFrame != null)
|
|
{
|
|
colorCamDepthDataFrame = null;
|
|
sensorData.colorCamDepthImage = null;
|
|
}
|
|
}
|
|
|
|
// returns the latest color camera depth frame along with the last frame time. the returned data frame is ushort array.
|
|
public virtual ushort[] GetColorCameraDepthFrame(KinectInterop.SensorData sensorData, ref ushort[] copyToFrame, ref ulong frameTime)
|
|
{
|
|
if (sensorData.colorCamDepthImage == null)
|
|
return null;
|
|
|
|
lock (colorCamDepthFrameLock)
|
|
{
|
|
if (copyToFrame != null && sensorData.lastColorCamDepthFrameTime != frameTime)
|
|
{
|
|
KinectInterop.CopyBytes(sensorData.colorCamDepthImage, sizeof(ushort), copyToFrame, sizeof(ushort));
|
|
}
|
|
|
|
frameTime = sensorData.lastColorCamDepthFrameTime;
|
|
}
|
|
|
|
return sensorData.colorCamDepthImage;
|
|
}
|
|
|
|
// returns the latest color camera depth frame along with the last frame time. the returned data frame is byte array.
|
|
public virtual byte[] GetColorCameraDepthFrameBytes(KinectInterop.SensorData sensorData, ref byte[] copyToFrame, ref ulong frameTime)
|
|
{
|
|
if (sensorData.colorCamDepthImage == null)
|
|
return null;
|
|
|
|
byte[] frameData = null;
|
|
|
|
lock (colorCamDepthFrameLock)
|
|
{
|
|
if (copyToFrame != null && sensorData.lastColorCamDepthFrameTime != frameTime)
|
|
{
|
|
KinectInterop.CopyBytes(sensorData.colorCamDepthImage, sizeof(ushort), copyToFrame, sizeof(byte));
|
|
frameData = copyToFrame;
|
|
}
|
|
|
|
if(frameData == null)
|
|
{
|
|
frameData = new byte[sensorData.colorCamDepthImage.Length * sizeof(ushort)];
|
|
KinectInterop.CopyBytes(sensorData.colorCamDepthImage, sizeof(ushort), frameData, sizeof(byte));
|
|
}
|
|
|
|
frameTime = sensorData.lastColorCamDepthFrameTime;
|
|
}
|
|
|
|
return frameData;
|
|
}
|
|
|
|
// enables or disables color camera infrared frame processing
|
|
public virtual void EnableColorCameraInfraredFrame(KinectInterop.SensorData sensorData, bool isEnableRawData, bool isEnableTexture)
|
|
{
|
|
if (isEnableRawData && colorCamInfraredDataFrame == null)
|
|
{
|
|
colorCamInfraredDataFrame = new ushort[sensorData.colorImageWidth * sensorData.colorImageHeight];
|
|
sensorData.colorCamInfraredImage = new ushort[sensorData.colorImageWidth * sensorData.colorImageHeight];
|
|
}
|
|
else if (!isEnableRawData && colorCamInfraredDataFrame != null)
|
|
{
|
|
colorCamInfraredDataFrame = null;
|
|
sensorData.colorCamInfraredImage = null;
|
|
}
|
|
|
|
if(isEnableTexture && sensorData.colorInfraredBuffer == null)
|
|
{
|
|
if(sensorData.colorImageWidth > 0 && sensorData.colorImageHeight > 0)
|
|
{
|
|
int bufferLength = sensorData.colorImageWidth * sensorData.colorImageHeight / 2;
|
|
sensorData.colorInfraredBuffer = new ComputeBuffer(bufferLength, sizeof(uint));
|
|
}
|
|
}
|
|
else if(!isEnableTexture && sensorData.colorInfraredBuffer != null)
|
|
{
|
|
sensorData.colorInfraredBuffer.Release();
|
|
sensorData.colorInfraredBuffer.Dispose();
|
|
sensorData.colorInfraredBuffer = null;
|
|
}
|
|
}
|
|
|
|
// returns the latest color camera infrared frame along with the last frame time. the returned data frame is ushort array.
|
|
public virtual ushort[] GetColorCameraInfraredFrame(KinectInterop.SensorData sensorData, ref ushort[] copyToFrame, ref ulong frameTime)
|
|
{
|
|
if (sensorData.colorCamInfraredImage == null)
|
|
return null;
|
|
|
|
lock (colorCamInfraredFrameLock)
|
|
{
|
|
if (copyToFrame != null && sensorData.lastColorCamInfraredFrameTime != frameTime)
|
|
{
|
|
KinectInterop.CopyBytes(sensorData.colorCamInfraredImage, sizeof(ushort), copyToFrame, sizeof(ushort));
|
|
}
|
|
|
|
frameTime = sensorData.lastColorCamInfraredFrameTime;
|
|
}
|
|
|
|
return sensorData.colorCamInfraredImage;
|
|
}
|
|
|
|
//// returns the latest color camera infrared frame along with the last frame time. the returned data frame is byte array.
|
|
//public virtual byte[] GetColorCameraInfraredFrameBytes(KinectInterop.SensorData sensorData, ref byte[] copyToFrame, ref ulong frameTime)
|
|
//{
|
|
// if (sensorData.colorCamInfraredImage == null)
|
|
// return null;
|
|
|
|
// byte[] frameData = null;
|
|
|
|
// lock (colorCamInfraredFrameLock)
|
|
// {
|
|
// if (copyToFrame != null && sensorData.lastColorCamInfraredFrameTime != frameTime)
|
|
// {
|
|
// KinectInterop.CopyBytes(sensorData.colorCamInfraredImage, sizeof(ushort), copyToFrame, sizeof(byte));
|
|
// frameData = copyToFrame;
|
|
// }
|
|
|
|
// if (frameData == null)
|
|
// {
|
|
// frameData = new byte[sensorData.colorCamInfraredImage.Length * sizeof(ushort)];
|
|
// KinectInterop.CopyBytes(sensorData.colorCamInfraredImage, sizeof(ushort), frameData, sizeof(byte));
|
|
// }
|
|
|
|
// frameTime = sensorData.lastColorCamInfraredFrameTime;
|
|
// }
|
|
|
|
// return frameData;
|
|
//}
|
|
|
|
// returns the latest color camera infrared frame texture along with the last frame time
|
|
public virtual Texture GetColorCameraInfraredFrameTexture(KinectInterop.SensorData sensorData, ref Texture2D copyToTex2D, ref ulong frameTime)
|
|
{
|
|
if (sensorData.colorInfraredTexture == null)
|
|
return null;
|
|
|
|
//lock (colorCamInfraredFrameLock)
|
|
{
|
|
if (copyToTex2D != null && sensorData.lastInfraredImageTime != frameTime)
|
|
{
|
|
// CopyTexture() doesn't work in this case, because it's on GPU, while EncodeToJPG() is on CPU side.
|
|
// see: https://answers.unity.com/questions/1479723/encodetojpg-creates-gray-image-after-using-copytex.html
|
|
//Graphics.CopyTexture(sensorData.colorInfraredTexture, copyToTex2D);
|
|
KinectInterop.RenderTex2Tex2D(sensorData.colorInfraredTexture, ref copyToTex2D);
|
|
}
|
|
|
|
frameTime = sensorData.lastInfraredImageTime;
|
|
//Debug.Log("LastInfraredImageTime: " + sensorData.lastInfraredImageTime);
|
|
}
|
|
|
|
return sensorData.colorInfraredTexture;
|
|
}
|
|
|
|
// enables or disables color camera body-index frame processing
|
|
public virtual void EnableColorCameraBodyIndexFrame(KinectInterop.SensorData sensorData, bool isEnable)
|
|
{
|
|
if (isEnable && colorCamBodyIndexFrame == null)
|
|
{
|
|
colorCamBodyIndexFrame = new byte[sensorData.colorImageWidth * sensorData.colorImageHeight];
|
|
sensorData.colorCamBodyIndexImage = new byte[sensorData.colorImageWidth * sensorData.colorImageHeight];
|
|
}
|
|
else if (!isEnable && colorCamBodyIndexFrame != null)
|
|
{
|
|
colorCamBodyIndexFrame = null;
|
|
sensorData.colorCamBodyIndexImage = null;
|
|
}
|
|
}
|
|
|
|
// returns the latest color camera body-index frame along with the last frame time
|
|
public virtual byte[] GetColorCameraBodyIndexFrame(KinectInterop.SensorData sensorData, ref byte[] copyToFrame, ref ulong frameTime)
|
|
{
|
|
if (sensorData.colorCamBodyIndexImage == null)
|
|
return null;
|
|
|
|
lock (colorCamDepthFrameLock)
|
|
{
|
|
if (copyToFrame != null && sensorData.lastColorCamBodyIndexFrameTime != frameTime)
|
|
{
|
|
KinectInterop.CopyBytes(sensorData.colorCamBodyIndexImage, sizeof(byte), copyToFrame, sizeof(byte));
|
|
}
|
|
|
|
frameTime = sensorData.lastColorCamBodyIndexFrameTime;
|
|
}
|
|
|
|
return sensorData.colorCamBodyIndexImage;
|
|
}
|
|
|
|
}
|
|
}
|
|
|