/// <summary>
/// Create a vehicle from a simulation file.
/// </summary>
/// <param name="descriptor">Scene descriptor text.</param>
/// <returns>Simulated vehicle parsed from file.</returns>
public static SimulatedVehicle <MeasurerT, PoseT, MeasurementT> FromFile(string descriptor)
{
Dictionary <string, List <string> > dict = Util.ParseDictionary(descriptor);
double[] vehiclepose = FP.ParseDoubleList(dict["pose"][0]);
double[] measurerdata = null;
string measurerkey = (dict.ContainsKey("focal")) ? "focal" :
(dict.ContainsKey("params")) ? "params" : "";
// "focal" is deprecated in lieu of "params" which is more global to different kinds of sensors
if (!string.IsNullOrEmpty(measurerkey))
{
measurerdata = FP.ParseDoubleList(dict[measurerkey][0]);
}
PoseT dummy = new PoseT();
PoseT pose = dummy.FromState(vehiclepose);
List <double[]> maploc = new List <double[]>();
List <string> mapdescript = dict["landmarks"];
for (int i = 0; i < mapdescript.Count; i++)
{
double[] landmark = FP.ParseDoubleList(mapdescript[i]);
if (landmark.Length != 3)
{
throw new FormatException("Map landmarks must be 3D");
}
maploc.Add(landmark);
}
if (measurerdata != null)
{
MeasurerT dummyM = new MeasurerT();
MeasurerT measurer = dummyM.FromLinear(measurerdata);
return(new SimulatedVehicle <MeasurerT, PoseT, MeasurementT>(pose, maploc, measurer));
}
else
{
return(new SimulatedVehicle <MeasurerT, PoseT, MeasurementT>(pose, maploc));
}
}
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);
}
/// <summary>
/// Create a visualization object from a pair of formatted description files.
/// </summary>
/// <param name="datafile">Compressed prerecorded data file.</param>
/// <param name="filterhistory">If true, show the filtered trajectory history, otherwise, the smooth one,
/// i.e. the smooth history may change retroactively from future knowledge. Note however that the
/// recorded vehicle may not support smoothing and may perform the same in both modes.
/// Note also that this is not the algorithm mode, only the visualization; the algorithm
/// could still take past information into account, but it won't be visualized.</param>
/// <param name="screenshotmode">If true, skip to the screenshot tags,
/// set the camera, take the shots and close the file.</param>
/// <param name="tmpdir">Temporary data directory, to be removed after use.</param>
/// <returns>Prepared visualization object.</returns>
/// <remarks>All file must be previously sorted by time value. This property is assumed.</remarks>
public static Viewer <MeasurerT, PoseT, MeasurementT> FromFiles(string datafile, bool filterhistory,
double reftime, bool screenshotmode, out string tmpdir)
{
tmpdir = Util.TemporaryDir();
string datadir = Path.Combine(tmpdir, "data");
ZipFile.ExtractToDirectory(datafile, datadir);
string scenefile = Path.Combine(datadir, "scene.world");
string vehiclefile = Path.Combine(datadir, "trajectory.out");
string estimatefile = Path.Combine(datadir, "estimate.out");
string mapfile = Path.Combine(datadir, "maps.out");
string measurefile = Path.Combine(datadir, "measurements.out");
string tagfile = Path.Combine(datadir, "tags.out");
string sidebarfile = Path.Combine(datadir, "sidebar.avi");
if (!File.Exists(scenefile))
{
scenefile = "";
}
if (!File.Exists(measurefile))
{
measurefile = "";
}
if (!File.Exists(tagfile))
{
tagfile = "";
}
if (!File.Exists(sidebarfile))
{
sidebarfile = "";
}
SimulatedVehicle <MeasurerT, PoseT, MeasurementT> explorer;
TimedState trajectory;
TimedTrajectory estimate;
TimedMapModel map;
TimedMeasurements measurements;
TimedMessage tags;
PoseT dummyP = new PoseT();
MeasurementT dummyM = new MeasurementT();
trajectory = FP.TimedArrayFromDescriptor(File.ReadAllLines(vehiclefile), dummyP.StateSize);
estimate = FP.TrajectoryHistoryFromDescriptor(File.ReadAllText(estimatefile), dummyP.StateSize, filterhistory);
map = FP.MapHistoryFromDescriptor(File.ReadAllText(mapfile), 3);
if (!string.IsNullOrEmpty(measurefile))
{
measurements = FP.MeasurementsFromDescriptor(File.ReadAllText(measurefile), dummyM.Size);
}
else
{
measurements = new TimedMeasurements();
}
if (!string.IsNullOrEmpty(tagfile))
{
tags = FP.TimedMessageFromDescriptor(File.ReadAllLines(tagfile));
}
else
{
tags = new TimedMessage();
}
if (!string.IsNullOrEmpty(scenefile))
{
explorer = SimulatedVehicle <MeasurerT, PoseT, MeasurementT> .
FromFile(File.ReadAllText(scenefile));
}
else
{
explorer = new SimulatedVehicle <MeasurerT, PoseT, MeasurementT>(new PoseT().IdentityP(), new List <double[]>());
}
bool online = (estimate[0].Item2.Count < estimate[estimate.Count - 1].Item2.Count &&
estimate.Count == trajectory.Count);
return(new Viewer <MeasurerT, PoseT, MeasurementT>("monorfs - viewing " + datafile, explorer,
trajectory, estimate, map, measurements, tags,
reftime, online, 30, sidebarfile, screenshotmode));
}
