예제 #1
0
        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);
        }
예제 #2
0
        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);
        }
예제 #3
0
        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);
        }
예제 #4
0
        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
                    });
                }
            }
        }