public override KinectInterop.SensorData OpenSensor(KinectInterop.FrameSource dwFlags, bool bSyncDepthAndColor, bool bSyncBodyAndDepth)
{
// save initial parameters
base.OpenSensor(dwFlags, bSyncDepthAndColor, bSyncBodyAndDepth);
// ensure resources are in path
//KinectInterop.CopyResourceFile("depthengine_1_0.x64.dll", "depthengine_1_0.dll");
KinectInterop.CopyResourceFile("onnxruntime.dll", "onnxruntime.dll");
KinectInterop.CopyResourceFile("dnn_model.onnx", "dnn_model.onnx");
// try to open the sensor or play back the recording
KinectInterop.SensorData sensorData = new KinectInterop.SensorData();
if (deviceStreamingMode == KinectInterop.DeviceStreamingMode.PlayRecording)
{
if (string.IsNullOrEmpty(recordingFile))
{
Debug.LogError("Playback 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);
}
Debug.Log("Playing back: " + recordingFile);
kinectPlayback = new Playback(recordingFile);
colorCameraMode = (ColorCameraMode)kinectPlayback.playback_config.color_resolution;
depthCameraMode = (DepthCameraMode)kinectPlayback.playback_config.depth_mode;
deviceSyncMode = kinectPlayback.playback_config.wired_sync_mode;
coordMapper = kinectPlayback.playback_calibration;
playbackStartTime = DateTime.Now.Ticks;
Debug.Log(string.Format("color_track_enabled: {0}, depth_track_enabled: {1}, ir_track_enabled: {2}, imu_track_enabled: {3}, depth_delay_off_color_usec: {4}",
kinectPlayback.playback_config.color_track_enabled, kinectPlayback.playback_config.depth_track_enabled,
kinectPlayback.playback_config.ir_track_enabled, kinectPlayback.playback_config.imu_track_enabled,
kinectPlayback.playback_config.depth_delay_off_color_usec));
}
else
{
List <KinectInterop.SensorDeviceInfo> alSensors = GetAvailableSensors();
if (deviceIndex >= alSensors.Count)
{
Debug.Log(" D" + deviceIndex + " is not available. You can set the device index to -1, to disable it.");
return(null);
}
// try to open the sensor
kinectSensor = Device.Open(deviceIndex);
if (kinectSensor == null)
{
Debug.LogError(" D" + recordingFile + " cannot be opened.");
return(null);
}
DeviceConfiguration kinectConfig = new DeviceConfiguration();
kinectConfig.SynchronizedImagesOnly = isSyncDepthAndColor;
kinectConfig.WiredSyncMode = deviceSyncMode;
kinectConfig.SuboridinateDelayOffMaster = new TimeSpan(subDeviceDelayUsec * 10);
// color
kinectConfig.ColorFormat = ImageFormat.ColorBGRA32;
if ((dwFlags & KinectInterop.FrameSource.TypeColor) != 0)
{
kinectConfig.ColorResolution = (ColorResolution)colorCameraMode;
}
else
{
kinectConfig.ColorResolution = ColorResolution.Off;
}
// depth
if ((dwFlags & KinectInterop.FrameSource.TypeDepth) != 0)
{
kinectConfig.DepthMode = (DepthMode)depthCameraMode;
}
else
{
kinectConfig.DepthMode = DepthMode.Off;
}
// infrared
if ((dwFlags & KinectInterop.FrameSource.TypeInfrared) != 0)
{
// ??
}
// start the cameras
kinectSensor.StartCameras(kinectConfig);
isCamerasStarted = true;
if ((dwFlags & KinectInterop.FrameSource.TypePose) != 0)
{
// start the IMU
kinectSensor.StartImu();
isImuStarted = true;
}
// get reference to the coordinate mapper
coordMapper = kinectSensor.GetCalibration();
}
// reset the frame number
currentFrameNumber = 0;
// flip color & depth image vertically
sensorData.colorImageScale = new Vector3(-1f, -1f, 1f);
sensorData.depthImageScale = new Vector3(-1f, -1f, 1f);
sensorData.infraredImageScale = new Vector3(-1f, -1f, 1f);
sensorData.sensorSpaceScale = new Vector3(-1f, -1f, 1f);
// color camera data & intrinsics
sensorData.colorImageFormat = TextureFormat.BGRA32;
sensorData.colorImageStride = 4; // 4 bytes per pixel
if ((dwFlags & KinectInterop.FrameSource.TypeColor) != 0)
{
CameraCalibration colorCamera = coordMapper.ColorCameraCalibration;
sensorData.colorImageWidth = colorCamera.ResolutionWidth;
sensorData.colorImageHeight = colorCamera.ResolutionHeight;
rawColorImage = new byte[sensorData.colorImageWidth * sensorData.colorImageHeight * 4];
sensorData.colorImageTexture = new Texture2D(sensorData.colorImageWidth, sensorData.colorImageHeight, TextureFormat.BGRA32, false);
sensorData.colorImageTexture.wrapMode = TextureWrapMode.Clamp;
sensorData.colorImageTexture.filterMode = FilterMode.Point;
}
// depth camera data & intrinsics
if ((dwFlags & KinectInterop.FrameSource.TypeDepth) != 0)
{
CameraCalibration depthCamera = coordMapper.DepthCameraCalibration;
sensorData.depthImageWidth = depthCamera.ResolutionWidth;
sensorData.depthImageHeight = depthCamera.ResolutionHeight;
rawDepthImage = new ushort[sensorData.depthImageWidth * sensorData.depthImageHeight];
sensorData.depthImage = new ushort[sensorData.depthImageWidth * sensorData.depthImageHeight];
}
// infrared data
if ((dwFlags & KinectInterop.FrameSource.TypeInfrared) != 0)
{
if (sensorData.depthImageWidth == 0 || sensorData.depthImageHeight == 0)
{
CameraCalibration depthCamera = coordMapper.DepthCameraCalibration;
sensorData.depthImageWidth = depthCamera.ResolutionWidth;
sensorData.depthImageHeight = depthCamera.ResolutionHeight;
}
rawInfraredImage = new ushort[sensorData.depthImageWidth * sensorData.depthImageHeight];
sensorData.infraredImage = new ushort[sensorData.depthImageWidth * sensorData.depthImageHeight];
}
// calibration data
GetCameraIntrinsics(CalibrationDeviceType.Color, coordMapper.ColorCameraCalibration, ref sensorData.colorCamIntr);
GetCameraIntrinsics(CalibrationDeviceType.Depth, coordMapper.DepthCameraCalibration, ref sensorData.depthCamIntr);
GetCameraExtrinsics(coordMapper.ColorCameraCalibration.Extrinsics, ref sensorData.depth2ColorExtr);
Debug.Log("Kinect4Azure-sensor opened.");
return(sensorData);
}
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(deviceMode == KinectInterop.DepthSensorMode.CreateRecording && !string.IsNullOrEmpty(deviceFilePath))
//{
// if (!string.IsNullOrEmpty(deviceFilePath))
// {
// config.EnableRecordToFile(deviceFilePath);
// }
// 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);
}
// Apply the orientation constraints
public void Constrain(ref KinectInterop.BodyData bodyData)
{
KinectManager kinectManager = KinectManager.Instance;
frameNum++;
for (int i = 0; i < jointConstraints.Count; i++)
{
BoneOrientationConstraint jc = this.jointConstraints[i];
if (jc.thisJoint == (int)KinectInterop.JointType.Pelvis || bodyData.joint[jc.thisJoint].normalRotation == Quaternion.identity)
{
continue;
}
if (bodyData.joint[jc.thisJoint].trackingState == KinectInterop.TrackingState.NotTracked)
{
continue;
}
int prevJoint = (int)KinectInterop.GetParentJoint((KinectInterop.JointType)jc.thisJoint);
if (bodyData.joint[prevJoint].trackingState == KinectInterop.TrackingState.NotTracked)
{
continue;
}
Quaternion rotParentN = bodyData.joint[prevJoint].normalRotation;
//Quaternion rotDefaultN = Quaternion.identity; // Quaternion.FromToRotation(KinectInterop.JointBaseDir[prevJoint], KinectInterop.JointBaseDir[jc.thisJoint]);
//rotParentN = rotParentN * rotDefaultN;
Quaternion rotJointN = bodyData.joint[jc.thisJoint].normalRotation;
Quaternion rotLocalN = Quaternion.Inverse(rotParentN) * rotJointN;
Vector3 eulerAnglesN = rotLocalN.eulerAngles;
bool isConstrained = false;
//string sDebug = string.Empty;
for (int a = 0; a < jc.axisConstrainrs.Count; a++)
{
AxisOrientationConstraint ac = jc.axisConstrainrs[a];
Quaternion rotLimited = rotLocalN;
switch (ac.consType)
{
case 0:
break;
case CT.LimA:
eulerAnglesN = LimitAngles(eulerAnglesN, ac.axis, ac.angleMin, ac.angleMax);
rotLimited = Quaternion.Euler(eulerAnglesN);
break;
case CT.LimST:
rotLimited = LimitSwing(rotLocalN, ac.axis, ac.angleMin);
rotLimited = LimitTwist(rotLimited, ac.axis, ac.angleMax);
break;
case CT.LimH:
float lastAngle = bodyData.joint[jc.thisJoint].lastAngle;
rotLimited = LimitHinge(rotLocalN, ac.axis, ac.angleMin, ac.angleMax, ref lastAngle);
bodyData.joint[jc.thisJoint].lastAngle = lastAngle;
break;
default:
throw new Exception("Undefined constraint type found: " + (int)ac.consType);
}
if (rotLimited != rotLocalN)
{
rotLocalN = rotLimited;
isConstrained = true;
}
}
//if (sDebug.Length > 0)
//{
// if (debugText != null && jc.thisJoint == (int)KinectInterop.JointType.ElbowLeft)
// {
// // debugText.text = sDebug;
// }
// Debug.Log(sDebug);
//}
if (isConstrained)
{
rotJointN = rotParentN * rotLocalN;
Vector3 eulerJoint = rotJointN.eulerAngles;
Vector3 eulerJointM = new Vector3(eulerJoint.x, -eulerJoint.y, -eulerJoint.z);
Quaternion rotJointM = Quaternion.Euler(eulerJointM);
// put it back into the bone orientations
bodyData.joint[jc.thisJoint].normalRotation = rotJointN;
bodyData.joint[jc.thisJoint].mirroredRotation = rotJointM;
}
}
}
// initializes background removal with shaders
private bool InitBackgroundRemoval(KinectInterop.SensorData sensorData)
{
if (sensorData != null && sensorData.sensorInterface != null && KinectInterop.IsDirectX11Available())
{
if (filterByBody != null)
{
if (!filterByBody.InitBackgroundRemoval(sensorData, MAX_BODY_COUNT))
{
Debug.LogError("Could not init the background removal by body bounds!");
return(false);
}
}
else if (filterByBI != null)
{
if (!filterByBI.InitBackgroundRemoval(sensorData))
{
Debug.LogError("Could not init the background removal by body index!");
return(false);
}
}
sensorInt = (DepthSensorBase)sensorData.sensorInterface;
// set the texture resolution
if (sensorInt.pointCloudColorTexture == null && sensorInt.pointCloudVertexTexture == null)
{
sensorInt.pointCloudResolution = foregroundImageResolution;
}
textureRes = sensorInt.GetPointCloudTexResolution(sensorData);
colorTexture = KinectInterop.CreateRenderTexture(colorTexture, textureRes.x, textureRes.y, RenderTextureFormat.ARGB32);
if (filterByBI == null)
{
vertexTexture = KinectInterop.CreateRenderTexture(vertexTexture, textureRes.x, textureRes.y, RenderTextureFormat.ARGBHalf);
}
alphaTexture = KinectInterop.CreateRenderTexture(alphaTexture, textureRes.x, textureRes.y, RenderTextureFormat.ARGB32);
foregroundTexture = KinectInterop.CreateRenderTexture(foregroundTexture, textureRes.x, textureRes.y, RenderTextureFormat.ARGB32);
sensorInt.pointCloudColorTexture = colorTexture;
sensorInt.pointCloudVertexTexture = vertexTexture;
Shader erodeShader = Shader.Find("Kinect/ErodeShader");
erodeFilterMat = new Material(erodeShader);
erodeFilterMat.SetFloat("_TexResX", (float)textureRes.x);
erodeFilterMat.SetFloat("_TexResY", (float)textureRes.y);
//sensorData.erodeBodyMaterial.SetTexture("_MainTex", sensorData.bodyIndexTexture);
Shader dilateShader = Shader.Find("Kinect/DilateShader");
dilateFilterMat = new Material(dilateShader);
dilateFilterMat.SetFloat("_TexResX", (float)textureRes.x);
dilateFilterMat.SetFloat("_TexResY", (float)textureRes.y);
//sensorData.dilateBodyMaterial.SetTexture("_MainTex", sensorData.bodyIndexTexture);
Shader gradientShader = Shader.Find("Kinect/GradientShader");
gradientFilterMat = new Material(gradientShader);
Shader medianShader = Shader.Find("Kinect/MedianShader");
medianFilterMat = new Material(medianShader);
//sensorData.medianBodyMaterial.SetFloat("_Amount", 1.0f);
Shader blurShader = Shader.Find("Kinect/BlurShader");
blurFilterMat = new Material(blurShader);
Shader invertShader = Shader.Find("Kinect/InvertShader");
invertAlphaMat = new Material(invertShader);
Shader foregroundShader = Shader.Find("Kinect/ForegroundShader");
foregroundMat = new Material(foregroundShader);
return(true);
}
return(false);
}
// 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(capture != null)
// Debug.Log("BtDepthTimestamp: " + capture.Depth.DeviceTimestamp.Ticks);
}
}
// check for body & depth sync
if (!isSyncBodyAndDepth || btCapture != null /**&& (btQueueCount == 0 && sensorData.lastBodyFrameTime == rawDepthTimestamp)*/) // currentBodyTimestamp
{
// 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 = (ulong)capture.Color.DeviceTimestamp.Ticks;
//Debug.Log("D" + deviceIndex + " RawColorTimestamp: " + rawColorTimestamp);
}
}
// 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);
}
}
// 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 = (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 &&
(rawColorTimestamp != sensorData.lastColorFrameTime || rawDepthTimestamp != sensorData.lastDepthFrameTime))
{
if (coordMapperTransform == null)
{
coordMapperTransform = coordMapperCalib.CreateTransformation();
}
if (depthCamColorDataFrame != null)
{
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 = (ulong)capture.Depth.DeviceTimestamp.Ticks;
}
}
if (colorCamDepthDataFrame != null)
{
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 = (ulong)capture.Color.DeviceTimestamp.Ticks;
}
}
}
}
else
{
// ignore the capture
capture = null;
}
if (btCapture != null)
{
// dispose body capture
btCapture.Dispose();
}
}
catch (System.Exception ex)
{
Debug.LogException(ex);
}
}
public override KinectInterop.SensorData OpenSensor(KinectInterop.FrameSource dwFlags, bool bSyncDepthAndColor, bool bSyncBodyAndDepth)
{
// save initial parameters
base.OpenSensor(dwFlags, bSyncDepthAndColor, bSyncBodyAndDepth);
// try to open the sensor or play back the recording
KinectInterop.SensorData sensorData = new KinectInterop.SensorData();
if (deviceStreamingMode == KinectInterop.DeviceStreamingMode.PlayRecording)
{
if (string.IsNullOrEmpty(recordingFile))
{
Debug.LogError("Playback 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);
}
Debug.Log("Playing back: " + recordingFile);
kinectPlayback = new Playback(recordingFile);
sensorPlatform = KinectInterop.DepthSensorPlatform.Kinect4Azure;
sensorDeviceId = KinectInterop.GetFileName(recordingFile, false); // deviceIndex.ToString();
//Debug.Log("D" + deviceIndex + " ID: " + sensorDeviceId);
colorCameraMode = (ColorCameraMode)kinectPlayback.playback_config.color_resolution;
depthCameraMode = (DepthCameraMode)kinectPlayback.playback_config.depth_mode;
deviceSyncMode = kinectPlayback.playback_config.wired_sync_mode;
coordMapperCalib = kinectPlayback.playback_calibration;
playbackStartTime = 0;
Debug.Log(string.Format("color_track_enabled: {0}, depth_track_enabled: {1}, ir_track_enabled: {2}, imu_track_enabled: {3}, depth_delay_off_color_usec: {4}",
kinectPlayback.playback_config.color_track_enabled, kinectPlayback.playback_config.depth_track_enabled,
kinectPlayback.playback_config.ir_track_enabled, kinectPlayback.playback_config.imu_track_enabled,
kinectPlayback.playback_config.depth_delay_off_color_usec));
}
else
{
List <KinectInterop.SensorDeviceInfo> alSensors = GetAvailableSensors();
if (deviceIndex >= alSensors.Count)
{
Debug.Log(" D" + deviceIndex + " is not available. You can set the device index to -1, to disable it.");
return(null);
}
kinectSensor = Device.Open(deviceIndex);
if (kinectSensor == null)
{
Debug.LogError(" D" + recordingFile + " cannot be opened.");
return(null);
}
// try to open the sensor
sensorPlatform = KinectInterop.DepthSensorPlatform.Kinect4Azure;
sensorDeviceId = kinectSensor.SerialNum;
//Debug.Log("D" + deviceIndex + " ID: " + sensorDeviceId);
if (deviceSyncMode == WiredSyncMode.Master && (dwFlags & KinectInterop.FrameSource.TypeColor) == 0)
{
// fix by Andreas Pedroni - master requires color camera on
dwFlags |= KinectInterop.FrameSource.TypeColor;
}
DeviceConfiguration kinectConfig = new DeviceConfiguration();
kinectConfig.SynchronizedImagesOnly = isSyncDepthAndColor;
kinectConfig.WiredSyncMode = deviceSyncMode;
kinectConfig.SuboridinateDelayOffMaster = new TimeSpan(subDeviceDelayUsec * 10);
// color
kinectConfig.ColorFormat = ImageFormat.ColorBGRA32;
if ((dwFlags & KinectInterop.FrameSource.TypeColor) != 0)
{
kinectConfig.ColorResolution = (ColorResolution)colorCameraMode;
}
else
{
kinectConfig.ColorResolution = ColorResolution.Off;
}
// depth
if ((dwFlags & KinectInterop.FrameSource.TypeDepth) != 0)
{
kinectConfig.DepthMode = (DepthMode)depthCameraMode;
}
else
{
kinectConfig.DepthMode = DepthMode.Off;
}
// fps
if (colorCameraMode != ColorCameraMode._4096_x_3072_15Fps && depthCameraMode != DepthCameraMode._1024x1024_15Fps_2_21mWfov)
{
kinectConfig.CameraFPS = FPS.FPS30;
}
else
{
kinectConfig.CameraFPS = FPS.FPS15;
}
// infrared
if ((dwFlags & KinectInterop.FrameSource.TypeInfrared) != 0)
{
// ??
}
// start the cameras
kinectSensor.StartCameras(kinectConfig);
isCamerasStarted = true;
if ((dwFlags & KinectInterop.FrameSource.TypePose) != 0)
{
// start the IMU
kinectSensor.StartImu();
isImuStarted = true;
}
// get reference to the coordinate mapper
coordMapperCalib = kinectSensor.GetCalibration();
}
// reset the frame number
//currentFrameNumber = 0;
// flip color & depth image vertically
sensorData.colorImageScale = new Vector3(-1f, -1f, 1f);
sensorData.depthImageScale = new Vector3(-1f, -1f, 1f);
sensorData.infraredImageScale = new Vector3(-1f, -1f, 1f);
sensorData.sensorSpaceScale = new Vector3(-1f, -1f, 1f);
// depth camera offset & matrix z-flip
sensorRotOffset = new Vector3(6f, 0f, 0f); // the depth camera is tilted 6 degrees downwards
sensorRotFlipZ = true;
sensorRotIgnoreY = true;
// color camera data & intrinsics
sensorData.colorImageFormat = TextureFormat.BGRA32;
sensorData.colorImageStride = 4; // 4 bytes per pixel
if ((dwFlags & KinectInterop.FrameSource.TypeColor) != 0)
{
CameraCalibration colorCamera = coordMapperCalib.ColorCameraCalibration;
sensorData.colorImageWidth = colorCamera.ResolutionWidth;
sensorData.colorImageHeight = colorCamera.ResolutionHeight;
rawColorImage = new byte[sensorData.colorImageWidth * sensorData.colorImageHeight * 4];
sensorData.colorImageTexture = new Texture2D(sensorData.colorImageWidth, sensorData.colorImageHeight, TextureFormat.BGRA32, false);
sensorData.colorImageTexture.wrapMode = TextureWrapMode.Clamp;
sensorData.colorImageTexture.filterMode = FilterMode.Point;
}
// depth camera data & intrinsics
if ((dwFlags & KinectInterop.FrameSource.TypeDepth) != 0)
{
CameraCalibration depthCamera = coordMapperCalib.DepthCameraCalibration;
sensorData.depthImageWidth = depthCamera.ResolutionWidth;
sensorData.depthImageHeight = depthCamera.ResolutionHeight;
rawDepthImage = new ushort[sensorData.depthImageWidth * sensorData.depthImageHeight];
sensorData.depthImage = new ushort[sensorData.depthImageWidth * sensorData.depthImageHeight];
}
// infrared data
if ((dwFlags & KinectInterop.FrameSource.TypeInfrared) != 0)
{
if (sensorData.depthImageWidth == 0 || sensorData.depthImageHeight == 0)
{
CameraCalibration depthCamera = coordMapperCalib.DepthCameraCalibration;
sensorData.depthImageWidth = depthCamera.ResolutionWidth;
sensorData.depthImageHeight = depthCamera.ResolutionHeight;
}
rawInfraredImage = new ushort[sensorData.depthImageWidth * sensorData.depthImageHeight];
sensorData.infraredImage = new ushort[sensorData.depthImageWidth * sensorData.depthImageHeight];
minInfraredValue = 0f;
maxInfraredValue = 1000f;
}
// imu data
if (kinectPlayback != null || (dwFlags & KinectInterop.FrameSource.TypePose) != 0)
{
imuAhrs = new AHRS.MahonyAHRS(0.0006f, 5f); // 600us
imuTurnRot1 = Quaternion.Euler(0f, 90f, 90f);
imuTurnRot2 = Quaternion.Euler(90f, !flipImuRotation ? 90f : -90f, 0f);
//lastFlipImuRot = flipImuRotation;
}
// calibration data
GetCameraIntrinsics(CalibrationDeviceType.Color, coordMapperCalib.ColorCameraCalibration, ref sensorData.colorCamIntr);
GetCameraIntrinsics(CalibrationDeviceType.Depth, coordMapperCalib.DepthCameraCalibration, ref sensorData.depthCamIntr);
GetCameraExtrinsics(coordMapperCalib.ColorCameraCalibration.Extrinsics, ref sensorData.depth2ColorExtr);
GetCameraExtrinsics(coordMapperCalib.DepthCameraCalibration.Extrinsics, ref sensorData.color2DepthExtr);
// body & body-index data
if ((dwFlags & KinectInterop.FrameSource.TypeBody) != 0)
{
InitBodyTracking(dwFlags, sensorData, coordMapperCalib, true);
if (isSyncBodyAndDepth && bodyTracker == null)
{
// don't sync body and depth if the body tracker can't be created
isSyncBodyAndDepth = false;
}
}
Debug.Log("Kinect4Azure-sensor opened.");
return(sensorData);
}
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)
{
// body-tracker frame
if (btCapture.Color != null && rawColorImage != null && 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 = (ulong)btCapture.Color.DeviceTimestamp.Ticks;
//Debug.Log("D" + deviceIndex + " RawColorTimestamp: " + rawColorTimestamp);
}
}
if (btCapture.Depth != null && rawDepthImage != null && 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.IR != null && rawInfraredImage != null && 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 = (ulong)btCapture.IR.DeviceTimestamp.Ticks;
//Debug.Log("D" + deviceIndex + " RawInfraredTimestamp: " + rawInfraredTimestamp);
}
}
}
else
{
// sensor frame
if (frames.colorFrame != null && rawColorImage != null && rawColorTimestamp == sensorData.lastColorFrameTime)
{
lock (colorFrameLock)
{
KinectInterop.CopyBytes(frames.colorFrame.Data, rawColorImage.Length, rawColorImage, rawColorImage.Length, sizeof(byte));
rawColorTimestamp = frames.deviceTimestamp;
//Debug.Log("D" + deviceIndex + " RawColorTimestamp: " + rawColorTimestamp);
}
}
if (frames.depthFrame != null && rawDepthImage != null && rawDepthTimestamp == sensorData.lastDepthFrameTime)
{
lock (depthFrameLock)
{
frames.depthFrame.CopyTo <ushort>(rawDepthImage);
rawDepthTimestamp = frames.deviceTimestamp;
//Debug.Log("D" + deviceIndex + " RawDepthTimestamp: " + rawDepthTimestamp);
}
}
if (frames.infraredFrame != null && rawInfraredImage != null && 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 = 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);
}
}
}
