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