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)); }
FromFile(string datafile, bool extrainfo, out TimedState estimate, out TimedArray xodometry, out TimedMeasurements xmeasurements) { string tmpdir = Util.TemporaryDir(); string datadir = Path.Combine(tmpdir, "data"); ZipFile.ExtractToDirectory(datafile, datadir); string scenefile = Path.Combine(datadir, "scene.world"); string trajectoryfile = Path.Combine(datadir, "trajectory.out"); string estimatefile = Path.Combine(datadir, "estimate.out"); string odometryfile = Path.Combine(datadir, "odometry.out"); string measurefile = Path.Combine(datadir, "measurements.out"); string vismapfile = Path.Combine(datadir, "vismaps.out"); string tagfile = Path.Combine(datadir, "tags.out"); if (!File.Exists(scenefile)) { throw new ArgumentException("Missing scene file"); } if (extrainfo && !File.Exists(estimatefile)) { throw new ArgumentException("Missing estimate file"); } if (!File.Exists(trajectoryfile)) { throw new ArgumentException("Missing trajectory file"); } if (!File.Exists(odometryfile)) { throw new ArgumentException("Missing odometry file"); } if (!File.Exists(measurefile)) { throw new ArgumentException("Missing measurement file"); } if (!File.Exists(vismapfile)) { vismapfile = ""; } if (!File.Exists(tagfile)) { tagfile = ""; } RecordVehicle <MeasurerT, PoseT, MeasurementT> explorer; TimedState trajectory; TimedArray odometry; TimedMeasurements measurements; TimedMapModel vismap; TimedMessage tags; var template = SimulatedVehicle <MeasurerT, PoseT, MeasurementT> . FromFile(File.ReadAllText(scenefile)); trajectory = FP.TimedArrayFromDescriptor(File.ReadAllLines(trajectoryfile), StateSize); odometry = FP.TimedArrayFromDescriptor(File.ReadAllLines(odometryfile), OdoSize); measurements = FP.MeasurementsFromDescriptor(File.ReadAllText(measurefile), MeasureSize); if (!string.IsNullOrEmpty(vismapfile)) { vismap = FP.MapHistoryFromDescriptor(File.ReadAllText(vismapfile), 3); for (int i = vismap.Count; i < trajectory.Count; i++) { vismap.Add(Tuple.Create(trajectory[i].Item1, new Map(3))); } } else { vismap = new TimedMapModel(); foreach (var entry in trajectory) { vismap.Add(Tuple.Create(entry.Item1, new Map(3))); } } if (!string.IsNullOrEmpty(tagfile)) { tags = FP.TimedMessageFromDescriptor(File.ReadAllLines(tagfile)); } else { tags = new TimedMessage(); } explorer = new RecordVehicle <MeasurerT, PoseT, MeasurementT>(trajectory, odometry, measurements, tags, template.Landmarks, template.Measurer); explorer.WayVisibleMaps = vismap; if (extrainfo) { TimedTrajectory fullestimate = FP.TrajectoryHistoryFromDescriptor(File.ReadAllText(estimatefile), StateSize, false); estimate = fullestimate[fullestimate.Count - 1].Item2; explorer.WayPoints = trajectory; xodometry = odometry; xmeasurements = measurements; } else { estimate = null; xodometry = null; xmeasurements = null; } Directory.Delete(tmpdir, true); return(explorer); }