/// <summary>
/// Construct a visualization from its components.
/// </summary>
/// <param name="title">Window title.</param>
/// <param name="explorer">Main vehicle in the visualization.</param>
/// <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>
/// <param name="measurements">Recorded vehicle measurements.</param>
/// <param name="tags">Tags in the timeline.</param>
/// <param name="reftime">Reference time.</param>
/// <param name="online">True if the navigator works
/// incrementally with each time step.</param>
/// <param name="fps">Frame rate.</param>
/// <param name="sidebarfile">Sidebar video filename.</param>
/// <param name="screenshotmode">If true, skip to the screenshot tags,
/// set the camera, take the shots and close the file.</param>
public Viewer(string title,
SimulatedVehicle <MeasurerT, PoseT, MeasurementT> explorer,
TimedState trajectory, TimedTrajectory estimate,
TimedMapModel map, TimedMeasurements measurements, TimedMessage tags,
double reftime, bool online, double fps, string sidebarfile, bool screenshotmode)
: base(title, explorer, new FakeNavigator <MeasurerT, PoseT, MeasurementT>(explorer, online), false, fps)
{
xnavigator = Navigator as FakeNavigator <MeasurerT, PoseT, MeasurementT>;
Trajectory = trajectory;
Estimate = estimate;
Map = map;
Measurements = measurements;
Tags = tags;
FrameIndex = 0;
TagChanged = false;
VehicleWaypoints = new TimedTrajectory();
if (xnavigator.Online)
{
for (int i = 0; i < Trajectory.Count; i++)
{
TimedState partialtrajectory = new TimedState();
for (int k = 0; k <= i; k++)
{
partialtrajectory.Add(Tuple.Create(Trajectory[k].Item1, Trajectory[k].Item2));
}
VehicleWaypoints.Add(Tuple.Create(Trajectory[i].Item1, partialtrajectory));
}
for (int i = VehicleWaypoints.Count; i < Estimate.Count; i++)
{
VehicleWaypoints.Add(Tuple.Create(Estimate[i].Item1, Trajectory));
}
}
else
{
for (int i = 0; i < Estimate.Count; i++)
{
VehicleWaypoints.Add(Tuple.Create(Estimate[i].Item1, Trajectory));
}
}
Measurements.Insert(0, Tuple.Create(0.0, new List <double[]>()));
while (Measurements.Count < map.Count)
{
Measurements.Add(Tuple.Create(map[Measurements.Count].Item1, new List <double[]>()));
}
ScreenshotMode = screenshotmode;
if (screenshotmode)
{
List <int> shotindices = new List <int>();
foreach (var entry in Tags)
{
double time = entry.Item1;
string tag = entry.Item2;
if (tag.StartsWith("screenshot"))
{
int index = map.BinarySearch(Tuple.Create(time, new Map(3)),
new ComparisonComparer <Tuple <double, Map> >(
(Tuple <double, Map> a, Tuple <double, Map> b) =>
Math.Sign(a.Item1 - b.Item1)));
if (index != ~Tags.Count)
{
if (index < 0)
{
index = ~index;
}
if (Math.Abs(map[index].Item1 - time) < 1e-5)
{
shotindices.Add(index);
}
}
}
}
trajectory = new TimedState();
estimate = new TimedTrajectory();
map = new TimedMapModel();
measurements = new TimedMeasurements();
var waypoints = new TimedTrajectory();
foreach (int index in shotindices)
{
trajectory.Add(Trajectory [index]);
estimate.Add(Estimate [index]);
map.Add(Map [index]);
measurements.Add(Measurements [index]);
waypoints.Add(VehicleWaypoints[index]);
}
Trajectory = trajectory;
Estimate = estimate;
Map = map;
Measurements = measurements;
VehicleWaypoints = waypoints;
}
else
{
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;
PoseT dummy = new PoseT();
for (int i = 0; i < Estimate.Count; i++)
{
if (Estimate[i].Item2.Count > RefTime)
{
double[] delta = dummy.FromState(Trajectory[RefTime].Item2).Subtract(
dummy.FromState(Estimate[i].Item2[RefTime].Item2));
PoseT deltaP = dummy.FromLinear(delta);
double[] dtranslation = deltaP.Location;
double[][] drotation = deltaP.Orientation.ToMatrix();
double[] rotcenter = dummy.FromState(Estimate[i].Item2[RefTime].Item2).Location;
for (int k = 0; k < Estimate[i].Item2.Count; k++)
{
PoseT transformed = dummy.FromState(Estimate[i].Item2[k].Item2).Add(delta);
Estimate[i].Item2[k] = Tuple.Create(Estimate[i].Item2[k].Item1, transformed.State);
}
Map refmap = new Map(3);
foreach (var component in Map[i].Item2)
{
double[] transformed = drotation.Multiply(component.Mean.Subtract(rotcenter)).Add(rotcenter).
Add(dtranslation);
refmap.Add(new Gaussian(transformed,
drotation.Multiply(component.Covariance).MultiplyByTranspose(drotation),
component.Weight));
}
Map[i] = Tuple.Create(Map[i].Item1, refmap);
}
}
}
mapindices = new int[estimate.Count];
int h = 0;
for (int k = 0; k < estimate.Count; k++)
{
while (h < map.Count - 1 && map[h].Item1 < estimate[k].Item1)
{
h++;
}
mapindices[k] = h;
}
// note that the sidebar video read has to wait until LoadContent to get an appropiate Graphics context
this.sidebarfile = sidebarfile;
IsFixedTimeStep = true;
// override base class behavior (that is based on explorer HasSidebar)
if (!string.IsNullOrEmpty(sidebarfile))
{
SidebarWidth = 640;
SidebarHeight = 2 * 480;
ScreenCut = 0.7;
}
}
