public static SlamMapEventData Parse(byte[] data)
{
SlamMapEventData obj = new SlamMapEventData();
obj.PublishType = (SlamPublishType)data[0];
if (obj.PublishType == SlamPublishType.Frame || obj.PublishType == SlamPublishType.KeyframeWithPoints)
{
int sizeFrame = ParseFrame(data, 1, obj);
if (obj.PublishType == SlamPublishType.KeyframeWithPoints)
{
int sizeKeyFrame = ParseKeyFrame(data, 1 + sizeFrame, obj);
obj.KeyFrame.Frame = obj.Frame;
int sizePoint = 0;
for (int i = 0; i < obj.KeyFrame.Points.Count; i++)
{
Point p = new Point();
sizePoint = ParsePoint(data, 1 + sizeFrame + sizeKeyFrame + sizePoint * i, p);
obj.KeyFrame.Points[i] = p;
}
}
}
return(obj);
}
private static int ParseFrame(byte[] data, int offset, SlamMapEventData obj)
{
int index = offset;
uint id = BitConverter.ToUInt32(data, index);
index += 4;
double time = BitConverter.ToDouble(data, index);
index += 8;
SE3 Tcw = new SE3();
SE3 Tbw = new SE3();
Vector3 v = new Vector3();
Vector3 bg = new Vector3();
Vector3 ba = new Vector3();
index = ParseSE3(data, index, Tcw);
index = ParseSE3(data, index, Tbw);
index = ParseVector3(data, index, v);
index = ParseVector3(data, index, bg);
index = ParseVector3(data, index, ba);
double scale = BitConverter.ToDouble(data, index);
index += 8;
obj._Frame = new Frame(id, time, Tcw, Tbw, v, bg, ba, scale);
return(index - offset);
}
private static int ParseKeyFrame(byte[] data, int offset, SlamMapEventData obj)
{
int index = offset;
uint id = BitConverter.ToUInt32(data, index);
index += 4;
int points = BitConverter.ToInt32(data, index);
index += 4;
double fx = BitConverter.ToDouble(data, index);
index += 8;
double fy = BitConverter.ToDouble(data, index);
index += 8;
double cx = BitConverter.ToDouble(data, index);
index += 8;
double cy = BitConverter.ToDouble(data, index);
index += 8;
obj._KeyFrame = new KeyFrame(id, fx, fy, cx, cy, points, obj.Frame);
return(index - offset);
}
private void _SlamMapEventLabel_Fired(LinkUpEventLabel label, byte[] data)
{
List <Tuple <IProxyEventSubscriber, ProxyEventType> > subscriber = null;
lock (_Subscriptions)
{
subscriber = _Subscriptions.Where(c => c.Item2 == ProxyEventType.SlamMapEvent).ToList();
}
if (label == _SlamMapEventLabel && subscriber != null && subscriber.Count() > 0)
{
SlamMapEventData eventData = SlamMapEventData.Parse(data);
foreach (Tuple <IProxyEventSubscriber, ProxyEventType> t in subscriber)
{
t.Item1.Fired(this, new List <AbstractProxyEventData>()
{
eventData
});
}
}
}
