/// <summary>
/// Predict the state of the map in the next iteration
/// given the trajectory of the vehicle.
/// </summary>
/// <param name="measurements">Sensor measurements in range-bearing form.</param>
/// <param name="pose">Vehicle pose that conditions the mapping.</param>
/// <param name="model">Associated map model.</param>
/// <param name="unexplored">Input/Output, unexplored candidates from previous timestep..</param>
/// <returns>Predicted map model.</returns>
/// <remarks>Though in theory the predict doesn't depend on the measurements,
/// the birth region (i.e. the unexplored areas) is large to represent on gaussians.
/// To diminish the computational time, only the areas near the new measurements are
/// used (i.e. the same measurements). This is equivalent to artificially increasing
/// belief on the new measurements (when in a new area).</remarks>
public Map PredictConditional(List <MeasurementT> measurements,
SimulatedVehicle <MeasurerT, PoseT, MeasurementT> pose,
Map model, List <double[]> unexplored)
{
// gaussian are born on any unexplored areas,
// as something is expected to be there,
// but this is too resource-intensive so
// here's a little cheat: only do it on areas
// there has been a new measurement lately
Map predicted = new Map(model);
unexplored.Clear();
foreach (MeasurementT measurement in measurements)
{
double[] candidate = pose.Measurer.MeasureToMap(pose.Pose, measurement);
if (!Explored(model, candidate))
{
unexplored.Add(candidate);
}
}
// birth RFS
foreach (double[] candidate in unexplored)
{
predicted.Add(new Gaussian(candidate, BirthCovariance, BirthWeight));
}
return(predicted);
}
/// <summary>
/// Copy constructor. Perform a deep copy of another simulated vehicle.
/// </summary>
/// <param name="that">Copied simulated vehicle.</param>
/// <param name="copytrajectory">If true, the vehicle historic trajectory is copied. Relatively heavy operation.</param>
public SimulatedVehicle(SimulatedVehicle <MeasurerT, PoseT, MeasurementT> that,
bool copytrajectory = false)
: this((Vehicle <MeasurerT, PoseT, MeasurementT>)that, copytrajectory)
{
this.detectionProbability = that.detectionProbability;
this.ClutterDensity = that.ClutterDensity;
this.ClutterCount = that.ClutterCount;
this.clutterGen = that.clutterGen;
}
/// <summary>
/// Generate a log-likelihood matrix relating each measurement with each landmark.
/// </summary>
/// <param name="measurements">Sensor measurements in pixel-range form.</param>
/// <param name="map">Map model.</param>
/// <param name="pose">Vehicle pose.</param>
/// <returns>Set log-likelihood matrix.</returns>
public static SparseMatrix SetLogLikeMatrix(List <MeasurementT> measurements, IMap map,
SimulatedVehicle <MeasurerT, PoseT, MeasurementT> pose)
{
// exact calculation if there are few components/measurements;
// use the most probable components approximation otherwise
SparseMatrix logprobs = new SparseMatrix(map.Count + measurements.Count, map.Count + measurements.Count, double.NegativeInfinity);
double logclutter = Math.Log(pose.ClutterDensity);
Gaussian[] zprobs = new Gaussian[map.Count];
int n = 0;
foreach (Gaussian landmark in map)
{
MeasurementT m = pose.Measurer.MeasurePerfect(pose.Pose, landmark.Mean);
double[] mlinear = m.ToLinear();
zprobs[n] = new Gaussian(mlinear, pose.MeasurementCovariance, pose.DetectionProbabilityM(m));
n++;
}
for (int i = 0; i < zprobs.Length; i++)
{
for (int k = 0; k < measurements.Count; k++)
{
double d = zprobs[i].Mahalanobis(measurements[k].ToLinear());
if (d < 5)
{
// prob = log (pD * zprob(measurement))
// this way multiplying probabilities equals to adding (negative) profits
logprobs[i, k] = Math.Log(zprobs[i].Weight) + Math.Log(zprobs[i].Multiplier) - 0.5 * d * d;
}
}
}
for (int i = 0; i < map.Count; i++)
{
logprobs[i, measurements.Count + i] = Math.Log(1 - zprobs[i].Weight);
}
for (int i = 0; i < measurements.Count; i++)
{
logprobs[map.Count + i, i] = logclutter;
}
return(logprobs);
}
/// <summary>
/// Construct a simulation from its components.
/// </summary>
/// <param name="title">Window title.</param>
/// <param name="explorer">Main vehicle in the simulation. This is the only entity the user directly controls.</param>
/// <param name="navigator">SLAM solver.</param>
/// <param name="commands">List of stored commands for the vehicle as odometry readings.</param>
/// <param name="realtime">If true, the system works at the highest speed it can;
/// otherwise, the framerate is fixed and even if processing takes longer than the timestep, the simulation works
/// as it had taken exactly the nominal rate.</param>
/// <param name="noterminate"> If true, the simulation will remain active after cues
/// like command depletion or vehicle requirements ask to do so.</param>
public Simulation(string title,
Vehicle <MeasurerT, PoseT, MeasurementT> explorer,
Navigator <MeasurerT, PoseT, MeasurementT> navigator,
List <double[]> commands,
bool realtime, bool noterminate)
: base(title, explorer, navigator, realtime)
{
Commands = commands;
commandindex = 0;
commanddepleted = false;
this.noterminate = noterminate;
timer = new Stopwatch();
timer.Start();
InitVehicle = new SimulatedVehicle <MeasurerT, PoseT, MeasurementT>(explorer);
SidebarHistory = new List <Color[]>();
WayMeasurements = new TimedMeasurements();
// note that WayMeasurements starts empty as measurements are between frames
// so it will have a length |frames| - 1
}
/// <summary>
/// Update the weights of each vehicle using the sequential importance sampling technique.
/// It uses most-probable-components approximation to solve the problem in polynomial time.
/// </summary>
/// <param name="measurements">Sensor measurements in pixel-range form.</param>
/// <param name="predicted">Predicted map model.</param>
/// <param name="corrected">Corrected map model.</param>
/// <param name="pose">Vehicle pose.</param>
/// <returns>Total weight update factor.</returns>
public double WeightAlpha(List <MeasurementT> measurements, Map predicted, Map corrected,
SimulatedVehicle <MeasurerT, PoseT, MeasurementT> pose)
{
IMap jmap = corrected.BestMapEstimate;
double ploglikelihood = 0;
double cloglikelihood = 0;
foreach (var component in jmap)
{
ploglikelihood += Math.Log(predicted.Evaluate(component.Mean));
cloglikelihood += Math.Log(corrected.Evaluate(component.Mean));
}
double pcount = predicted.ExpectedSize;
double ccount = corrected.ExpectedSize;
double setloglikelihood = SetLogLikelihood(measurements, jmap, pose);
double loglikelihoodratio = (ploglikelihood - pcount) - (cloglikelihood - ccount);
return(Math.Exp(setloglikelihood + loglikelihoodratio));
}
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));
}
/// <summary>
/// Copy constructor. Perform a deep copy of another tracker vehicle.
/// </summary>
/// <param name="that">Copied simulated vehicle.</param>
/// <param name="copytrajectory">If true, the vehicle historic trajectory is copied. Relatively heavy operation.</param>
public TrackVehicle(SimulatedVehicle <MeasurerT, PoseT, MeasurementT> that,
bool copytrajectory = false)
: base(that, copytrajectory)
{
}
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));
}
/// <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;
}
}
/// <summary>
/// Correct the estimated map given the localization
/// using a set of measurements.
/// </summary>
/// <param name="measurements">Sensor measurements in range-bearing form.</param>
/// <param name="pose">Vehicle pose that conditions the mapping.</param>
/// <param name="model">Associated map model.</param>
/// <returns>Corrected map model.</returns>
public Map CorrectConditional(List <MeasurementT> measurements,
SimulatedVehicle <MeasurerT, PoseT, MeasurementT> pose,
Map model)
{
Map corrected = new Map(3);
// reduce predicted weight to acocunt for misdetection
// v += (1-PD) v
foreach (Gaussian component in model)
{
corrected.Add(component.Reweight((1 - pose.DetectionProbability(component.Mean)) *
component.Weight));
}
// measurement PHD update
double[][] R = pose.MeasurementCovariance;
double[][] I = Accord.Math.Matrix.Identity(3).ToArray();
double[][] m = new double [model.Count][];
double[][] mp = new double [model.Count][];
double[][][] H = new double [model.Count][][];
double[][][] P = new double [model.Count][][];
double[][][] PH = new double [model.Count][][];
double[][][] S = new double [model.Count][][];
double[] PD = new double [model.Count];
Gaussian[] mc = new Gaussian[model.Count];
var qindex = new Dictionary <Gaussian, int>();
MeasurementT dummyM = new MeasurementT();
int n = 0;
foreach (Gaussian component in model)
{
m [n] = component.Mean;
mp[n] = pose.Measurer.MeasurePerfect(pose.Pose, m[n]).ToLinear();
H [n] = pose.Measurer.MeasurementJacobianL(pose.Pose, m[n]);
P [n] = component.Covariance;
PH[n] = P[n].MultiplyByTranspose(H[n]);
S [n] = H[n].Multiply(PH[n]).Add(R);
mc[n] = new Gaussian(mp[n], S[n], component.Weight);
PD[n] = pose.DetectionProbabilityM(dummyM.FromLinear(mp[n]));
qindex[component] = n;
n++;
}
// for each measurement z and a priori map component,
// v += w' N(x, m', P'), where
// w' = PD w q(z) / (k(z) + sum over components(PD w q(z)))
// m' = m + K (z - h(m))
// P' = [I - K H] P
// K = P H^T (H P H^T + R)^-1
// q(z) = N(z, h(m), H P H^T + R)
// R is the measurement covariance
// H is the measurement linear operator (jacobian) from the model
foreach (MeasurementT measurement in measurements)
{
Map near = model.Near(pose.Measurer.MeasureToMap(pose.Pose, measurement), DensityDistanceThreshold);
double[] mlinear = measurement.ToLinear();
double weightsum = 0;
foreach (Gaussian landmark in near)
{
int i = qindex[landmark];
Gaussian mlandmark = mc[i];
weightsum += PD[i] * mlandmark.Weight * mlandmark.Evaluate(mlinear);
}
foreach (var landmark in near)
{
int i = qindex[landmark];
double[][] gain = PH[i].Multiply(mc[i].CovarianceInverse);
double[] mean = m[i].Add(gain.Multiply(mlinear.Subtract(mp[i])));
double[][] covariance = I.Subtract(gain.Multiply(H[i])).Multiply(P[i]);
double weight = PD[i] * mc[i].Weight * mc[i].Evaluate(mlinear) / (pose.ClutterDensity + weightsum);
corrected.Add(new Gaussian(mean, covariance, weight));
}
}
return(corrected);
}
/// <summary>
/// Calculate the set log-likelihood log(P(Z|X, M)), but do not consider visibility
/// (everything is fully visible). This is used to avoid uninteresting solution to the
/// optimization problem max_X log(P(Z|X, M)). Also gives the pose gradient at the evaluated pose.
/// </summary>
/// <param name="measurements">Sensor measurements in pixel-range form.</param>
/// <param name="map">Map model.</param>
/// <param name="pose">Vehicle pose.</param>
/// <param name="calcgradient">If true, calculate the gradient; otherwise, the gradient param will be null.</param>
/// <param name="gradient">Log-likelihood gradient wrt. the pose.</param>
/// <returns>Set log-likelihood.</returns>
private static double quasiSetLogLikelihood(List <MeasurementT> measurements, IMap map,
SimulatedVehicle <MeasurerT, PoseT, MeasurementT> pose,
bool calcgradient, out double[] gradient)
{
// exact calculation if there are few components/measurements;
// use the most probable components approximation otherwise
SparseMatrix llmatrix = new SparseMatrix(map.Count + measurements.Count, map.Count + measurements.Count, double.NegativeInfinity);
var dlldp = new SparseMatrix <double[]>();
if (calcgradient)
{
dlldp = new SparseMatrix <double[]>(map.Count + measurements.Count, map.Count + measurements.Count, new double[OdoSize]);
}
double logPD = Math.Log(pose.PD);
double log1PD = Math.Log(1 - pose.PD);
double logclutter = Math.Log(pose.ClutterDensity);
Gaussian[] zprobs = new Gaussian[map.Count];
double[][][] zjacobians = (calcgradient) ? new double[map.Count][][] : null;
int n = 0;
foreach (Gaussian landmark in map)
{
MeasurementT m = pose.Measurer.MeasurePerfect(pose.Pose, landmark.Mean);
double[] mlinear = m.ToLinear();
zprobs[n] = new Gaussian(mlinear, pose.MeasurementCovariance, 1);
n++;
}
if (calcgradient)
{
n = 0;
foreach (Gaussian landmark in map)
{
zjacobians[n] = pose.Measurer.MeasurementJacobianP(pose.Pose, landmark.Mean);
n++;
}
}
if (calcgradient)
{
for (int i = 0; i < zprobs.Length; i++)
{
for (int k = 0; k < measurements.Count; k++)
{
double[] m = measurements[k].ToLinear();
double d = zprobs[i].Mahalanobis(m);
if (d < 12)
{
// prob = log (pD * zprob(measurement))
// this way multiplying probabilities equals to adding (negative) profits
llmatrix[i, k] = logPD + Math.Log(zprobs[i].Multiplier) - 0.5 * d * d;
dlldp [i, k] = (m.Subtract(zprobs[i].Mean)).Transpose().
Multiply(zprobs[i].CovarianceInverse).
Multiply(zjacobians[i]).
GetRow(0);
}
}
}
}
else
{
for (int i = 0; i < zprobs.Length; i++)
{
for (int k = 0; k < measurements.Count; k++)
{
double[] m = measurements[k].ToLinear();
double d = zprobs[i].Mahalanobis(m);
if (d < 12)
{
// prob = log (pD * zprob(measurement))
// this way multiplying probabilities equals to adding (negative) profits
llmatrix[i, k] = logPD + Math.Log(zprobs[i].Multiplier) - 0.5 * d * d;
}
}
}
}
for (int i = 0; i < map.Count; i++)
{
llmatrix[i, measurements.Count + i] = log1PD;
}
for (int i = 0; i < measurements.Count; i++)
{
llmatrix[map.Count + i, i] = logclutter;
}
List <SparseMatrix> connectedfull = GraphCombinatorics.ConnectedComponents(llmatrix);
double[] logcomp = new double[200];
double[][] dlogcompdp = (calcgradient) ? new double[200][] : null;
double total = 0;
gradient = (calcgradient) ? new double[OdoSize] : null;
for (int i = 0; i < connectedfull.Count; i++)
{
int[] rows;
int[] cols;
SparseMatrix component = connectedfull[i].Compact(out rows, out cols);
SparseMatrix <double[]> dcomp = (calcgradient) ? dlldp.Submatrix(rows, cols) : null;
// fill the (Misdetection x Clutter) quadrant of the matrix with zeros (don't contribute)
// NOTE this is filled after the connected components partition because
// otherwise everything would be connected through this quadrant
for (int k = 0; k < rows.Length; k++)
{
if (rows[k] >= map.Count)
{
for (int h = 0; h < cols.Length; h++)
{
if (cols[h] >= measurements.Count)
{
component[k, h] = 0;
}
}
}
}
IEnumerable <Tuple <int[], double> > assignments;
bool enumerateall = false;
if (component.Rows.Count <= 5)
{
assignments = GraphCombinatorics.LexicographicalPairing(component, map.Count);
enumerateall = true;
}
else
{
assignments = GraphCombinatorics.MurtyPairing(component);
enumerateall = false;
}
int m = 0;
if (calcgradient)
{
foreach (Tuple <int[], double> assignment in assignments)
{
if (m >= logcomp.Length || (!enumerateall && logcomp[m] - logcomp[0] < -10))
{
break;
}
logcomp [m] = assignment.Item2;
dlogcompdp[m] = new double[OdoSize];
for (int p = 0; p < assignment.Item1.Length; p++)
{
dlogcompdp[m] = dlogcompdp[m].Add(dcomp[p, assignment.Item1[p]]);
}
m++;
}
}
else
{
foreach (Tuple <int[], double> assignment in assignments)
{
if (m >= logcomp.Length || (!enumerateall && logcomp[m] - logcomp[0] < -10))
{
break;
}
logcomp[m] = assignment.Item2;
m++;
}
}
total += logcomp.LogSumExp(0, m);
if (calcgradient)
{
gradient = gradient.Add(dlogcompdp.TemperedAverage(logcomp, 0, m));
}
}
return(total);
}
/// <summary>
/// Calculate the set log-likelihood log(P(Z|X, M)), but do not consider visibility
/// (everything is fully visible). This is used to avoid uninteresting solution to the
/// optimization problem max_X log(P(Z|X, M)). Also gives the pose gradient at the evaluated pose.
/// </summary>
/// <param name="measurements">Sensor measurements in pixel-range form.</param>
/// <param name="map">Map model.</param>
/// <param name="pose">Vehicle pose.</param>
/// <param name="gradient">Log-likelihood gradient wrt. the pose.</param>
/// <returns>Set log-likelihood.</returns>
public static double QuasiSetLogLikelihood(List <MeasurementT> measurements, IMap map,
SimulatedVehicle <MeasurerT, PoseT, MeasurementT> pose,
out double[] gradient)
{
return(quasiSetLogLikelihood(measurements, map, pose, true, out gradient));
}
/// <summary>
/// Calculate the set log-likelihood log(P(Z|X, M)), but do not consider visibility
/// (everything is fully visible). This is used to avoid uninteresting solution to the
/// optimization problem max_X log(P(Z|X, M)).
/// </summary>
/// <param name="measurements">Sensor measurements in pixel-range form.</param>
/// <param name="map">Map model.</param>
/// <param name="pose">Vehicle pose.</param>
/// <returns>Set log-likelihood.</returns>
public static double QuasiSetLogLikelihood(List <MeasurementT> measurements, IMap map,
SimulatedVehicle <MeasurerT, PoseT, MeasurementT> pose)
{
double[] dummy = null;
return(quasiSetLogLikelihood(measurements, map, pose, false, out dummy));
}
/// <summary>
/// Calculate the set log-likelihood log(P(Z|X, M)).
/// </summary>
/// <param name="measurements">Sensor measurements in pixel-range form.</param>
/// <param name="map">Map model.</param>
/// <param name="pose">Vehicle pose.</param>
/// <returns>Set log-likelihood.</returns>
public static double SetLogLikelihood(List <MeasurementT> measurements, IMap map,
SimulatedVehicle <MeasurerT, PoseT, MeasurementT> pose)
{
// exact calculation if there are few components/measurements;
// use the most probable components approximation otherwise
SparseMatrix llmatrix = SetLogLikeMatrix(measurements, map, pose);
List <SparseMatrix> connectedfull = GraphCombinatorics.ConnectedComponents(llmatrix);
double[] logcomp = new double[200];
double total = 0;
for (int i = 0; i < connectedfull.Count; i++)
{
int[] rows;
int[] cols;
SparseMatrix component = connectedfull[i].Compact(out rows, out cols);
// fill the (Misdetection x Clutter) quadrant of the matrix with zeros (don't contribute)
// NOTE this is filled after the connected components partition because
// otherwise everything would be connected through this quadrant
for (int k = 0; k < rows.Length; k++)
{
if (rows[k] >= map.Count)
{
for (int h = 0; h < cols.Length; h++)
{
if (cols[h] >= measurements.Count)
{
component[k, h] = 0;
}
}
}
}
IEnumerable <Tuple <int[], double> > assignments;
bool enumerateall = false;
if (component.Rows.Count <= 5)
{
assignments = GraphCombinatorics.LexicographicalPairing(component, map.Count);
enumerateall = true;
}
else
{
assignments = GraphCombinatorics.MurtyPairing(component);
enumerateall = false;
}
int m = 0;
foreach (Tuple <int[], double> assignment in assignments)
{
if (m >= logcomp.Length || (!enumerateall && logcomp[m] - logcomp[0] < -10))
{
break;
}
logcomp[m] = assignment.Item2;
m++;
}
total += logcomp.LogSumExp(0, m);
}
return(total);
}
/// <summary>
/// Calculate the set likelihood P(Z|X, M).
/// </summary>
/// <param name="measurements">Sensor measurements in pixel-range form.</param>
/// <param name="map">Map model.</param>
/// <param name="pose">Vehicle pose.</param>
/// <returns>Set likelihood.</returns>
public static double SetLikelihood(List <MeasurementT> measurements, IMap map,
SimulatedVehicle <MeasurerT, PoseT, MeasurementT> pose)
{
return(Math.Exp(SetLogLikelihood(measurements, map, pose)));
}
