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);
}
