using System.Collections; using System.Collections.Generic; using UnityEngine; using Microsoft.Azure.Kinect.Sensor; using System; namespace com.rfilkov.kinect { public class Kinect4AzureInterface : DepthSensorBase { [Tooltip("Color camera resolution.")] public ColorCameraMode colorCameraMode = ColorCameraMode._1920_x_1080_30Fps; public enum ColorCameraMode : int { _1280_x_720_30Fps = 1, _1920_x_1080_30Fps = 2, _2560_x_1440_30Fps = 3, _2048_x_1536_30Fps = 4, _3840_x_2160_30Fps = 5, _4096_x_3072_15Fps = 6 } [Tooltip("Depth camera mode.")] public DepthCameraMode depthCameraMode = DepthCameraMode._640_x_576_30Fps_3_86mNfov; public enum DepthCameraMode : int { _320_x_288_30Fps_5_46mNfov = 1, _640_x_576_30Fps_3_86mNfov = 2, _512_x_512_30Fps_2_88mWfov = 3, _1024x1024_15Fps_2_21mWfov = 4, PassiveIR_30Fps = 5 } [Tooltip("Device sync mode, in case of multiple wired sensors.")] public WiredSyncMode deviceSyncMode = WiredSyncMode.Standalone; [Tooltip("Subordinate device delay off master (in usec), in case of multiple wired sensors.")] public int subDeviceDelayUsec = 0; [Tooltip("Whether to flip the IMU rotation.")] public bool flipImuRotation = false; // references to the sensor public Device kinectSensor = null; public Calibration coordMapper; public Transformation coordMapperTransform = null; private Image d2cColorData = null; private Image c2dDepthData = null; // playback and record public Playback kinectPlayback = null; private long playbackStartTime = 0; // status of the cameras private bool isCamerasStarted = false; private bool isImuStarted = false; // current frame number private ulong currentFrameNumber = 0; private TimeSpan timeToWait = TimeSpan.FromMilliseconds(0); // imu private ImuSample lastImuSample = null; private ImuSample curImuSample = null; private AHRS.MahonyAHRS imuAhrs = null; private Quaternion imuTurnRot1 = Quaternion.identity; private Quaternion imuTurnRot2 = Quaternion.identity; //private bool lastFlipImuRot = false; private Quaternion imuRotation = Quaternion.identity; private float prevRotZ = 0f; private float[] prevQuat = new float[4]; private Vector3 imuYawAtStart = Vector3.zero; private Quaternion imuFixAtStart = Quaternion.identity; private bool imuYawAtStartSet = false; // TODO: to be removed later //public event Action OnImuSample; public override KinectInterop.DepthSensorPlatform GetSensorPlatform() { return KinectInterop.DepthSensorPlatform.Kinect4Azure; } public override List GetAvailableSensors() { List alSensorInfo = new List(); int deviceCount = Device.GetInstalledCount(); for (int i = 0; i < deviceCount; i++) { KinectInterop.SensorDeviceInfo sensorInfo = new KinectInterop.SensorDeviceInfo(); sensorInfo.sensorId = i.ToString(); sensorInfo.sensorName = "Kinect4Azure"; sensorInfo.sensorCaps = KinectInterop.FrameSource.TypeAll; 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(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); 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 = 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 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; } 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 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); // 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 = 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]; 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, coordMapper.ColorCameraCalibration, ref sensorData.colorCamIntr); GetCameraIntrinsics(CalibrationDeviceType.Depth, coordMapper.DepthCameraCalibration, ref sensorData.depthCamIntr); GetCameraExtrinsics(coordMapper.ColorCameraCalibration.Extrinsics, ref sensorData.depth2ColorExtr); GetCameraExtrinsics(coordMapper.DepthCameraCalibration.Extrinsics, ref sensorData.color2DepthExtr); // body & body-index data if ((dwFlags & KinectInterop.FrameSource.TypeBody) != 0) { InitBodyTracking(dwFlags, sensorData, coordMapper, true); } Debug.Log("Kinect4Azure-sensor opened."); return sensorData; } public override void CloseSensor(KinectInterop.SensorData sensorData) { base.CloseSensor(sensorData); if(d2cColorData != null) { d2cColorData.Dispose(); d2cColorData = null; } if (c2dDepthData != null) { c2dDepthData.Dispose(); c2dDepthData = null; } if (coordMapperTransform != null) { // dispose coord-mapper transform coordMapperTransform.Dispose(); coordMapperTransform = null; } if(kinectPlayback != null) { // close the playback file kinectPlayback.Dispose(); kinectPlayback = null; } if (kinectSensor != null) { // stop cameras, if needed if(isCamerasStarted) { isCamerasStarted = false; kinectSensor.StopCameras(); } // stop IMU, if needed if(isImuStarted) { isImuStarted = false; kinectSensor.StopImu(); } // close the sensor kinectSensor.Dispose(); kinectSensor = null; } Debug.Log("Kinect4Azure-sensor closed."); } public override void PollSensorFrames(KinectInterop.SensorData sensorData) { try { if (kinectPlayback != null) { if (kinectPlayback.IsEndOfStream()) return; if(playbackStartTime == 0) { // start time playbackStartTime = DateTime.Now.Ticks; } long currentPlayTime = DateTime.Now.Ticks - playbackStartTime; if ((frameSourceFlags & (KinectInterop.FrameSource)0x7F) != 0) { kinectPlayback.SeekTimestamp((ulong)(currentPlayTime / 10)); Capture capture = kinectPlayback.GetNextCapture(); if (capture != null) { ProcessCameraFrames(sensorData, capture); capture.Dispose(); currentFrameNumber++; } else { Debug.Log("End of recording detected."); } } if ((frameSourceFlags & KinectInterop.FrameSource.TypePose) != 0) { ImuSample imuSample = kinectPlayback.GetNextImuSample(); while (imuSample != null) { ProcessImuFrame(imuSample); ulong imuTimestamp = (ulong)imuSample.AccelerometerTimestamp.Ticks; if (kinectPlayback.IsEndOfStream() || imuTimestamp >= rawDepthTimestamp) break; imuSample = kinectPlayback.GetNextImuSample(); } } } else { if (isCamerasStarted) { Capture capture = kinectSensor.GetCapture(timeToWait); ProcessCameraFrames(sensorData, capture); capture.Dispose(); currentFrameNumber++; } if (isImuStarted) { ImuSample imuSample = kinectSensor.GetImuSample(timeToWait); while(imuSample != null) { ProcessImuFrame(imuSample); imuSample = kinectSensor.GetImuSample(timeToWait); } } } } catch (System.TimeoutException) { // do nothing } catch (System.Exception ex) { Debug.LogException(ex); } } // 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("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("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("RawInfraredTimestamp: " + rawInfraredTimestamp); } } // transformation data frames if ((depth2ColorDataFrame != null || color2DepthDataFrame != null) && capture.Color != null && capture.Depth != null && (rawColorTimestamp != sensorData.lastColorFrameTime || rawDepthTimestamp != sensorData.lastDepthFrameTime)) { if (coordMapperTransform == null) { coordMapperTransform = coordMapper.CreateTransformation(); } if (depth2ColorDataFrame != null) { if(d2cColorData == null) { d2cColorData = new Image(ImageFormat.ColorBGRA32, sensorData.depthImageWidth, sensorData.depthImageHeight, sensorData.depthImageWidth * sensorData.colorImageStride); } lock (depth2ColorFrameLock) { coordMapperTransform.ColorImageToDepthCamera(capture.Depth, capture.Color, d2cColorData); d2cColorData.CopyTo(depth2ColorDataFrame, 0, 0, depth2ColorDataFrame.Length); lastDepth2ColorFrameTime = (ulong)capture.Depth.DeviceTimestamp.Ticks; } } if (color2DepthDataFrame != null) { if (c2dDepthData == null) { c2dDepthData = new Image(ImageFormat.Depth16, sensorData.colorImageWidth, sensorData.colorImageHeight, sensorData.colorImageWidth * sizeof(ushort)); } lock (color2DepthFrameLock) { coordMapperTransform.DepthImageToColorCamera(capture.Depth, c2dDepthData); c2dDepthData.CopyTo(color2DepthDataFrame, 0, 0, color2DepthDataFrame.Length); lastColor2DepthFrameTime = (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); } } // in playback mode - waits until the given time stamp private void WaitForPlaybackTimestamp(string frameSource, long frameTimestamp) { //Debug.Log(string.Format("{0} ts: {1}, sys: {2}", frameSource, frameTimestamp, (DateTime.Now.Ticks - playbackStartTime))); long currentPlayTime = DateTime.Now.Ticks - playbackStartTime; while (currentPlayTime < frameTimestamp) { currentPlayTime = DateTime.Now.Ticks - playbackStartTime; } } // processes the IMU frame private void ProcessImuFrame(ImuSample imuSample) { if (kinectPlayback != null) { //WaitForPlaybackTimestamp("acc", imuSample.AccelerometerTimestampInUsec); //WaitForPlaybackTimestamp("gyro", imuSample.GyroTimestampInUsec); } lastImuSample = curImuSample; curImuSample = imuSample; if (!flipImuRotation && lastImuSample == null && Mathf.Abs(imuSample.AccelerometerSample.Y) >= 1f) { // check for rolled imu flipImuRotation = true; imuTurnRot2 = Quaternion.Euler(90f, -90f, 0f); } if (lastImuSample != null && imuAhrs != null && imuSample.AccelerometerTimestamp.Ticks != lastImuSample.AccelerometerTimestamp.Ticks) { prevRotZ = imuRotation.eulerAngles.z; Array.Copy(imuAhrs.Quaternion, prevQuat, prevQuat.Length); imuAhrs.SamplePeriod = ((imuSample.AccelerometerTimestamp.Ticks - lastImuSample.AccelerometerTimestamp.Ticks) / 10) * 0.000001f; imuAhrs.Update(imuSample.GyroSample.X, imuSample.GyroSample.Y, imuSample.GyroSample.Z, imuSample.AccelerometerSample.X, imuSample.AccelerometerSample.Y, imuSample.AccelerometerSample.Z); float[] ahrsQuat = imuAhrs.Quaternion; imuRotation = new Quaternion(ahrsQuat[1], ahrsQuat[2], ahrsQuat[3], ahrsQuat[0]); Quaternion transRotation = imuTurnRot2 * imuRotation * imuTurnRot1; if (imuAhrs.E2 < 0.0001f && Mathf.Abs(imuRotation.eulerAngles.z - prevRotZ) <= 0.1f) { // correct the continuous yaw, when imu doesn't move Array.Copy(prevQuat, imuAhrs.Quaternion, prevQuat.Length); ahrsQuat = prevQuat; imuRotation = new Quaternion(ahrsQuat[1], ahrsQuat[2], ahrsQuat[3], ahrsQuat[0]); transRotation = imuTurnRot2 * imuRotation * imuTurnRot1; if (!imuYawAtStartSet) { // initial yaw imuYawAtStartSet = true; imuYawAtStart = new Vector3(0f, transRotation.eulerAngles.y, 0f); imuFixAtStart = Quaternion.Inverse(Quaternion.Euler(imuYawAtStart)); } } if (imuYawAtStartSet) { // fix the initial yaw transRotation = imuFixAtStart * transRotation; } lock (poseFrameLock) { // rawPosePosition rawPoseRotation = transRotation; rawPoseTimestamp = (ulong)imuSample.AccelerometerTimestamp.Ticks; //poseFrameNumber = currentFrameNumber; } } //var onImuSample = OnImuSample; //if(onImuSample != null) //{ // onImuSample.Invoke(imuSample, rawDepthTimestamp, rawColorTimestamp); //} } //public override bool UpdateSensorData(KinectInterop.SensorData sensorData, KinectManager kinectManager) //{ // bool bSuccess = base.UpdateSensorData(sensorData, kinectManager); // if (lastFlipImuRot != flipImuRotation) // { // Vector3 imuRot2 = imuTurnRot2.eulerAngles; // imuRot2.y = -imuRot2.y; // imuTurnRot2 = Quaternion.Euler(imuRot2); // } // return bSuccess; //} // gets the given camera intrinsics private void GetCameraIntrinsics(CalibrationDeviceType camType, CameraCalibration camParams, ref KinectInterop.CameraIntrinsics intr) { Intrinsics camIntr = camParams.Intrinsics; if (camIntr.Parameters.Length < 15) throw new System.Exception("Intrinsics length is less than expected: " + camIntr.ParameterCount); intr = new KinectInterop.CameraIntrinsics(); intr.cameraType = (int)camType; intr.width = camParams.ResolutionWidth; intr.height = camParams.ResolutionHeight; // 0 float cx; // 1 float cy; intr.ppx = camIntr.Parameters[0]; intr.ppy = camIntr.Parameters[1]; // 2 float fx; /**< Focal length x */ // 3 float fy; /**< Focal length y */ intr.fx = camIntr.Parameters[2]; intr.fy = camIntr.Parameters[3]; // 4 float k1; // 5 float k2; // 6 float k3; // 7 float k4; // 8 float k5; // 9 float k6; intr.distCoeffs = new float[6]; intr.distCoeffs[0] = camIntr.Parameters[4]; intr.distCoeffs[1] = camIntr.Parameters[5]; intr.distCoeffs[2] = camIntr.Parameters[6]; intr.distCoeffs[3] = camIntr.Parameters[7]; intr.distCoeffs[4] = camIntr.Parameters[8]; intr.distCoeffs[5] = camIntr.Parameters[9]; if (camIntr.Type == CalibrationModelType.Theta) intr.distType = KinectInterop.DistortionType.Theta; else if (camIntr.Type == CalibrationModelType.Polynomial3K) intr.distType = KinectInterop.DistortionType.Polynomial3K; else if (camIntr.Type == CalibrationModelType.Rational6KT) intr.distType = KinectInterop.DistortionType.Rational6KT; else intr.distType = (KinectInterop.DistortionType)camIntr.Type; // 10 float codx; // 11 float cody; intr.codx = camIntr.Parameters[10]; intr.cody = camIntr.Parameters[11]; // 12 float p2; // 13 float p1; intr.p2 = camIntr.Parameters[12]; intr.p1 = camIntr.Parameters[13]; // 14 float metric_radius; intr.maxRadius = camIntr.Parameters[14]; EstimateFOV(intr); } // gets the given camera extrinsics private void GetCameraExtrinsics(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); } public override 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); Vector3 colorSpacePos = TransformPoint(sensorData.depth2ColorExtr, depthSpacePos); Vector2 colorPos = ProjectPoint(sensorData.colorCamIntr, colorSpacePos); return colorPos; } return Vector2.zero; } // unprojects plane point into the space protected override Vector3 UnprojectPoint(KinectInterop.CameraIntrinsics intr, Vector2 pixel, float depth) { if (depth <= 0f) return Vector3.zero; System.Numerics.Vector2 fPixel = new System.Numerics.Vector2(pixel.x, pixel.y); System.Numerics.Vector3? fPoint = coordMapper.TransformTo3D(fPixel, depth, (CalibrationDeviceType)intr.cameraType, (CalibrationDeviceType)intr.cameraType); Vector3 point = fPoint.HasValue ? new Vector3(fPoint.Value.X, fPoint.Value.Y, fPoint.Value.Z) : Vector3.zero; return point; } // projects space point onto a plane protected override Vector2 ProjectPoint(KinectInterop.CameraIntrinsics intr, Vector3 point) { if (point == Vector3.zero) return Vector2.zero; System.Numerics.Vector3 fPoint = new System.Numerics.Vector3(point.x, point.y, point.z); System.Numerics.Vector2? fPixel = coordMapper.TransformTo2D(fPoint, (CalibrationDeviceType)intr.cameraType, (CalibrationDeviceType)intr.cameraType); Vector2 pixel = fPixel.HasValue ? new Vector2(fPixel.Value.X, fPixel.Value.Y) : Vector2.zero; return pixel; } // transforms a point from one space to another protected override Vector3 TransformPoint(KinectInterop.CameraExtrinsics extr, Vector3 point) { float toPointX = extr.rotation[0] * point.x + extr.rotation[1] * point.y + extr.rotation[2] * point.z + extr.translation[0]; float toPointY = extr.rotation[3] * point.x + extr.rotation[4] * point.y + extr.rotation[5] * point.z + extr.translation[1]; float toPointZ = extr.rotation[6] * point.x + extr.rotation[7] * point.y + extr.rotation[8] * point.z + extr.translation[2]; return new Vector3(toPointX, toPointY, toPointZ); } //public override bool MapDepthFrameToColorData(KinectInterop.SensorData sensorData, ref byte[] vColorFrameData) //{ // if(coordMapperTransform == null) // { // coordMapperTransform = coordMapper.CreateTransformation(); // } // if(vColorFrameData == null) // { // vColorFrameData = new byte[sensorData.colorImageWidth * sensorData.colorImageHeight * sensorData.colorImageStride]; // } // return false; //} //public override bool MapColorFrameToDepthData(KinectInterop.SensorData sensorData, ref ushort[] vDepthFrameData) //{ // return false; //} } }