dwelling act 4 (live motion cap w/ kinect azure)
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.

1094 lines
47 KiB

1 year ago
using UnityEngine;
using System.Collections;
using System.Collections.Generic;
using Intel.RealSense;
using System.Threading;
using System;
using System.Linq;
using System.Runtime.InteropServices;
namespace com.rfilkov.kinect
{
/// <summary>
/// RealSenseInterface is sensor-interface to the RealSense sensors.
/// </summary>
public class RealSenseInterface : DepthSensorBase
{
[Tooltip("Color camera resolution.")]
public ColorCameraMode colorCameraMode = ColorCameraMode._640_x_480_30Fps;
public enum ColorCameraMode : int { _320_x_180_30Fps = 10, _320_x_240_30Fps = 15, _424_x_240_30Fps = 20, _640_x_360_30Fps = 30, _640_x_480_30Fps = 35, _848_x_480_30Fps = 40, _1280_x_720_30Fps = 60, _1920_x_1080_30Fps = 70 }
[Tooltip("Depth camera resolution.")]
public DepthCameraMode depthCameraMode = DepthCameraMode._640_x_480_30Fps;
public enum DepthCameraMode : int { _424_x_240_30Fps = 20, _480_x_270_30Fps = 25, _640_x_360_30Fps = 30, _640_x_480_30Fps = 35, _848_x_480_30Fps = 40, _1280_x_720_30Fps = 60, _1920_x_1080_30Fps = 70 }
// realsense pipeline
private Pipeline m_pipeline = null;
//public bool Streaming { get; protected set; }
private PipelineProfile activeProfile = null;
// current frame number
//private ulong currentFrameNumber = 0;
//private ulong currentFrameTimestamp = 0;
// stream profiles
private VideoStreamProfile depthStreamProfile = null;
private VideoStreamProfile colorStreamProfile = null;
// raw infrared data
protected byte[] rawInfraredImage1 = null;
protected byte[] rawInfraredImage2 = null; // used by BT
protected ushort[] rawInfraredImageBT = null;
// raw IMU data
private RealSensePoseData rsPoseData;
// rs frame holder
protected RealSenseFrames rsFrames = new RealSenseFrames();
// depth sensor settings
[System.Serializable]
public class RSSensorSettings : DepthSensorBase.BaseSensorSettings
{
public int colorCameraMode;
public int depthCameraMode;
}
[StructLayout(LayoutKind.Sequential)]
struct RealSensePoseData
{
public Vector3 translation;
public Vector3 velocity;
public Vector3 acceleration;
public Quaternion rotation;
public Vector3 angular_velocity;
public Vector3 angular_acceleration;
public int tracker_confidence;
public int mapper_confidence;
}
// rs frame set
public class RealSenseFrames
{
public VideoFrame colorFrame = null;
public VideoFrame depthFrame = null;
public VideoFrame infraredFrame = null;
public PoseFrame poseFrame = null;
public ulong deviceTimestamp = 0;
public void DisposeFrames()
{
if (colorFrame != null)
{
colorFrame.Dispose();
colorFrame = null;
}
if (depthFrame != null)
{
depthFrame.Dispose();
depthFrame = null;
}
if (infraredFrame != null)
{
infraredFrame.Dispose();
infraredFrame = null;
}
if (poseFrame != null)
{
poseFrame.Dispose();
poseFrame = null;
}
}
}
public override KinectInterop.DepthSensorPlatform GetSensorPlatform()
{
return KinectInterop.DepthSensorPlatform.RealSense;
}
public override System.Type GetSensorSettingsType()
{
return typeof(RSSensorSettings);
}
public override BaseSensorSettings GetSensorSettings(BaseSensorSettings settings)
{
if (settings == null)
{
settings = new RSSensorSettings();
}
RSSensorSettings extSettings = (RSSensorSettings)base.GetSensorSettings(settings);
extSettings.colorCameraMode = (int)colorCameraMode;
extSettings.depthCameraMode = (int)depthCameraMode;
return settings;
}
public override void SetSensorSettings(BaseSensorSettings settings)
{
if (settings == null)
return;
base.SetSensorSettings(settings);
RSSensorSettings extSettings = (RSSensorSettings)settings;
colorCameraMode = (ColorCameraMode)extSettings.colorCameraMode;
depthCameraMode = (DepthCameraMode)extSettings.depthCameraMode;
}
public override List<KinectInterop.SensorDeviceInfo> GetAvailableSensors()
{
List<KinectInterop.SensorDeviceInfo> alSensorInfo = new List<KinectInterop.SensorDeviceInfo>();
using (var ctx = new Context())
{
DeviceList devices = ctx.QueryDevices();
for(int i = 0; i < devices.Count; i++)
{
using (Intel.RealSense.Device device = devices[i])
{
KinectInterop.SensorDeviceInfo sensorInfo = new KinectInterop.SensorDeviceInfo();
sensorInfo.sensorId = device.Info[CameraInfo.SerialNumber];
sensorInfo.sensorName = device.Info[CameraInfo.Name];
if (sensorInfo.sensorName.StartsWith("Intel RealSense D"))
sensorInfo.sensorCaps = KinectInterop.FrameSource.TypeColor | KinectInterop.FrameSource.TypeDepth | KinectInterop.FrameSource.TypeInfrared;
else if (sensorInfo.sensorName.StartsWith("Intel RealSense T"))
sensorInfo.sensorCaps = KinectInterop.FrameSource.TypePose;
else
sensorInfo.sensorCaps = KinectInterop.FrameSource.TypeNone;
if (consoleLogMessages)
Debug.Log(string.Format(" D{0}: {1}, id: {2}", i, sensorInfo.sensorName, sensorInfo.sensorId));
alSensorInfo.Add(sensorInfo);
}
}
}
//if (alSensorInfo.Count == 0)
//{
// Debug.Log(" No sensor devices found.");
//}
return alSensorInfo;
}
public override KinectInterop.SensorData OpenSensor(KinectManager kinectManager, KinectInterop.FrameSource dwFlags, bool bSyncDepthAndColor, bool bSyncBodyAndDepth)
{
// save initial parameters
base.OpenSensor(kinectManager, dwFlags, bSyncDepthAndColor, bSyncBodyAndDepth);
// color settings
int colorWidth = 0, colorHeight = 0, colorFps = 0;
ParseCameraMode(colorCameraMode.ToString(), out colorWidth, out colorHeight, out colorFps);
// depth settings
int depthWidth = 0, depthHeight = 0, depthFps = 0;
ParseCameraMode(depthCameraMode.ToString(), out depthWidth, out depthHeight, out depthFps);
string sensorName = "Intel RealSense";
KinectInterop.FrameSource sensorCaps = KinectInterop.FrameSource.TypeColor | KinectInterop.FrameSource.TypeDepth | KinectInterop.FrameSource.TypeInfrared; ;
try
{
m_pipeline = new Pipeline();
using (Config config = new Config())
{
if(deviceStreamingMode == KinectInterop.DeviceStreamingMode.PlayRecording)
{
if (string.IsNullOrEmpty(recordingFile))
{
Debug.LogError("PlayRecording selected, but the path to recording file is missing.");
return null;
}
if(!System.IO.File.Exists(recordingFile))
{
Debug.LogError("PlayRecording selected, but the recording file cannot be found: " + recordingFile);
return null;
}
sensorPlatform = KinectInterop.DepthSensorPlatform.RealSense;
sensorDeviceId = KinectInterop.GetFileName(recordingFile, false);
// playback from file
if (consoleLogMessages)
Debug.Log(" Playing back: " + recordingFile);
config.EnableDeviceFromFile(recordingFile, false);
}
else
{
// get the list of available sensors
List<KinectInterop.SensorDeviceInfo> alSensors = GetAvailableSensors();
if (deviceIndex >= alSensors.Count)
{
Debug.LogError(" D" + deviceIndex + " is not available. You can set the device index to -1, to disable it.");
return null;
}
// sensor serial number
sensorPlatform = KinectInterop.DepthSensorPlatform.RealSense;
sensorDeviceId = alSensors[deviceIndex].sensorId;
sensorName = alSensors[deviceIndex].sensorName;
sensorCaps = alSensors[deviceIndex].sensorCaps;
config.EnableDevice(sensorDeviceId);
// color
if ((dwFlags & KinectInterop.FrameSource.TypeColor) != 0)
{
//Debug.Log(string.Format("Color camera mode: {0} x {1} @ {2} FPS", colorWidth, colorHeight, colorFps));
config.EnableStream(Stream.Color, -1, colorWidth, colorHeight, Format.Rgb8, colorFps);
}
// depth
if ((dwFlags & KinectInterop.FrameSource.TypeDepth) != 0)
{
//Debug.Log(string.Format("Depth camera mode: {0} x {1} @ {2} FPS", depthWidth, depthHeight, depthFps));
config.EnableStream(Stream.Depth, -1, depthWidth, depthHeight, Format.Z16, depthFps);
}
// infrared
if ((dwFlags & KinectInterop.FrameSource.TypeInfrared) != 0 /**|| (dwFlags & KinectInterop.FrameSource.TypeBody) != 0*/)
{
//Debug.Log(string.Format("Infrared camera mode: {0} x {1} @ {2} FPS", depthWidth, depthHeight, depthFps));
config.EnableStream(Stream.Infrared, 1, depthWidth, depthHeight, Format.Y8, depthFps);
//config.EnableStream(Stream.Infrared, 2, depthWidth, depthHeight, Format.Y8, depthFps);
}
// pose
if ((dwFlags & KinectInterop.FrameSource.TypePose) != 0)
{
config.EnableStream(Stream.Pose, Format.SixDOF);
}
// record to file
if (deviceStreamingMode == KinectInterop.DeviceStreamingMode.SaveRecording)
{
if (!string.IsNullOrEmpty(recordingFile))
{
if (consoleLogMessages)
Debug.Log(" Recording to file: " + recordingFile);
config.EnableRecordToFile(recordingFile);
}
else
{
Debug.LogError("Record selected, but the path to recording file is missing.");
}
}
}
activeProfile = m_pipeline.Start(config);
}
}
catch (Exception ex)
{
Debug.LogError("RealSenseInterface: " + ex.ToString());
}
// check if the profile was successfully created
if(activeProfile == null)
return null;
KinectInterop.SensorData sensorData = new KinectInterop.SensorData();
sensorData.sensorIntPlatform = sensorPlatform;
sensorData.sensorId = sensorDeviceId;
sensorData.sensorName = sensorName;
sensorData.sensorCaps = sensorCaps;
// flip color & depth images vertically
sensorData.colorImageScale = new Vector3(-1f, -1f, 1f);
sensorData.depthImageScale = new Vector3(-1f, -1f, 1f);
sensorData.infraredImageScale = new Vector3(-1f, -1f, 1f);
sensorData.sensorSpaceScale = new Vector3(-1f, -1f, 1f);
sensorData.unitToMeterFactor = 1f;
// depth camera offset & matrix z-flip
sensorRotOffset = Vector3.zero; // if for instance the depth camera is tilted downwards
sensorRotFlipZ = true;
sensorRotIgnoreY = false;
// color
sensorData.colorImageWidth = colorWidth;
sensorData.colorImageHeight = colorHeight;
sensorData.colorImageFormat = TextureFormat.RGB24;
sensorData.colorImageStride = 3; // 3 bytes per pixel
if ((dwFlags & KinectInterop.FrameSource.TypeColor) != 0)
{
rawColorImage = new byte[sensorData.colorImageWidth * sensorData.colorImageHeight * 3];
sensorData.colorImageTexture = new Texture2D(sensorData.colorImageWidth, sensorData.colorImageHeight, TextureFormat.RGB24, false);
sensorData.colorImageTexture.wrapMode = TextureWrapMode.Clamp;
sensorData.colorImageTexture.filterMode = FilterMode.Point;
}
// depth
sensorData.depthImageWidth = depthWidth;
sensorData.depthImageHeight = depthHeight;
if ((dwFlags & KinectInterop.FrameSource.TypeDepth) != 0)
{
rawDepthImage = new ushort[sensorData.depthImageWidth * sensorData.depthImageHeight];
sensorData.depthImage = new ushort[sensorData.depthImageWidth * sensorData.depthImageHeight];
}
// infrared
if ((dwFlags & KinectInterop.FrameSource.TypeInfrared) != 0 || (dwFlags & KinectInterop.FrameSource.TypeBody) != 0)
{
rawInfraredImage1 = new byte[sensorData.depthImageWidth * sensorData.depthImageHeight];
rawInfraredImage2 = new byte[sensorData.depthImageWidth * sensorData.depthImageHeight];
rawInfraredImageBT = new ushort[sensorData.depthImageWidth * sensorData.depthImageHeight];
rawInfraredImage = new ushort[sensorData.depthImageWidth * sensorData.depthImageHeight];
sensorData.infraredImage = new ushort[sensorData.depthImageWidth * sensorData.depthImageHeight];
minInfraredValue = 0f;
maxInfraredValue = 1000f;
}
// don't get all frames
getAllSensorFrames = false;
if (consoleLogMessages)
Debug.Log("D" + deviceIndex + " RealSense-sensor opened: " + sensorDeviceId);
return sensorData;
}
// parses the given camera mode and returns width, height and fps
private void ParseCameraMode(string sCameraMode, out int camWidth, out int camHeight, out int camFps)
{
camWidth = camHeight = camFps = 0;
if (string.IsNullOrEmpty(sCameraMode))
throw new Exception("Invalid camera mode" + sCameraMode);
// _640_x_360_30Fps
string[] asModeParts = sCameraMode.Split("_".ToCharArray());
if(asModeParts.Length != 5)
throw new Exception("Invalid camera mode" + sCameraMode);
camWidth = int.Parse(asModeParts[1]);
camHeight = int.Parse(asModeParts[3]);
int iF = asModeParts[4].IndexOf('F');
if (iF >= 0)
asModeParts[4] = asModeParts[4].Substring(0, iF);
camFps = int.Parse(asModeParts[4]);
}
public override void CloseSensor(KinectInterop.SensorData sensorData)
{
base.CloseSensor(sensorData);
if (activeProfile != null)
{
activeProfile.Dispose();
activeProfile = null;
}
if (m_pipeline != null)
{
m_pipeline.Dispose();
m_pipeline = null;
}
if (consoleLogMessages)
Debug.Log("D" + deviceIndex + " RealSense-sensor closed: " + sensorDeviceId);
}
public override void EnablePoseStream(KinectInterop.SensorData sensorData, bool bEnable)
{
base.EnablePoseStream(sensorData, bEnable);
if ((frameSourceFlags & KinectInterop.FrameSource.TypePose) != 0)
{
// todo
}
else
{
// todo
}
}
public override void PollSensorFrames(KinectInterop.SensorData sensorData)
{
FrameSet frames;
if (m_pipeline.PollForFrames(out frames))
{
//using (frames)
// RaiseSampleEvent(frames);
using (frames)
{
try
{
rsFrames.colorFrame = frames.ColorFrame;
rsFrames.depthFrame = frames.DepthFrame;
rsFrames.infraredFrame = frames.InfraredFrame;
rsFrames.poseFrame = frames.PoseFrame;
rsFrames.deviceTimestamp = (ulong)(frames.Timestamp * 1000.0);
//currentFrameNumber = frames.Number;
//currentFrameTimestamp = (ulong)(frames.Timestamp * 1000.0);
// check for color-depth sync
if(!isSyncDepthAndColor || (rsFrames.colorFrame != null && rsFrames.depthFrame != null))
{
ProcessCameraFrames(sensorData, rsFrames);
}
rsFrames.DisposeFrames();
//pipelineFrameNumber++;
}
catch (Exception e)
{
Debug.LogException(e);
}
}
}
}
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);
}
}
}
//// 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;
//}
public override bool UpdateSensorData(KinectInterop.SensorData sensorData, KinectManager kinectManager, bool isPlayMode)
{
base.UpdateSensorData(sensorData, kinectManager, isPlayMode);
if(sensorData.colorCamIntr == null && colorStreamProfile != null)
{
lock (colorFrameLock)
{
Intel.RealSense.Intrinsics colorCamIntr = colorStreamProfile.GetIntrinsics();
if (colorCamIntr.model != Distortion.None)
{
GetCameraIntrinsics(colorCamIntr, ref sensorData.colorCamIntr, 1);
}
}
}
if (sensorData.depthCamIntr == null && depthStreamProfile != null)
{
lock (depthFrameLock)
{
Intel.RealSense.Intrinsics depthCamIntr = depthStreamProfile.GetIntrinsics();
//Debug.Log("RS distType: " + depthCamIntr.model);
if (depthCamIntr.model != Distortion.None || deviceStreamingMode == KinectInterop.DeviceStreamingMode.PlayRecording)
{
GetCameraIntrinsics(depthCamIntr, ref sensorData.depthCamIntr, 0);
if (depthCamIntr.model == Distortion.None && deviceStreamingMode == KinectInterop.DeviceStreamingMode.PlayRecording)
{
// workaround for playback mode (model & coeffs are missing there)
sensorData.depthCamIntr.distType = KinectInterop.DistortionType.BrownConrady;
}
// body & body-index data
if ((frameSourceFlags & KinectInterop.FrameSource.TypeBody) != 0)
{
Debug.LogWarning("Body tracking is not supported for RealSense sensors!");
//if(depthCamIntr.width == 640 && depthCamIntr.height == 480)
//{
// Calibration cal = GetBodyTrackerCalibration(sensorData.depthCamIntr);
// InitBodyTracking(frameSourceFlags, sensorData, cal, true);
// if (isSyncBodyAndDepth && bodyTracker == null)
// {
// // don't sync body and depth if the body tracker can't be created
// isSyncBodyAndDepth = false;
// }
//}
}
}
}
}
if(sensorData.depth2ColorExtr == null && depthStreamProfile != null && colorStreamProfile != null)
{
lock (depthFrameLock)
{
lock (colorFrameLock)
{
Intel.RealSense.Extrinsics depth2ColorExtr = depthStreamProfile.GetExtrinsicsTo(colorStreamProfile);
GetCameraExtrinsics(depth2ColorExtr, ref sensorData.depth2ColorExtr);
}
}
}
if (sensorData.color2DepthExtr == null && colorStreamProfile != null && depthStreamProfile != null)
{
lock (colorFrameLock)
{
lock (depthFrameLock)
{
Intel.RealSense.Extrinsics color2DepthExtr = colorStreamProfile.GetExtrinsicsTo(depthStreamProfile);
GetCameraExtrinsics(color2DepthExtr, ref sensorData.color2DepthExtr);
}
}
}
return true;
}
// gets the given camera intrinsics
private void GetCameraIntrinsics(Intel.RealSense.Intrinsics camIntr, ref KinectInterop.CameraIntrinsics intr, int camType)
{
intr = new KinectInterop.CameraIntrinsics();
intr.cameraType = camType;
intr.width = camIntr.width;
intr.height = camIntr.height;
intr.ppx = camIntr.ppx;
intr.ppy = camIntr.ppy;
intr.fx = camIntr.fx;
intr.fy = camIntr.fy;
intr.distCoeffs = new float[camIntr.coeffs.Length];
camIntr.coeffs.CopyTo(intr.distCoeffs, 0);
intr.distType = (KinectInterop.DistortionType)camIntr.model;
EstimateFOV(intr);
}
// gets the given camera extrinsics
private void GetCameraExtrinsics(Intel.RealSense.Extrinsics camExtr, ref KinectInterop.CameraExtrinsics extr)
{
extr = new KinectInterop.CameraExtrinsics();
extr.rotation = new float[camExtr.rotation.Length];
camExtr.rotation.CopyTo(extr.rotation, 0);
extr.translation = new float[camExtr.translation.Length];
camExtr.translation.CopyTo(extr.translation, 0);
}
// unprojects plane point into the space
public override Vector3 UnprojectPoint(KinectInterop.CameraIntrinsics intr, Vector2 pixel, float depth)
{
float x = (pixel.x - intr.ppx) / intr.fx;
float y = (pixel.y - intr.ppy) / intr.fy;
if (intr.distType == KinectInterop.DistortionType.InverseBrownConrady)
{
float r2 = x * x + y * y;
float f = 1 + intr.distCoeffs[0] * r2 + intr.distCoeffs[1] * r2 * r2 + intr.distCoeffs[4] * r2 * r2 * r2;
float ux = x * f + 2 * intr.distCoeffs[2] * x * y + intr.distCoeffs[3] * (r2 + 2 * x * x);
float uy = y * f + 2 * intr.distCoeffs[3] * x * y + intr.distCoeffs[2] * (r2 + 2 * y * y);
x = ux;
y = uy;
}
Vector3 point = new Vector3(depth * x, depth * y, depth);
return point;
}
// projects space point onto a plane
public override Vector2 ProjectPoint(KinectInterop.CameraIntrinsics intr, Vector3 point)
{
float x = point.x / point.z;
float y = point.y / point.z;
if (intr.distType == KinectInterop.DistortionType.ModifiedBrownConrady)
{
float r2 = x * x + y * y;
float f = 1f + intr.distCoeffs[0] * r2 + intr.distCoeffs[1] * r2 * r2 + intr.distCoeffs[4] * r2 * r2 * r2;
x *= f;
y *= f;
float dx = x + 2f * intr.distCoeffs[2] * x * y + intr.distCoeffs[3] * (r2 + 2 * x * x);
float dy = y + 2f * intr.distCoeffs[3] * x * y + intr.distCoeffs[2] * (r2 + 2 * y * y);
x = dx;
y = dy;
}
if (intr.distType == KinectInterop.DistortionType.Theta)
{
float r = (float)Math.Sqrt(x * x + y * y);
float rd = (1f / intr.distCoeffs[0] * (float)Math.Atan(2f * r * (float)Math.Tan(intr.distCoeffs[0] / 2f)));
x *= rd / r;
y *= rd / r;
}
Vector2 pixel = new Vector2(x * intr.fx + intr.ppx, y * intr.fy + intr.ppy);
return pixel;
}
// transforms a point from one space to another
public override Vector3 TransformPoint(KinectInterop.CameraExtrinsics extr, Vector3 point)
{
float toPointX = extr.rotation[0] * point.x + extr.rotation[3] * point.y + extr.rotation[6] * point.z + extr.translation[0];
float toPointY = extr.rotation[1] * point.x + extr.rotation[4] * point.y + extr.rotation[7] * point.z + extr.translation[1];
float toPointZ = extr.rotation[2] * point.x + extr.rotation[5] * point.y + extr.rotation[8] * point.z + extr.translation[2];
return new Vector3(toPointX, toPointY, toPointZ);
}
public override void PollCoordTransformFrames(KinectInterop.SensorData sensorData)
{
if (lastDepthCoordFrameTime != rawDepthTimestamp)
{
lastDepthCoordFrameTime = rawDepthTimestamp;
// depth2color frame
if (depthCamColorDataFrame != null)
{
lock (depthCamColorFrameLock)
{
TransformColorFrameToDepthCamResolution(sensorData, depthCamColorDataFrame);
lastDepthCamColorFrameTime = lastDepthCoordFrameTime;
//Debug.Log("DepthCamColorFrameTime: " + lastDepthCamColorFrameTime);
}
}
// color2depth frame
if (colorCamDepthDataFrame != null)
{
lock (colorCamDepthFrameLock)
{
TransformDepthFrameToColorCamResolution(sensorData, colorCamDepthDataFrame);
lastColorCamDepthFrameTime = lastDepthCoordFrameTime;
//Debug.Log("ColorCamDepthFrameTime: " + lastColorCamDepthFrameTime);
}
}
// color2infrared frame
if (colorCamInfraredDataFrame != null)
{
lock (colorCamInfraredFrameLock)
{
TransformInfraredFrameToColorCamResolution(sensorData, colorCamInfraredDataFrame);
lastColorCamInfraredFrameTime = lastDepthCoordFrameTime;
//Debug.Log("ColorCamInfraredFrameTime: " + lastColorCamInfraredFrameTime);
}
}
}
}
// transforms the color frame to depth-camera resolution
protected bool TransformColorFrameToDepthCamResolution(KinectInterop.SensorData sensorData, byte[] c2dColorFrame)
{
if (rawDepthImage != null && rawColorImage != null && sensorData.depthCamIntr != null && sensorData.colorCamIntr != null && sensorData.depth2ColorExtr != null)
{
int depthImageW = sensorData.depthImageWidth;
int depthImageH = sensorData.depthImageHeight;
int colorImageW = sensorData.colorImageWidth;
int colorImageS = sensorData.colorImageStride;
int mapImageLen = depthImageW * depthImageH * colorImageS;
if (c2dColorFrame == null || c2dColorFrame.Length != mapImageLen)
{
throw new Exception("d2cColorFrame is not big enough. Should be " + mapImageLen + " bytes.");
}
for (int dy = 0, dIndex = 0, d2cIndex = 0; dy < depthImageH; dy++)
{
for (int dx = 0; dx < depthImageW; dx++, dIndex++, d2cIndex += colorImageS)
{
ushort depthVal = rawDepthImage[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);
Vector2 colorFramePos = ProjectPoint(sensorData.colorCamIntr, colorSpacePos);
int cIndex = ((int)colorFramePos.x + (int)colorFramePos.y * colorImageW) * colorImageS;
if (cIndex >= 0 && (cIndex + colorImageS - 1) < rawColorImage.Length)
{
c2dColorFrame[d2cIndex] = rawColorImage[cIndex];
c2dColorFrame[d2cIndex + 1] = rawColorImage[cIndex + 1];
c2dColorFrame[d2cIndex + 2] = rawColorImage[cIndex + 2];
//c2dColorFrame[d2cIndex + 3] = 255;
}
}
else
{
c2dColorFrame[d2cIndex] = 0;
c2dColorFrame[d2cIndex + 1] = 0;
c2dColorFrame[d2cIndex + 2] = 0;
//c2dColorFrame[d2cIndex + 3] = 0;
}
}
}
return true;
}
return false;
}
// transforms the depth frame to color-camera resolution
protected bool TransformDepthFrameToColorCamResolution(KinectInterop.SensorData sensorData, ushort[] d2cDepthFrame)
{
if (rawDepthImage != null && rawColorImage != null && sensorData.depthCamIntr != null && sensorData.colorCamIntr != null && sensorData.depth2ColorExtr != null)
{
int depthImageW = sensorData.depthImageWidth;
int depthImageH = sensorData.depthImageHeight;
int colorImageW = sensorData.colorImageWidth;
int colorImageH = sensorData.colorImageHeight;
int mapImageLen = sensorData.colorImageWidth * sensorData.colorImageHeight;
if (d2cDepthFrame == null || d2cDepthFrame.Length != mapImageLen)
{
throw new Exception("d2cDepthFrame is not big enough. Should be " + mapImageLen + " ushorts.");
}
//Intrinsics depthIntr = depthStreamProfile.GetIntrinsics();
//Intrinsics colorIntr = colorStreamProfile.GetIntrinsics();
//Extrinsics d2cExtr = depthStreamProfile.GetExtrinsicsTo(colorStreamProfile);
for (int dy = 0, dIndex = 0; dy < depthImageH; dy++)
{
for (int dx = 0; dx < depthImageW; dx++, dIndex++)
{
ushort depthVal = rawDepthImage[dIndex];
if (depthVal != 0)
{
float depth = (float)depthVal * 0.001f;
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 = Mathf.RoundToInt(colorPos2.x);
int colorPos2Y = Mathf.RoundToInt(colorPos2.y);
if (colorPos1X < 0 || colorPos1Y < 0 || colorPos2X >= colorImageW || colorPos2Y >= colorImageH)
continue;
for (int y = colorPos1Y; y <= colorPos2Y; y++)
{
int cIndex = y * colorImageW + colorPos1X;
for (int x = colorPos1X; x <= colorPos2X; x++, cIndex++)
{
d2cDepthFrame[cIndex] = depthVal;
}
}
}
else
{
//d2cDepthFrame[cIndex] = 0;
}
}
}
return true;
}
return false;
}
// transforms the infrared frame to color-camera resolution
protected bool TransformInfraredFrameToColorCamResolution(KinectInterop.SensorData sensorData, ushort[] d2cInfraredFrame)
{
if (rawDepthImage != null && rawColorImage != null && rawInfraredImage != null &&
sensorData.depthCamIntr != null && sensorData.colorCamIntr != null && sensorData.depth2ColorExtr != null)
{
int depthImageW = sensorData.depthImageWidth;
int depthImageH = sensorData.depthImageHeight;
int colorImageW = sensorData.colorImageWidth;
int colorImageH = sensorData.colorImageHeight;
int mapImageLen = sensorData.colorImageWidth * sensorData.colorImageHeight;
if (d2cInfraredFrame == null || d2cInfraredFrame.Length != mapImageLen)
{
throw new Exception("d2cInfraredFrame is not big enough. Should be " + mapImageLen + " ushorts.");
}
for (int dy = 0, dIndex = 0; dy < depthImageH; dy++)
{
for (int dx = 0; dx < depthImageW; dx++, dIndex++)
{
ushort depthVal = rawDepthImage[dIndex];
if (depthVal != 0)
{
float depth = (float)depthVal * 0.001f;
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 = Mathf.RoundToInt(colorPos2.x);
int colorPos2Y = Mathf.RoundToInt(colorPos2.y);
if (colorPos1X < 0 || colorPos1Y < 0 || colorPos2X >= colorImageW || colorPos2Y >= colorImageH)
continue;
ushort irValue = rawInfraredImage[dIndex];
//ushort irValue = sensorData.infraredImage[dIndex];
for (int y = colorPos1Y; y <= colorPos2Y; y++)
{
int cIndex = y * colorImageW + colorPos1X;
for (int x = colorPos1X; x <= colorPos2X; x++, cIndex++)
{
d2cInfraredFrame[cIndex] = irValue;
}
}
}
else
{
//d2cInfraredFrame[cIndex] = 0;
}
}
}
return true;
}
return false;
}
}
}