Ejemplo n.º 1
0
        public void ReplayOffline(RawDataReader reader, Action <TimeSpan> updateTime, Action onClose, Func <bool> isPaused, Func <bool> isStopped)
        {
            _Tasks.Add(Task.Factory.StartNew(() =>
            {
                ProxyMode       = IOProxyMode.Offline;
                Stopwatch watch = new Stopwatch();
                watch.Start();
                int nextTimeUpdate = 1000;
                long startTime     = -1;
                int currentTime    = 0;
                while (reader.HasNext())
                {
                    while (isPaused())
                    {
                        watch.Stop();
                        Thread.Sleep(500);
                        watch.Start();
                    }
                    if (isStopped())
                    {
                        break;
                    }

                    Tuple <long, List <Tuple <RawReaderMode, object> > > res = reader.Next();
                    if (startTime == -1)
                    {
                        startTime = res.Item1;
                    }

                    ImuEventData imuEventData       = null;
                    CameraEventData cameraEventData = null;

                    int rawSize         = 0;
                    byte[] rawImage     = null;
                    byte[] rawImu       = null;
                    double exposureTime = 0.0;

                    foreach (Tuple <RawReaderMode, object> val in res.Item2)
                    {
                        if (val.Item1 == RawReaderMode.Imu0)
                        {
                            imuEventData = ImuEventData.Parse(res.Item1, (Tuple <double, double, double, double, double, double>)val.Item2, res.Item2.Any(c => c.Item1 == RawReaderMode.Camera0));
                            rawSize     += imuEventData.RawSize;
                            rawImu       = imuEventData.GetRaw(_SettingContainer.Settings.ImuSettings.GyroscopeScale, _SettingContainer.Settings.ImuSettings.AccelerometerScale, _SettingContainer.Settings.ImuSettings.TemperatureScale, _SettingContainer.Settings.ImuSettings.TemperatureOffset);
                        }
                        if (val.Item1 == RawReaderMode.Camera0)
                        {
                            cameraEventData = CameraEventData.Parse(((Tuple <double, byte[]>)val.Item2).Item2, 0, false, ((Tuple <double, byte[]>)val.Item2).Item1);
                            rawSize        += cameraEventData.RawSize;
                            rawImage        = ((Tuple <double, byte[]>)val.Item2).Item2;
                            exposureTime    = ((Tuple <double, byte[]>)val.Item2).Item1;
                        }
                    }
                    if (ConnectivityState == LinkUpConnectivityState.Connected)
                    {
                        if (rawSize > 0)
                        {
                            byte[] data = new byte[rawSize];
                            Array.Copy(rawImu, data, rawImu.Length);
                            if (rawImage != null)
                            {
                                Array.Copy(BitConverter.GetBytes(exposureTime), 0, data, imuEventData.RawSize, sizeof(double));
                                Array.Copy(rawImage, 0, data, imuEventData.RawSize + sizeof(double), rawImage.Length);
                            }
                            _ReplayDataSend.AsyncCall(data);
                        }
                    }
                    else
                    {
                        _BackgroundQueue.Add(new Tuple <ImuEventData, CameraEventData>(imuEventData, cameraEventData));
                    }

                    currentTime += reader.DeltaTimeMs;
                    int sleep    = (int)(currentTime - watch.ElapsedMilliseconds);
                    if (sleep > reader.DeltaTimeMs)
                    {
                        Thread.Sleep(sleep);
                    }

                    if (res.Item1 / 1000 > nextTimeUpdate)
                    {
                        nextTimeUpdate += 1000;
                        updateTime(reader.Length - TimeSpan.FromMilliseconds((res.Item1 - startTime) / (1000 * 1000)));
                    }
                }
                reader.Close();
                ProxyMode = IOProxyMode.Live;
                onClose();
            }, TaskCreationOptions.LongRunning));
        }
Ejemplo n.º 2
0
        public void Render(VIMatlabImporter matlabImporter, RawDataReader reader, Action <double> progress = null)
        {
            int    count     = reader.Count;
            double startTime = 0;

            int i = 0;

            List <KeyFrame> keyFrames = matlabImporter.GetKeyFrames();
            List <Frame>    frames    = matlabImporter.GetFrames();

            while (reader.HasNext())
            {
                i++;
                if (i % 100 == 0)
                {
                    progress?.Invoke((double)i / count);
                }

                Tuple <long, List <Tuple <RawReaderMode, object> > > res = reader.Next();
                double time = (double)res.Item1 / (1000 * 1000 * 1000);
                if (i == 1)
                {
                    startTime = time;
                }

                foreach (Tuple <RawReaderMode, object> val in res.Item2)
                {
                    if (val.Item1 == RawReaderMode.Camera0)
                    {
                        Mat rawImage = new Mat();
                        CvInvoke.Imdecode(((Tuple <double, byte[]>)val.Item2).Item2, ImreadModes.Grayscale, rawImage);

                        List <KeyFrame> kfs = keyFrames.Where(c => c.Frame.Time <= time).ToList();

                        if (kfs.Count > 0)
                        {
                            UpdateLastKeyFrame(kfs.Last(), rawImage, 0.2, 10);
                        }

                        foreach (KeyFrame keyFrame in kfs)
                        {
                            _SlamModel3D.AddNewKeyFrame(keyFrame);
                            keyFrames.Remove(keyFrame);
                        }

                        List <Frame> fs = frames.Where(c => c.Time <= time).ToList();

                        foreach (Frame frame in fs)
                        {
                            _SlamModel3D.AddNewFrame(frame);
                            frames.Remove(frame);
                        }

                        _SlamModel3D.Render();

                        SE3 Twc_last = _SlamModel3D.LastTransformation();

                        MoveCamera(Twc_last.Translation.X, Twc_last.Translation.Y, time - startTime);
                        WriteFrame(rawImage, RenderViewport());
                    }
                }
            }
        }