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);
}
}
