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