public void SetData(ushort[] data)
{
int frameLen = data.Length * sizeof(ushort);
frameData = new byte[frameLen];
KinectInterop.CopyBytes(data, sizeof(ushort), frameData, sizeof(byte));
}
// sends the net-cst data to clients
private void CtrlSendGetCST()
{
Vector3[] avSpaceTable = sensorData.sensorInterface.GetColorCameraSpaceTable(sensorData);
if (avSpaceTable != null)
{
byte[] btSpaceTable = new byte[avSpaceTable.Length * 3 * sizeof(float)];
KinectInterop.CopyBytes(avSpaceTable, 3 * sizeof(float), btSpaceTable, sizeof(byte));
ulong ulTimeNow = (ulong)System.DateTime.Now.Ticks;
NetMessageData message = new NetMessageData(NetMessageType.Control, FrameEncodeType.CSTraw, sensorData.colorImageWidth, sensorData.colorImageHeight, ulTimeNow);
message.SetData(btSpaceTable);
if (compressRawFrames)
{
message.SetCompressor(controlFrameCompressor);
}
controlFrameServer.SendMessageToAllConnections(message);
}
}
// converts the raw image data to capture
protected Capture GetBodyTrackerCapture(KinectInterop.SensorData sensorData, RealSenseFrames rsFrames)
{
Capture capture = new Capture();
int depthW = sensorData.depthImageWidth;
int depthH = sensorData.depthImageHeight;
if (rsFrames.colorFrame != null && rawColorImage != null)
{
Image colorImage = new Image(ImageFormat.ColorBGRA32, sensorData.colorImageWidth, sensorData.colorImageHeight, sensorData.colorImageWidth * 4);
KinectInterop.CopyBytes(rsFrames.colorFrame.Data, colorImage.GetBuffer(), rawColorImage.Length);
colorImage.DeviceTimestamp = TimeSpan.FromTicks((long)rsFrames.deviceTimestamp);
capture.Color = colorImage;
}
if (rsFrames.depthFrame != null && rawDepthImage != null)
{
Image depthImage = new Image(ImageFormat.Depth16, depthW, depthH, depthW * sizeof(ushort));
KinectInterop.CopyBytes(rsFrames.depthFrame.Data, depthImage.GetBuffer(), rawDepthImage.Length * sizeof(ushort));
depthImage.DeviceTimestamp = TimeSpan.FromTicks((long)rsFrames.deviceTimestamp);
capture.Depth = depthImage;
}
if (rsFrames.infraredFrame != null && rawInfraredImage2 != null && rawInfraredImageBT != null)
{
Image infraredFrame = new Image(ImageFormat.IR16, depthW, depthH, depthW * sizeof(ushort));
rsFrames.infraredFrame.CopyTo <byte>(rawInfraredImage2);
for (int i = 0; i < rawInfraredImage2.Length; i++)
{
rawInfraredImageBT[i] = (ushort)(rawInfraredImage2[i] << 4);
}
KinectInterop.CopyBytes(rawInfraredImageBT, rawInfraredImageBT.Length, sizeof(ushort), infraredFrame.GetBuffer(), (int)infraredFrame.Size);
infraredFrame.DeviceTimestamp = TimeSpan.FromTicks((long)rsFrames.deviceTimestamp);
capture.IR = infraredFrame;
}
return(capture);
}
// processes the camera frames
private void ProcessCameraFrames(KinectInterop.SensorData sensorData, Capture capture)
{
//// check for color & depth sync
//if (isSyncDepthAndColor && (capture.Color == null || capture.Depth == null))
// return;
Capture btCapture = null;
try
{
if (bodyTracker != null)
{
// check for available body frame
btCapture = PollBodyFrame(sensorData, capture);
if (isSyncBodyAndDepth)
{
// process the body tracking capture
capture = btCapture;
//if (btCapture != null && btCapture.Depth != null)
// Debug.Log("D" + deviceIndex + " PolledBtCapture, Timestamp: " + btCapture.Depth.DeviceTimestamp.Ticks);
}
}
// check for body & depth sync
if (!isSyncBodyAndDepth || btCapture != null /**&& (btQueueCount == 0 && sensorData.lastBodyFrameTime == rawDepthTimestamp)*/) // currentBodyTimestamp
{
ulong depthFrameTime = isSyncDepthAndColor && capture != null && capture.Depth != null ? (ulong)capture.Depth.DeviceTimestamp.Ticks : 0;
// depth frame
if (capture.Depth != null && rawDepthImage != null && rawDepthTimestamp == sensorData.lastDepthFrameTime)
{
//if (kinectPlayback != null)
// WaitForPlaybackTimestamp("depth", capture.Depth.DeviceTimestamp.Ticks);
lock (depthFrameLock)
{
//capture.Depth.CopyTo(rawDepthImage, 0, 0, rawDepthImage.Length);
KinectInterop.CopyBytes(capture.Depth.GetBuffer(), (int)capture.Depth.Size, rawDepthImage, rawDepthImage.Length, sizeof(ushort));
rawDepthTimestamp = (ulong)capture.Depth.DeviceTimestamp.Ticks;
//Debug.Log("D" + deviceIndex + " RawDepthTimestamp: " + rawDepthTimestamp);
}
}
// color frame
if (capture.Color != null && rawColorImage != null && rawColorTimestamp == sensorData.lastColorFrameTime)
{
//if (kinectPlayback != null)
// WaitForPlaybackTimestamp("color", capture.Color.DeviceTimestamp.Ticks);
lock (colorFrameLock)
{
//capture.Color.CopyBytesTo(rawColorImage, 0, 0, rawColorImage.Length);
KinectInterop.CopyBytes(capture.Color.GetBuffer(), (int)capture.Color.Size, rawColorImage, rawColorImage.Length, sizeof(byte));
rawColorTimestamp = depthFrameTime != 0 ? depthFrameTime : (ulong)capture.Color.DeviceTimestamp.Ticks;
//Debug.Log("D" + deviceIndex + " RawColorTimestamp: " + rawColorTimestamp);
}
}
// infrared frame
if (capture.IR != null && rawInfraredImage != null && rawInfraredTimestamp == sensorData.lastInfraredFrameTime)
{
//if (kinectPlayback != null)
// WaitForPlaybackTimestamp("ir", capture.IR.DeviceTimestamp.Ticks);
lock (infraredFrameLock)
{
//capture.IR.CopyTo(rawInfraredImage, 0, 0, rawInfraredImage.Length);
KinectInterop.CopyBytes(capture.IR.GetBuffer(), (int)capture.IR.Size, rawInfraredImage, rawInfraredImage.Length, sizeof(ushort));
rawInfraredTimestamp = depthFrameTime != 0 ? depthFrameTime : (ulong)capture.IR.DeviceTimestamp.Ticks;
//Debug.Log("D" + deviceIndex + " RawInfraredTimestamp: " + rawInfraredTimestamp);
}
}
// transformation data frames
if ((depthCamColorDataFrame != null || colorCamDepthDataFrame != null) && capture.Color != null && capture.Depth != null &&
(!isSyncDepthAndColor || (rawColorTimestamp == rawDepthTimestamp)))
{
if (coordMapperTransform == null)
{
coordMapperTransform = coordMapperCalib.CreateTransformation();
}
//ulong depthFrameDeviceTimestamp = (ulong)capture.Depth.DeviceTimestamp.Ticks;
if (depthCamColorDataFrame != null && lastDepthCamColorFrameTime != rawDepthTimestamp)
{
if (d2cColorData == null)
{
d2cColorData = new Image(ImageFormat.ColorBGRA32, sensorData.depthImageWidth, sensorData.depthImageHeight, sensorData.depthImageWidth * sensorData.colorImageStride);
}
lock (depthCamColorFrameLock)
{
coordMapperTransform.ColorImageToDepthCamera(capture.Depth, capture.Color, d2cColorData);
d2cColorData.CopyTo <byte>(depthCamColorDataFrame, 0, 0, depthCamColorDataFrame.Length);
lastDepthCamColorFrameTime = rawDepthTimestamp;
//Debug.Log("D" + deviceIndex + " DepthCamColorFrameTime: " + lastDepthCamColorFrameTime);
}
}
if (colorCamDepthDataFrame != null && lastColorCamDepthFrameTime != rawDepthTimestamp)
{
if (c2dDepthData == null)
{
c2dDepthData = new Image(ImageFormat.Depth16, sensorData.colorImageWidth, sensorData.colorImageHeight, sensorData.colorImageWidth * sizeof(ushort));
}
lock (colorCamDepthFrameLock)
{
coordMapperTransform.DepthImageToColorCamera(capture.Depth, c2dDepthData);
c2dDepthData.CopyTo <ushort>(colorCamDepthDataFrame, 0, 0, colorCamDepthDataFrame.Length);
lastColorCamDepthFrameTime = rawDepthTimestamp;
//Debug.Log("D" + deviceIndex + " ColorCamDepthFrameTime: " + lastColorCamDepthFrameTime);
}
}
}
}
else
{
// ignore the capture
capture = null;
}
if (btCapture != null)
{
// dispose body capture
btCapture.Dispose();
}
}
catch (System.Exception ex)
{
Debug.LogException(ex);
}
}
public virtual bool UpdateSensorData(KinectInterop.SensorData sensorData, KinectManager kinectManager)
{
// color frame
lock (colorFrameLock)
{
if (rawColorImage != null && sensorData.lastColorFrameTime != currentColorTimestamp)
{
Texture2D colorImageTex2D = sensorData.colorImageTexture as Texture2D;
if (colorImageTex2D != null)
{
colorImageTex2D.LoadRawTextureData(rawColorImage);
colorImageTex2D.Apply();
}
sensorData.lastColorFrameTime = currentColorTimestamp;
//Debug.Log("Color frame. Timestamp: " + currentColorTimestamp);
}
}
// depth frame
lock (depthFrameLock)
{
if (rawDepthImage != null && sensorData.lastDepthFrameTime != currentDepthTimestamp)
{
// 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("Depth frame. Timestamp: " + currentDepthTimestamp);
}
}
// depth hist frame
lock (depthImageDataLock)
{
if (equalHistBufferData != null && sensorData.lastDepthHistTime != lastDepthImageTimestamp)
{
if (sensorData.depthHistBufferData != null)
{
KinectInterop.CopyBytes(equalHistBufferData, sizeof(int), sensorData.depthHistBufferData, sizeof(int));
}
sensorData.depthHistTotalPoints = histDataTotalPoints;
sensorData.lastDepthHistTime = lastDepthImageTimestamp;
//Debug.Log("Depth hist frame. Timestamp: " + lastDepthImageTimestamp);
}
}
// infrared frame
lock (infraredFrameLock)
{
if (rawInfraredImage != null && sensorData.lastInfraredFrameTime != currentInfraredTimestamp)
{
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("IR frame. Timestamp: " + currentDepthTimestamp);
}
}
return(true);
}
private void ProcessCameraFrames(KinectInterop.SensorData sensorData, RealSenseFrames frames)
{
Capture btCapture = null;
//if (bodyTracker != null)
//{
// // body frame
// if (frames.depthFrame != null && frames.infraredFrame != null)
// {
// Capture capture = GetBodyTrackerCapture(sensorData, frames);
// btCapture = PollBodyFrame(sensorData, capture);
// capture?.Dispose();
// }
//}
// check for body & depth sync
if (!isSyncBodyAndDepth || btCapture != null /**|| bodyTracker == null*/)
{
if (isSyncBodyAndDepth && btCapture != null)
{
ulong depthFrameTime = isSyncDepthAndColor && btCapture.Depth != null ? (ulong)btCapture.Depth.DeviceTimestamp.Ticks : 0;
// body-tracker frame
if (btCapture.Depth != null && rawDepthImage != null && (getAllSensorFrames || rawDepthTimestamp == sensorData.lastDepthFrameTime))
{
lock (depthFrameLock)
{
//btCapture.Depth.CopyTo(rawDepthImage, 0, 0, rawDepthImage.Length);
KinectInterop.CopyBytes(btCapture.Depth.GetBuffer(), (int)btCapture.Depth.Size, rawDepthImage, rawDepthImage.Length, sizeof(ushort));
rawDepthTimestamp = (ulong)btCapture.Depth.DeviceTimestamp.Ticks;
//Debug.Log("D" + deviceIndex + " RawDepthTimestamp: " + rawDepthTimestamp);
}
}
if (btCapture.Color != null && rawColorImage != null && (getAllSensorFrames || rawColorTimestamp == sensorData.lastColorFrameTime))
{
lock (colorFrameLock)
{
//btCapture.Color.CopyBytesTo(rawColorImage, 0, 0, rawColorImage.Length);
KinectInterop.CopyBytes(btCapture.Color.GetBuffer(), (int)btCapture.Color.Size, rawColorImage, rawColorImage.Length, sizeof(byte));
rawColorTimestamp = depthFrameTime != 0 ? depthFrameTime : (ulong)btCapture.Color.DeviceTimestamp.Ticks;
//Debug.Log("D" + deviceIndex + " RawColorTimestamp: " + rawColorTimestamp);
}
}
if (btCapture.IR != null && rawInfraredImage != null && (getAllSensorFrames || rawInfraredTimestamp == sensorData.lastInfraredFrameTime))
{
lock (infraredFrameLock)
{
//btCapture.IR.CopyTo(rawInfraredImage, 0, 0, rawInfraredImage.Length);
KinectInterop.CopyBytes(btCapture.IR.GetBuffer(), (int)btCapture.IR.Size, rawInfraredImage, rawInfraredImage.Length, sizeof(ushort));
rawInfraredTimestamp = depthFrameTime != 0 ? depthFrameTime : (ulong)btCapture.IR.DeviceTimestamp.Ticks;
//Debug.Log("D" + deviceIndex + " RawInfraredTimestamp: " + rawInfraredTimestamp);
}
}
}
else
{
// sensor frame
ulong depthFrameTime = isSyncDepthAndColor && frames.depthFrame != null ? frames.deviceTimestamp : 0;
if (frames.depthFrame != null && rawDepthImage != null && (getAllSensorFrames || rawDepthTimestamp == sensorData.lastDepthFrameTime))
{
lock (depthFrameLock)
{
frames.depthFrame.CopyTo <ushort>(rawDepthImage);
rawDepthTimestamp = frames.deviceTimestamp;
//Debug.Log("D" + deviceIndex + " RawDepthTimestamp: " + rawDepthTimestamp);
}
}
if (frames.colorFrame != null && rawColorImage != null && (getAllSensorFrames || rawColorTimestamp == sensorData.lastColorFrameTime))
{
lock (colorFrameLock)
{
KinectInterop.CopyBytes(frames.colorFrame.Data, rawColorImage.Length, rawColorImage, rawColorImage.Length, sizeof(byte));
rawColorTimestamp = depthFrameTime != 0 ? depthFrameTime : frames.deviceTimestamp;
//Debug.Log("D" + deviceIndex + " RawColorTimestamp: " + rawColorTimestamp);
}
}
if (frames.infraredFrame != null && rawInfraredImage != null && (getAllSensorFrames || rawInfraredTimestamp == sensorData.lastInfraredFrameTime))
{
lock (infraredFrameLock)
{
frames.infraredFrame.CopyTo <byte>(rawInfraredImage1);
for (int i = 0; i < rawInfraredImage1.Length; i++)
{
rawInfraredImage[i] = (ushort)(rawInfraredImage1[i] << 4);
}
rawInfraredTimestamp = depthFrameTime != 0 ? depthFrameTime : frames.deviceTimestamp;
//Debug.Log("D" + deviceIndex + " RawInfraredTimestamp: " + rawInfraredTimestamp);
}
}
}
}
// color & depth stream profiles
if (frames.colorFrame != null)
{
colorStreamProfile = frames.colorFrame.Profile.As <VideoStreamProfile>();
}
if (frames.depthFrame != null)
{
depthStreamProfile = frames.depthFrame.Profile.As <VideoStreamProfile>();
}
// dispose body capture
if (btCapture != null)
{
btCapture.Dispose();
}
// check for pose frame
if (frames.poseFrame != null)
{
frames.poseFrame.CopyTo(out rsPoseData);
lock (poseFrameLock)
{
rawPosePosition = new Vector3(rsPoseData.translation.x, rsPoseData.translation.y, -rsPoseData.translation.z); // (1, 1, -1)
rawPoseRotation = new Quaternion(-rsPoseData.rotation.x, -rsPoseData.rotation.y, rsPoseData.rotation.z, rsPoseData.rotation.w); // (-1, -1, 1, 1);
rawPoseTimestamp = frames.deviceTimestamp;
//Debug.Log("D" + deviceIndex + " RawPoseTimestamp: " + rawPoseTimestamp);
}
}
}