public static VideoSenderMessageData Parse(byte[] messageBytes) { var reader = new BinaryReader(new MemoryStream(messageBytes)); var videoSenderMessageData = new VideoSenderMessageData(); videoSenderMessageData.frameTimeStamp = reader.ReadSingle(); videoSenderMessageData.keyframe = reader.ReadBoolean(); int colorEncoderFrameSize = reader.ReadInt32(); videoSenderMessageData.colorEncoderFrame = reader.ReadBytes(colorEncoderFrameSize); int depthEncoderFrameSize = reader.ReadInt32(); videoSenderMessageData.depthEncoderFrame = reader.ReadBytes(depthEncoderFrameSize); bool hasFloor = reader.ReadBoolean(); if (hasFloor) { var floor = new float[4]; floor[0] = reader.ReadSingle(); floor[1] = reader.ReadSingle(); floor[2] = reader.ReadSingle(); floor[3] = reader.ReadSingle(); videoSenderMessageData.floor = floor; } else { videoSenderMessageData.floor = null; } return(videoSenderMessageData); }
public static void ConvertFloorFromVideoSenderMessageDataToPositionAndRotation(VideoSenderMessageData videoSenderMessageData, out Vector3 position, out Quaternion rotation) { //Vector3 upVector = new Vector3(floorSenderPacketData.a, floorSenderPacketData.b, floorSenderPacketData.c); // y component is fliped since the coordinate system of unity and azure kinect is different. Plane floorPacketPlane = new Plane(new Vector3(videoSenderMessageData.floor[0], -videoSenderMessageData.floor[1], videoSenderMessageData.floor[2]), videoSenderMessageData.floor[3]); //Vector3 upVector = new Vector3(floorSenderPacketData.a, -floorSenderPacketData.b, floorSenderPacketData.c); position = floorPacketPlane.normal * floorPacketPlane.distance; rotation = Quaternion.FromToRotation(Vector3.up, floorPacketPlane.normal); }
public static VideoSenderMessageData Parse(byte[] messageBytes) { var reader = new BinaryReader(new MemoryStream(messageBytes)); var videoSenderMessageData = new VideoSenderMessageData(); videoSenderMessageData.frameTimeStamp = reader.ReadSingle(); videoSenderMessageData.keyframe = reader.ReadBoolean(); int colorEncoderFrameSize = reader.ReadInt32(); int depthEncoderFrameSize = reader.ReadInt32(); videoSenderMessageData.colorEncoderFrame = reader.ReadBytes(colorEncoderFrameSize); videoSenderMessageData.depthEncoderFrame = reader.ReadBytes(depthEncoderFrameSize); return(videoSenderMessageData); }
public void UpdateFrame(List <Tuple <int, VideoSenderMessageData> > videoMessageList) { VideoSenderMessageData videoMessageData = null; foreach (var videoMessagePair in videoMessageList) { if (videoMessagePair.Item2.floor != null) { videoMessageData = videoMessagePair.Item2; } } if (videoMessageData == null) { return; } Vector3 position; Quaternion rotation; FloorUtils.ConvertFloorFromVideoSenderMessageDataToPositionAndRotation(videoMessageData, out position, out rotation); floorTransform.localPosition = position; floorTransform.localRotation = rotation; Vector3 inversePosition; Quaternion inverseRotation; FloorUtils.InvertPositionAndRotation(position, rotation, out inversePosition, out inverseRotation); Vector3 inversePlaneNormal; float inversePlaneHeight; FloorUtils.ConvertPositionAndRotationToNormalAndY(inversePosition, inverseRotation, out inversePlaneNormal, out inversePlaneHeight); //floorTransformInverterTransform.localPosition = new Vector3(0.0f, inversePositionY, 0.0f); //floorTransformInverterTransform.localRotation = inverseRotation; inversePlaneNormalQueue.Enqueue(inversePlaneNormal); inversePlaneHeightQueue.Enqueue(inversePlaneHeight); if (inversePlaneNormalQueue.Count > 30) { inversePlaneNormalQueue.Dequeue(); } if (inversePlaneHeightQueue.Count > 30) { inversePlaneHeightQueue.Dequeue(); } Vector3 inversePlaneNormalSum = Vector3.zero; foreach (var normal in inversePlaneNormalQueue) { inversePlaneNormalSum += normal; } float inversePlaneHeightSum = 0.0f; foreach (var height in inversePlaneHeightQueue) { inversePlaneHeightSum += height; } Vector3 inversePlaneNormalAverage = inversePlaneNormalSum / inversePlaneHeightQueue.Count; float inversePlaneHeightAverage = inversePlaneHeightSum / inversePlaneHeightQueue.Count; Quaternion inversePlaneNormalAverageRotation = Quaternion.FromToRotation(Vector3.up, inversePlaneNormalAverage); //floorTransformInverterTransform.localPosition = new Vector3(0.0f, inversePlaneHeightAverage, 0.0f); gimbalTransform.localRotation = inversePlaneNormalAverageRotation; }
public void Assemble(UdpSocket udpSocket, List <VideoSenderPacketData> videoPacketDataList, List <ParitySenderPacketData> parityPacketDataList, int lastVideoFrameId, List <Tuple <int, VideoSenderMessageData> > videoMessageList) { int?addedFrameId = null; // Collect the received video packets. foreach (var videoSenderPacketData in videoPacketDataList) { if (videoSenderPacketData.frameId <= lastVideoFrameId) { continue; } if (!videoPacketCollections.ContainsKey(videoSenderPacketData.frameId)) { videoPacketCollections[videoSenderPacketData.frameId] = new VideoSenderPacketData[videoSenderPacketData.packetCount]; // Assign the largest new frame_id. if (!addedFrameId.HasValue || addedFrameId < videoSenderPacketData.frameId) { addedFrameId = videoSenderPacketData.frameId; } } videoPacketCollections[videoSenderPacketData.frameId][videoSenderPacketData.packetIndex] = videoSenderPacketData; } // Collect the received parity packets. foreach (var paritySenderPacketData in parityPacketDataList) { if (paritySenderPacketData.frameId <= lastVideoFrameId) { continue; } if (!parityPacketCollections.ContainsKey(paritySenderPacketData.frameId)) { parityPacketCollections[paritySenderPacketData.frameId] = new ParitySenderPacketData[paritySenderPacketData.packetCount]; } parityPacketCollections[paritySenderPacketData.frameId][paritySenderPacketData.packetIndex] = paritySenderPacketData; } if (addedFrameId.HasValue) { foreach (var videoPacketCollection in videoPacketCollections) { int frameId = videoPacketCollection.Key; VideoSenderPacketData[] videoPackets = videoPacketCollection.Value; // Skip the frame that just got added or even newer. if (frameId >= addedFrameId) { continue; } // Find the parity packet collection corresponding to the video packet collection. // Skip if there is no parity packet collection for the video frame. if (!parityPacketCollections.ContainsKey(frameId)) { continue; } ParitySenderPacketData[] parityPackets = parityPacketCollections[frameId]; // Loop per each parity packet. // Collect video packet indices to request. List <int> videoPacketIndiecsToRequest = new List <int>(); List <int> parityPacketIndiecsToRequest = new List <int>(); for (int parityPacketIndex = 0; parityPacketIndex < parityPackets.Length; ++parityPacketIndex) { // Range of the video packets that correspond to the parity packet. int videoPacketStartIndex = parityPacketIndex * FEC_GROUP_SIZE; // Pick the end index with the end of video packet indices in mind (i.e., prevent overflow). int videoPacketEndIndex = Math.Min(videoPacketStartIndex + FEC_GROUP_SIZE, videoPackets.Length); // If the parity packet is missing, request all missing video packets and skip the FEC process. // Also request the parity packet if there is a relevant missing video packet. if (parityPackets[parityPacketIndex] == null) { bool parityPacketNeeded = false; for (int videoPacketIndex = videoPacketStartIndex; videoPacketIndex < videoPacketEndIndex; ++videoPacketIndex) { if (videoPackets[videoPacketIndex] == null) { videoPacketIndiecsToRequest.Add(videoPacketIndex); parityPacketNeeded = true; } } if (parityPacketNeeded) { parityPacketIndiecsToRequest.Add(parityPacketIndex); } continue; } // Find if there is existing video packets and missing video packet indices. // Check all video packets that relates to this parity packet. var existingVideoPackets = new List <VideoSenderPacketData>(); var missingVideoPacketIndices = new List <int>(); for (int videoPacketIndex = videoPacketStartIndex; videoPacketIndex < videoPacketEndIndex; ++videoPacketIndex) { if (videoPackets[videoPacketIndex] != null) { existingVideoPackets.Add(videoPackets[videoPacketIndex]); } else { missingVideoPacketIndices.Add(videoPacketIndex); } } // Skip if there all video packets already exist. if (missingVideoPacketIndices.Count == 0) { continue; } // XOR based FEC only works for a single missing packet. if (missingVideoPacketIndices.Count > 1) { foreach (int missingIndex in missingVideoPacketIndices) { // Add the missing video packet indices for the vector to request them. videoPacketIndiecsToRequest.Add(missingIndex); } continue; } // The missing video packet index. int missingVideoPacketIndex = missingVideoPacketIndices[0]; // Reconstruct the missing video packet. VideoSenderPacketData fecVideoPacketData = new VideoSenderPacketData(); fecVideoPacketData.frameId = frameId; fecVideoPacketData.packetIndex = missingVideoPacketIndex; fecVideoPacketData.packetCount = videoPackets.Length; // Assign from the parity packet here since other video packets will be XOR'ed in the below loop. fecVideoPacketData.messageData = parityPackets[parityPacketIndex].bytes; foreach (var existingVideoPacket in existingVideoPackets) { for (int i = 0; i < fecVideoPacketData.messageData.Length; ++i) { fecVideoPacketData.messageData[i] ^= existingVideoPacket.messageData[i]; } } // Insert the reconstructed packet. videoPacketCollections[frameId][missingVideoPacketIndex] = fecVideoPacketData; } // Request the video packets that FEC was not enough to fix. udpSocket.Send(PacketHelper.createRequestReceiverPacketBytes(sessionId, frameId, videoPacketIndiecsToRequest, parityPacketIndiecsToRequest), remoteEndPoint); } } // Find all full collections and their frame_ids. var fullFrameIds = new List <int>(); foreach (var collectionPair in videoPacketCollections) { bool full = true; foreach (var packetData in collectionPair.Value) { if (packetData == null) { full = false; break; } } if (full) { int frameId = collectionPair.Key; fullFrameIds.Add(frameId); } } // Extract messages from the full collections. foreach (int fullFrameId in fullFrameIds) { var ms = new MemoryStream(); foreach (var packetData in videoPacketCollections[fullFrameId]) { ms.Write(packetData.messageData, 0, packetData.messageData.Length); } var videoMessageData = VideoSenderMessageData.Parse(ms.ToArray()); videoMessageList.Add(new Tuple <int, VideoSenderMessageData>(fullFrameId, videoMessageData)); videoPacketCollections.Remove(fullFrameId); } // Clean up frame_packet_collections. var obsoleteFrameIds = new List <int>(); foreach (var collectionPair in videoPacketCollections) { if (collectionPair.Key <= lastVideoFrameId) { obsoleteFrameIds.Add(collectionPair.Key); } } foreach (int obsoleteFrameId in obsoleteFrameIds) { videoPacketCollections.Remove(obsoleteFrameId); } }
public void Run(KinectReceiver kinectReceiver, ConcurrentQueue <VideoSenderPacketData> videoPacketDataQueue, ConcurrentQueue <FecSenderPacketData> fecPacketDataQueue) { // The logic for XOR FEC packets are almost the same to frame packets. // The operations for XOR FEC packets should happen before the frame packets // so that frame packet can be created with XOR FEC packets when a missing // frame packet is detected. FecSenderPacketData fecSenderPacketData; while (fecPacketDataQueue.TryDequeue(out fecSenderPacketData)) { if (fecSenderPacketData.frameId <= kinectReceiver.lastVideoFrameId) { continue; } if (!fecPacketCollections.ContainsKey(fecSenderPacketData.frameId)) { fecPacketCollections[fecSenderPacketData.frameId] = new FecSenderPacketData[fecSenderPacketData.packetCount]; } fecPacketCollections[fecSenderPacketData.frameId][fecSenderPacketData.packetIndex] = fecSenderPacketData; } VideoSenderPacketData videoSenderPacketData; while (videoPacketDataQueue.TryDequeue(out videoSenderPacketData)) { if (videoSenderPacketData.frameId <= kinectReceiver.lastVideoFrameId) { continue; } // If there is a packet for a new frame, check the previous frames, and if // there is a frame with missing packets, try to create them using xor packets. // If using the xor packets fails, request the sender to retransmit the packets. if (!videoPacketCollections.ContainsKey(videoSenderPacketData.frameId)) { videoPacketCollections[videoSenderPacketData.frameId] = new VideoSenderPacketData[videoSenderPacketData.packetCount]; /////////////////////////////////// // Forward Error Correction Start// /////////////////////////////////// // Request missing packets of the previous frames. foreach (var collectionPair in videoPacketCollections) { if (collectionPair.Key < videoSenderPacketData.frameId) { int missingFrameId = collectionPair.Key; //var missingPacketIndices = collectionPair.Value.GetMissingPacketIds(); var missingPacketIndices = new List <int>(); for (int i = 0; i < collectionPair.Value.Length; ++i) { if (collectionPair.Value[i] == null) { missingPacketIndices.Add(i); } } // Try correction using XOR FEC packets. var fecFailedPacketIndices = new List <int>(); var fecPacketIndices = new List <int>(); // missing_packet_index cannot get error corrected if there is another missing_packet_index // that belongs to the same XOR FEC packet... foreach (int i in missingPacketIndices) { bool found = false; foreach (int j in missingPacketIndices) { if (i == j) { continue; } if ((i / XOR_MAX_GROUP_SIZE) == (j / XOR_MAX_GROUP_SIZE)) { found = true; break; } } if (found) { fecFailedPacketIndices.Add(i); } else { fecPacketIndices.Add(i); } } foreach (int fecPacketIndex in fecPacketIndices) { // Try getting the XOR FEC packet for correction. int xorPacketIndex = fecPacketIndex / XOR_MAX_GROUP_SIZE; if (!fecPacketCollections.ContainsKey(missingFrameId)) { fecFailedPacketIndices.Add(fecPacketIndex); continue; } var fecPacketData = fecPacketCollections[missingFrameId][xorPacketIndex]; // Give up if there is no xor packet yet. if (fecPacketData == null) { fecFailedPacketIndices.Add(fecPacketIndex); continue; } var fecVideoPacketData = new VideoSenderPacketData(); fecVideoPacketData.frameId = missingFrameId; fecVideoPacketData.packetIndex = videoSenderPacketData.packetIndex; fecVideoPacketData.packetCount = videoSenderPacketData.packetCount; fecVideoPacketData.messageData = fecPacketData.bytes; int beginFramePacketIndex = xorPacketIndex * XOR_MAX_GROUP_SIZE; int endFramePacketIndex = Math.Min(beginFramePacketIndex + XOR_MAX_GROUP_SIZE, collectionPair.Value.Length); // Run bitwise XOR with all other packets belonging to the same XOR FEC packet. for (int i = beginFramePacketIndex; i < endFramePacketIndex; ++i) { if (i == fecPacketIndex) { continue; } for (int j = 0; j < fecVideoPacketData.messageData.Length; ++j) { fecVideoPacketData.messageData[j] ^= collectionPair.Value[i].messageData[j]; } } videoPacketCollections[missingFrameId][fecPacketIndex] = fecVideoPacketData; } // end of foreach (int missingPacketIndex in missingPacketIndices) kinectReceiver.udpSocket.Send(PacketHelper.createRequestReceiverPacketBytes(collectionPair.Key, fecFailedPacketIndices)); } } ///////////////////////////////// // Forward Error Correction End// ///////////////////////////////// } // End of if (frame_packet_collections.find(frame_id) == frame_packet_collections.end()) // which was for reacting to a packet for a new frame. videoPacketCollections[videoSenderPacketData.frameId][videoSenderPacketData.packetIndex] = videoSenderPacketData; } // Find all full collections and their frame_ids. var fullFrameIds = new List <int>(); foreach (var collectionPair in videoPacketCollections) { bool full = true; foreach (var packetData in collectionPair.Value) { if (packetData == null) { full = false; break; } } if (full) { int frameId = collectionPair.Key; fullFrameIds.Add(frameId); } } // Extract messages from the full collections. foreach (int fullFrameId in fullFrameIds) { var ms = new MemoryStream(); foreach (var packetData in videoPacketCollections[fullFrameId]) { ms.Write(packetData.messageData, 0, packetData.messageData.Length); } var videoMessageData = VideoSenderMessageData.Parse(ms.ToArray()); kinectReceiver.videoMessageQueue.Enqueue(new Tuple <int, VideoSenderMessageData>(fullFrameId, videoMessageData)); videoPacketCollections.Remove(fullFrameId); } // Clean up frame_packet_collections. var obsoleteFrameIds = new List <int>(); foreach (var collectionPair in videoPacketCollections) { if (collectionPair.Key <= kinectReceiver.lastVideoFrameId) { obsoleteFrameIds.Add(collectionPair.Key); } } foreach (int obsoleteFrameId in obsoleteFrameIds) { videoPacketCollections.Remove(obsoleteFrameId); } }