/// <summary>
/// Construct a visualization from its components.
/// </summary>
/// <param name="trajectory">Recorded vehicle trajectory.</param>
/// <param name="estimate">Recorded estimated vehicle trajectory.</param>
/// <param name="map">Recorded maximum-a-posteriori estimate for the map.</param>
public Plot(TimedState trajectory, TimedTrajectory estimate, TimedMapModel map,
TimedMapModel visiblelandmarks, Map landmarks, double c, double p, double reftime,
HistMode histmode, TimedMessage tags, bool online)
{
if (!online && histmode != HistMode.Timed)
{
throw new ArgumentException("Only time-average history is compatible with offline estimation.");
}
Trajectory = trajectory;
Estimate = estimate;
Map = map;
PoseErrorMode = histmode;
Landmarks = landmarks;
VisibleLandmarks = visiblelandmarks;
Tags = tags;
C = c;
P = p;
Online = online;
RefTime = Trajectory.BinarySearch(Tuple.Create(reftime, new double[0]), new ComparisonComparer <Tuple <double, double[]> >(
(Tuple <double, double[]> a, Tuple <double, double[]> b) => Math.Sign(a.Item1 - b.Item1)));
RefTime = (RefTime >= 0) ? RefTime : (RefTime != ~Trajectory.Count) ? ~RefTime : 0;
}
/// <summary>
/// The entry point of the program, where the program control starts and ends.
/// </summary>
/// <param name="args">The command-line arguments.</param>
public static void Main(string[] args)
{
string file = "data.zip";
double ospac = 1;
double ospap = 1;
double reftime = 0;
HistMode histmode = HistMode.Timed;
bool showhelp = false;
OptionSet options = new OptionSet {
{ "f|file=", "Recording file.", f => file = f },
{ "c=", "OSPA C parameter.", (double c) => ospac = c },
{ "p=", "OSPA P parameter.", (double p) => ospap = p },
{ "t|reftime=", "Reference time.", (double t) => reftime = t },
{ "H|history=", "Trajectory history mode: either 'filter', 'smooth' or 'timed'.", h => histmode = (h == "filter") ? HistMode.Filter : (h == "smooth") ? HistMode.Smooth : HistMode.Timed },
{ "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;
}
try {
Config.FromRecordFile(file);
}
catch (IOException) {
Console.WriteLine("Couldn't open the recording file.");
Environment.Exit(2);
}
try {
switch (Config.Model)
{
case DynamicsModel.Linear2D:
Run(Plot <Linear2DMeasurer, LinearPose2D, LinearMeasurement2D> .
FromFiles(file, histmode, ospac, ospap, reftime), file);
break;
case DynamicsModel.PRM3D:
default:
Run(Plot <PRM3DMeasurer, Pose3D, PixelRangeMeasurement> .
FromFiles(file, histmode, ospac, ospap, reftime), file);
break;
}
}
catch (FileNotFoundException e) {
Console.WriteLine("Error: File '" + e.FileName + "' not found.");
Environment.Exit(3);
}
}
FromFiles(string datafile, HistMode histmode,
double c, double p, double reftime)
{
string 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 vismapfile = Path.Combine(datadir, "vismaps.out");
string tagfile = Path.Combine(datadir, "tags.out");
if (!File.Exists(scenefile))
{
scenefile = "";
}
if (!File.Exists(vismapfile))
{
vismapfile = "";
}
if (!File.Exists(tagfile))
{
tagfile = "";
}
TimedState trajectory;
TimedTrajectory estimate;
TimedMapModel map;
TimedMapModel vislandmarks;
Map landmarks = new Map(3);
TimedMessage tags;
PoseT dummyP = new PoseT();
trajectory = FP.TimedArrayFromDescriptor(File.ReadAllLines(vehiclefile), dummyP.StateSize);
estimate = FP.TrajectoryHistoryFromDescriptor(File.ReadAllText(estimatefile), dummyP.StateSize);
map = FP.MapHistoryFromDescriptor(File.ReadAllText(mapfile), 3);
if (!string.IsNullOrEmpty(vismapfile))
{
vislandmarks = FP.MapHistoryFromDescriptor(File.ReadAllText(vismapfile), 3);
for (int i = vislandmarks.Count; i < map.Count; i++)
{
vislandmarks.Add(Tuple.Create(map[i].Item1, new Map(3)));
}
}
else
{
vislandmarks = new TimedMapModel();
foreach (var entry in map)
{
vislandmarks.Add(Tuple.Create(entry.Item1, new Map(3)));
}
}
double[][] did = { new double[3] {
1e-3, 0, 0
},
new double[3] {
0, 1e-3, 0
},
new double[3] {
0, 0, 1e-3
} };
if (!string.IsNullOrEmpty(scenefile))
{
var explorer = SimulatedVehicle <MeasurerT, PoseT, MeasurementT> .
FromFile(File.ReadAllText(scenefile));
foreach (var landmark in explorer.Landmarks)
{
landmarks.Add(new Gaussian(landmark, did, 1.0));
}
}
if (!string.IsNullOrEmpty(tagfile))
{
tags = FP.TimedMessageFromDescriptor(File.ReadAllLines(tagfile));
}
else
{
tags = new TimedMessage();
}
bool online = (estimate[0].Item2.Count < estimate[estimate.Count - 1].Item2.Count &&
estimate.Count == trajectory.Count);
if (!online && histmode != HistMode.Timed)
{
Console.WriteLine("Warning: Offline mode is not compatible with the selected history mode. " +
"Switching to time-average.");
histmode = HistMode.Timed;
}
return(new Plot <MeasurerT, PoseT, MeasurementT>(trajectory, estimate, map, vislandmarks,
landmarks, c, p, reftime, histmode, tags, online));
}