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));
}
/// <summary>
/// Allow the viewer to run logic such as updating the world,
/// and gathering input.
/// </summary>
/// <param name="time">Provides a snapshot of timing values.</param>
/// <param name="keyboard">Current keyboard state information.</param>
/// <param name="prevkeyboard">Old keyboard state information. Used to get newly pressed keys.</param>
/// <param name="multiplier">Movement scale multiplier.</param>
protected override void Update(GameTime time, KeyboardState keyboard, KeyboardState prevkeyboard, double multiplier)
{
if (FrameIndex >= Estimate.Count)
{
if (ScreenshotMode)
{
Exit();
return;
}
FrameIndex = Estimate.Count - 1;
Paused = true;
}
else if (FrameIndex < 0)
{
FrameIndex = 0;
Paused = true;
}
bool speedup = keyboard.IsKeyDown(Keys.LeftShift);
preframeindex = FrameIndex;
Explorer.WayPoints = VehicleWaypoints[FrameIndex].Item2;
Explorer.Pose = new PoseT().FromState(Explorer.WayPoints[Explorer.WayPoints.Count - 1].Item2);
xnavigator.MapModel = Map[mapindices[FrameIndex]].Item2;
xnavigator.WayTrajectories[0] = Estimate[FrameIndex];
xnavigator.Vehicle.WayPoints = Estimate[FrameIndex].Item2;
xnavigator.Vehicle.Pose = new PoseT().FromState(Navigator.BestEstimate.WayPoints[Navigator.BestEstimate.WayPoints.Count - 1].Item2);
if (xnavigator.Online)
{
MeasurementT dummy = new MeasurementT();
Explorer.MappedMeasurements.Clear();
foreach (double[] z in Measurements[mapindices[FrameIndex]].Item2)
{
Explorer.MappedMeasurements.Add(Explorer.Measurer.MeasureToMap(Explorer.Pose, dummy.FromLinear(z)));
}
}
if (!taggone)
{
TagColor = tagupdater.Current;
taggone = !tagupdater.MoveNext();
}
double viewtime = Estimate[FrameIndex].Item1;
int tagindex = Tags.BinarySearch(Tuple.Create(viewtime, ""));
if (tagindex != ~Tags.Count)
{
if (tagindex < 0)
{
tagindex = ~tagindex;
}
if (Math.Abs(Tags[tagindex].Item1 - viewtime) < 1e-5)
{
TagMessage = Tags[tagindex].Item2;
tagupdater = smoothFader(Color.White, Color.DimGray);
taggone = !tagupdater.MoveNext();
}
}
Message = string.Format("time = {0,12:N4} frame = {1,5:N0}", viewtime, FrameIndex);
if (ScreenshotMode)
{
// tag format: "screenshot theta_value phi_value zoom_value"
// e.g. "screenshot 12 -0.4 0.97"
if (TagMessage.StartsWith("screenshot"))
{
double[] parameters = FileParser.ParseDoubleList(TagMessage.Substring("screenshot".Length));
if (parameters.Length == 3)
{
double theta = parameters[0];
double phi = parameters[1];
double zoom = parameters[2];
SetCamera(theta, phi, zoom);
Draw(time);
Screenshot();
}
}
FrameIndex++;
}
else
{
// frame-by-frame fine time lookup, reverse
if (keyboard.IsKeyDown(Keys.Q) && !prevkeyboard.IsKeyDown(Keys.Q))
{
FrameIndex -= (speedup) ? 8 : 1;
Paused = true;
}
// frame-by-frame, forward
if (keyboard.IsKeyDown(Keys.W) && !prevkeyboard.IsKeyDown(Keys.W))
{
FrameIndex += (speedup) ? 8 : 1;
Paused = true;
}
// normal speed, reverse
if (keyboard.IsKeyDown(Keys.A))
{
FrameIndex -= (speedup) ? 8 : 1;
Paused = true;
}
// normal speed, forward
if (keyboard.IsKeyDown(Keys.S))
{
Paused = false;
}
// add a new tag
if (keyboard.IsKeyDown(Keys.G) && !prevkeyboard.IsKeyDown(Keys.G))
{
Tags.Add(Tuple.Create(viewtime, "User tag"));
Tags.Sort();
TagChanged = true;
}
if (!Paused)
{
FrameIndex += (speedup) ? 8 : 1;
}
}
}
