public override Vector MapToVector(params IKEPDist[] msgs)
{
var q = vectorMappers.Select(map => map.MapToVector(msgs));
Vector[] mapped = q.ToArray();
return(MatrixUtils.ConcatAll(mapped));
}
public override Vector MapToVector(params IKEPDist[] msgs)
{
Vector[] outs = mappers.Select(map => map.MapToVector(msgs)).ToArray();
Vector stack = MatrixUtils.ConcatAll(outs);
return(stack);
}
public static DVectorNormal ToJointGaussian(params IKEPDist[] msgs)
{
// Stack all messages to form a big Gaussian distribution
Vector[] means = msgs.Select(dist => dist.GetMeanVector()).ToArray();
Matrix[] covs = msgs.Select(dist => dist.GetCovarianceMatrix()).ToArray();
Vector M = MatrixUtils.ConcatAll(means);
Matrix V = MatrixUtils.BlkDiag(covs);
DVectorNormal joint = new DVectorNormal(M, V);
return(joint);
}
public override Vector MapToVector(params IKEPDist[] msgs)
{
Vector[] features = new Vector[maps.Length];
for (int i = 0; i < maps.Length; i++)
{
features[i] = maps[i].MapToVector(msgs);
}
Vector all = MatrixUtils.ConcatAll(features);
return(all);
}
public override void MapAndEstimateU(out Vector mapped,
out double[] uncertainty, out bool uncertain, params IKEPDist[] dists)
{
// ** Take only the fist uncertainty estimate from each mapper.
int m = mappers.Length;
uncertainty = new double[m];
Vector[] outs = new Vector[m];
bool[] uncertains = new bool[m];
for (int i = 0; i < m; i++)
{
double[] ui;
Vector outi;
mappers[i].MapAndEstimateU(out outi, out ui, out uncertains[i], dists);
outs[i] = outi;
uncertainty[i] = ui[0];
}
mapped = MatrixUtils.ConcatAll(outs);
uncertain = MatrixUtils.Or(uncertains);
}