예제 #1
0
        /// <summary>
        /// Perform a deep copy of another general vehicle,
        /// scaling the covariance matrices and setting a new detection probability.
        /// </summary>
        /// <param name="that">Copied vehicle.</param>
        /// <param name="motioncovmultiplier">Scalar multiplier for the motion covariance matrix.</param>
        /// <param name="measurecovmultiplier">Scalar multiplier for the measurement covariance matrix.</param>
        /// <param name="pdetection">Probability of detection.</param>
        /// <param name="clutter">Clutter density.</param>
        /// <param name="copytrajectory">If true, the vehicle historic trajectory is copied. Relatively heavy operation.</param>
        public KinectTrackVehicle(KinectVehicle that, double motioncovmultiplier, double measurecovmultiplier, double pdetection, double clutter, bool copytrajectory = false)
            : base(that, motioncovmultiplier, measurecovmultiplier, pdetection, clutter, copytrajectory)
        {
            // Since FilmArea is a property (and Rectangle a struct)
            // Inflate won't work (will occur on the temporary return value)
            // so a local copy is needed
            var newfilm = this.Measurer.FilmArea;

            newfilm.Inflate(-that.Border, -that.Border);

            this.Measurer = new KinectMeasurer(this.Measurer.VisionFocal,
                                               newfilm,
                                               this.Measurer.RangeClip,
                                               that.ResX, that.ResY, that.Border,
                                               () => that.DepthFrame);
        }
예제 #2
0
        FromFiles(string scenefile, string commandfile = "", int particlecount = 5,
                  VehicleType input             = VehicleType.Simulation,
                  NavigationAlgorithm algorithm = NavigationAlgorithm.PHD,
                  bool onlymapping = false, bool realtime   = false,
                  bool sidebar     = true, bool noterminate = false)
        {
            Vehicle <MeasurerT, PoseT, MeasurementT>   explorer;
            Navigator <MeasurerT, PoseT, MeasurementT> navigator;
            List <double[]> commands;

            // batch methods extra information
            TimedState        estimate     = null;
            TimedArray        odometry     = null;
            TimedMeasurements measurements = null;

            try {
                switch (input)
                {
                case VehicleType.Kinect:
                    if (typeof(MeasurementT) == typeof(PixelRangeMeasurement) &&
                        typeof(PoseT) == typeof(Pose3D) &&
                        typeof(MeasurerT) == typeof(KinectMeasurer))
                    {
                        explorer = KinectVehicle.FromSensor(scenefile, sidebar) as
                                   Vehicle <MeasurerT, PoseT, MeasurementT>;
                    }
                    else
                    {
                        throw new ArgumentException("KinectVehicle can only handle 3D Pose + PixelRange");
                    }
                    break;

                case VehicleType.Record:
                    if (algorithm == NavigationAlgorithm.LoopyPHD)
                    {
                        Console.WriteLine("reading Loopy PHD initialization data from file");
                        explorer = RecordVehicle <MeasurerT, PoseT, MeasurementT> .
                                   FromFile(scenefile, true, out estimate, out odometry, out measurements);
                    }
                    else
                    {
                        explorer = RecordVehicle <MeasurerT, PoseT, MeasurementT> .
                                   FromFile(scenefile, false, out estimate, out odometry, out measurements);
                    }
                    break;

                case VehicleType.Simulation:
                default:
                    explorer = SimulatedVehicle <MeasurerT, PoseT, MeasurementT> .
                               FromFile(File.ReadAllText(scenefile));

                    break;
                }
            }
            catch (IOException) {
                Console.WriteLine("Couldn't open vehicle file/device: " + scenefile);
                explorer = new SimulatedVehicle <MeasurerT, PoseT, MeasurementT>();
            }

            try {
                if (!string.IsNullOrEmpty(commandfile))
                {
                    commands = FP.CommandsFromDescriptor(File.ReadAllLines(commandfile));
                }
                else
                {
                    commands = new List <double[]>();
                }
            }
            catch (FileNotFoundException) {
                Console.WriteLine("command file not found: " + commandfile);
                commands = new List <double[]>();
            }

            switch (algorithm)
            {
            case NavigationAlgorithm.Odometry:
                navigator = new OdometryNavigator <MeasurerT, PoseT, MeasurementT>(explorer);
                break;

            case NavigationAlgorithm.ISAM2:
                navigator = new ISAM2Navigator <MeasurerT, PoseT, MeasurementT>(explorer, onlymapping);
                break;

            case NavigationAlgorithm.LoopyPHD:
                Console.WriteLine("initializing Loopy PHD");
                if (estimate != null)
                {
                    navigator = new LoopyPHDNavigator <MeasurerT, PoseT, MeasurementT>(explorer, estimate,
                                                                                       odometry, measurements);
                }
                else
                {
                    navigator = new LoopyPHDNavigator <MeasurerT, PoseT, MeasurementT>(explorer, commands,
                                                                                       new PHDNavigator <MeasurerT, PoseT, MeasurementT>(explorer, particlecount, onlymapping));
                }

                explorer = new FakeVehicle <MeasurerT, PoseT, MeasurementT>(explorer, true);

                Console.WriteLine("Loopy PHD initialized");

                break;

            case NavigationAlgorithm.PHD:
            default:
                navigator = new PHDNavigator <MeasurerT, PoseT, MeasurementT>(explorer, particlecount, onlymapping);
                break;
            }

            string title = "monorfs - simulating " + scenefile + " [" + algorithm + "]";

            return(new Simulation <MeasurerT, PoseT, MeasurementT>(title, explorer, navigator, commands, realtime, noterminate));
        }
예제 #3
0
        /// <summary>
        /// Primary entry point.
        /// </summary>
        /// <param name="args">Command line arguments.</param>
        //[STAThread]
        public static void Main(string[] args)
        {
            string recfile        = "data.zip";
            string scenefile      = "";
            string commandfile    = "";
            string configfile     = "";
            int    particlecount  = 1;
            double reftime        = 0;
            bool   onlymapping    = false;
            bool   realtime       = false;
            bool   viewer         = false;
            bool   filterhistory  = false;
            bool   screenshotmode = false;
            bool   headless       = false;
            bool   noterminate    = false;
            bool   showhelp       = false;

            VehicleType         input     = VehicleType.Simulation;
            NavigationAlgorithm algorithm = NavigationAlgorithm.PHD;

            OptionSet options = new OptionSet {
                { "f|scene=", "Scene description file. Simulated, recorded or device id.", f => scenefile = f },
                { "r|recfile=", "Recording file. Saves State and events for reviewing.", r => recfile = r },
                { "c|command=", "Auto-command file (simulates user input).", c => commandfile = c },
                { "g|config=", "Configuration file. Contains global constants.", g => configfile = g },
                { "a|algorithm=", "SLAM solver algorithm ('odometry', 'phd', 'loopy' or 'isam2').", a => algorithm = (a == "isam2") ? NavigationAlgorithm.ISAM2 : (a == "odometry") ? NavigationAlgorithm.Odometry : (a == "loopy") ? NavigationAlgorithm.LoopyPHD : NavigationAlgorithm.PHD },
                { "p|particles=", "Number of particles used for the RB-PHD.", (int p) => particlecount = p },
                { "y|onlymap", "Only do mapping, assuming known localization.", y => onlymapping = y != null },
                { "i|input=", "Vehicle input stream: 'kinect', 'simulation' or 'record'.", i => input = (i == "kinect") ? VehicleType.Kinect : (i == "record") ? VehicleType.Record : VehicleType.Simulation },
                { "R|realtime", "Process the system in realtime, instead of a fixed step.", R => realtime = R != null },
                { "v|view", "View a precorded session.", v => viewer = v != null },
                { "H|history=", "Trajectory history mode: either 'filter' or 'smooth'.", h => filterhistory = (h == "filter") },
                { "t|reftime=", "Reference time.", (double t) => reftime = t },
                { "s|screenshot", "Screenshot mode: just take the screenshots in the tags.", s => screenshotmode = s != null },
                { "x|headless", "Run headless, i.e. with no GUI.", x => headless = x != null },
                { "q|noterminate", "Skip simulation termination due command depletion o similars.", q => noterminate = q != null },
                { "h|help", "Show this message and exit.", h => showhelp = h != null }
            };

            try {
                options.Parse(args);
            }
            catch (OptionException e) {
                Console.Write("monorfs: ");
                Console.WriteLine(e.Message);
                Console.WriteLine("Try monorfs --help for more information.");
                Environment.Exit(1);
            }

            if (showhelp)
            {
                ShowHelp(options);
                return;
            }

            TimeSpan time = DateTime.Now.ToUniversalTime() - new DateTime(2010, 1, 1, 0, 0, 0, DateTimeKind.Utc);

            Util.SeedGenerators((int)time.TotalSeconds);

            if (!KinectVehicle.Initialize())
            {
                KinectVehicle.Shutdown();
                Environment.Exit(2);
            }

            // Reading configuration from cfg file specified in argument
            // or (if none was specified) from the internal config file of the recording file
            if (!string.IsNullOrEmpty(configfile))
            {
                try {
                    Config.FromFile(configfile);
                }
                catch (FileNotFoundException e) {
                    Console.WriteLine("Error: Configuration file '" + e.FileName + "' not found.");
                    Environment.Exit(3);
                }
            }
            else if (input == VehicleType.Record)
            {
                try {
                    Config.FromRecordFile(recfile);
                }
                catch (IOException) {
                    Console.WriteLine("Couldn't read config data from record file. Using default configuration.");
                }
            }
            else
            {
                Console.WriteLine("Using default configuration.");
            }

            try {
                switch (Config.Model)
                {
                case DynamicsModel.Linear1D:
                    Run <Linear1DMeasurer, LinearPose1D, LinearMeasurement1D>(
                        recfile, scenefile, commandfile,
                        particlecount, onlymapping, realtime, viewer,
                        filterhistory, reftime, screenshotmode, headless, noterminate,
                        input, algorithm);
                    break;

                case DynamicsModel.Linear2D:
                    Run <Linear2DMeasurer, LinearPose2D, LinearMeasurement2D>(
                        recfile, scenefile, commandfile,
                        particlecount, onlymapping, realtime, viewer,
                        filterhistory, reftime, screenshotmode, headless, noterminate,
                        input, algorithm);
                    break;

                case DynamicsModel.PRM3D:
                default:
                    if (input == VehicleType.Kinect)
                    {
                        Run <KinectMeasurer, Pose3D, PixelRangeMeasurement>(
                            recfile, scenefile, commandfile,
                            particlecount, onlymapping, realtime, viewer,
                            filterhistory, reftime, screenshotmode, headless, noterminate,
                            input, algorithm);
                    }
                    else
                    {
                        Run <PRM3DMeasurer, Pose3D, PixelRangeMeasurement>(
                            recfile, scenefile, commandfile,
                            particlecount, onlymapping, realtime, viewer,
                            filterhistory, reftime, screenshotmode, headless, noterminate,
                            input, algorithm);
                    }
                    break;
                }
            }
            catch (FileNotFoundException e) {
                Console.WriteLine("Error: File '" + e.FileName + "' not found.");
                Environment.Exit(4);
            }

            KinectVehicle.Shutdown();
            signalthread.Abort();
        }