/// <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(); }
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)); }
/// <summary> /// Run the simulation or viewer. /// </summary> public static void Run <MeasurerT, PoseT, MeasurementT>( string recfile, string scenefile, string commandfile, int particlecount, bool onlymapping, bool realtime, bool viewer, bool filterhistory, double reftime, bool screenshotmode, bool headless, bool noterminate, VehicleType input, NavigationAlgorithm algorithm) where PoseT : IPose <PoseT>, new() where MeasurementT : IMeasurement <MeasurementT>, new() where MeasurerT : IMeasurer <MeasurerT, PoseT, MeasurementT>, new() { if (viewer) { string tmpdir; using (var sim = Viewer <MeasurerT, PoseT, MeasurementT> . FromFiles(recfile, filterhistory, reftime, screenshotmode, out tmpdir)) { sim.ScreenshotPrefix = recfile + "-screenshot-"; setSignalHandler(sim); if (sim.Estimate.Count > 0) { sim.Run(); if (sim.TagChanged) { string datadir = Path.Combine(tmpdir, "data"); string bkpfile = recfile + ".bk"; Console.WriteLine(" -- tags were modified, rewriting"); File.WriteAllText(Path.Combine(datadir, "tags.out"), sim.SerializedTags); Console.WriteLine(" -- old file was moved to " + bkpfile); if (File.Exists(bkpfile)) { File.Delete(bkpfile); } File.Move(recfile, bkpfile); Console.WriteLine(" -- compressing"); ZipFile.CreateFromDirectory(datadir, recfile); } Directory.Delete(tmpdir, true); } else { Console.WriteLine("Nothing to show! Make sure there are appropriate frames " + "in the specified file. This can happen if screenshot mode is " + "used and there are no screenshot tags."); } } } else { using (var sim = Simulation <MeasurerT, PoseT, MeasurementT> . FromFiles(scenefile, commandfile, particlecount, input, algorithm, onlymapping, realtime, !headless, noterminate)) { sim.ScreenshotPrefix = recfile + "-screenshot-"; sim.CheckpointFile = recfile; setSignalHandler(sim); if (headless) { Stopwatch timer = new Stopwatch(); Console.WriteLine("running headless"); timer.Start(); sim.RunHeadless(); timer.Stop(); Console.WriteLine("finished running (" + timer.Elapsed.TotalSeconds.ToString("F4") + " s)"); } else { sim.Run(); } sim.SaveToFile(recfile); } } }