public void TestRemovingFeedbackReceivers()
{
FieldSize fieldsize = new FieldSize {
Xmin = 0, Xmax = 4, Ymax = 2, Ymin = 0, Zmax = 4, Zmin = 0
};
IParticleGenerator particleGenerator = new RandomParticleGenerator(new ContinuousUniform());
IResampler resampler = new MultinomialResampler();
INoiseGenerator noiseGenerator = new RandomNoiseGenerator(new ContinuousUniform());
ISmoother smoother = new MovingAverageSmoother(200);
ParticleFilter filter = new ParticleFilter(250, 0.1f, fieldsize, particleGenerator, resampler, noiseGenerator, smoother);
Mock <IOrientationFeedbackReceiver> orifeed = new Mock <IOrientationFeedbackReceiver>();
filter.RegisterReceiver(orifeed.Object);
filter.RegisterReceiver(orifeed.Object);
filter.UnregisterReceiver(orifeed.Object);
filter.UnregisterReceiver(orifeed.Object);
Mock <IPositionFeedbackReceiver> posfeed = new Mock <IPositionFeedbackReceiver>();
filter.RegisterReceiver(posfeed.Object);
filter.RegisterReceiver(posfeed.Object);
filter.UnregisterReceiver(posfeed.Object);
filter.UnregisterReceiver(posfeed.Object);
filter.CalculatePose(10);
posfeed.Verify(f => f.NotifyPositionFeedback(It.IsAny <FeedbackData <Vector3> >()), Times.Never);
orifeed.Verify(f => f.NotifyOrientationFeedback(It.IsAny <FeedbackData <Vector3> >()), Times.Never);
}
/// <summary>
/// Initializes a new instance of the <see cref="ParticleFilterCoupler"/> class
/// </summary>
/// <param name="fieldSize">The preferred game field size</param>
public ParticleFilterCoupler(FieldSize fieldSize)
{
int particleamount = 300;
IParticleGenerator prtclgen = new RandomParticleGenerator(new ContinuousUniform());
INoiseGenerator noisegen = new RandomNoiseGenerator(new ContinuousUniform());
IResampler resampler = new MultinomialResampler();
ISmoother smoother = new MovingAverageSmoother(1000);
this.localizer = new ParticleFilter(particleamount, 0.1f, fieldSize, prtclgen, resampler, noisegen, smoother);
}
public void Setup()
{
this.rngsource = new Mock <IContinuousDistribution>();
this.rngsource.SetupGet(foo => foo.Maximum).Returns(1);
this.rngsource.SetupGet(foo => foo.Minimum).Returns(0);
this.rngsource.Setup(foo => foo.Sample()).Returns(this.addedNoise);
this.particleGenerator = new Mock <IParticleGenerator>();
this.particleGenerator.Setup(foo => foo.Generate(It.IsAny <int>(), It.IsAny <float>(), It.IsAny <float>()))
.Returns <int, float, float>((par, min, max) => Enumerable.Repeat(min, par).ToArray());
this.particles = new LinearParticleController(this.particleGenerator.Object, this.particleCount, 0, 1);
this.rng = new RandomNoiseGenerator(this.rngsource.Object);
}
public void TestNoCrashWhenNoData()
{
FieldSize fieldsize = new FieldSize {
Xmin = 0, Xmax = 4, Ymax = 2, Ymin = 0, Zmax = 4, Zmin = 0
};
IParticleGenerator particleGenerator = new RandomParticleGenerator(new ContinuousUniform());
IResampler resampler = new MultinomialResampler();
INoiseGenerator noiseGenerator = new RandomNoiseGenerator(new ContinuousUniform());
ISmoother smoother = new MovingAverageSmoother(200);
ParticleFilter filter = new ParticleFilter(250, 0.1f, fieldsize, particleGenerator, resampler, noiseGenerator, smoother);
for (int i = 0; i < 10; i++)
{
filter.CalculatePose(i);
}
Assert.Pass();
}
public void TestAddingDisplacementSource()
{
FieldSize fieldsize = new FieldSize {
Xmin = 0, Xmax = 4, Ymax = 2, Ymin = 0, Zmax = 4, Zmin = 0
};
IParticleGenerator particleGenerator = new RandomParticleGenerator(new ContinuousUniform());
IResampler resampler = new MultinomialResampler();
INoiseGenerator noiseGenerator = new RandomNoiseGenerator(new ContinuousUniform());
ISmoother smoother = new MovingAverageSmoother(200);
ParticleFilter filter = new ParticleFilter(250, 0.1f, fieldsize, particleGenerator, resampler, noiseGenerator, smoother);
Mock <IDisplacementSource> sourcemock = new Mock <IDisplacementSource>();
sourcemock.SetReturnsDefault <Measurement <Vector3> >(new Measurement <Vector3>(new Vector3(), 0, new Normal(0.1)));
sourcemock.Setup(foo => foo.GetDisplacement(It.IsAny <int>(), It.IsAny <int>())).Returns(() => new Measurement <Vector3>(new Vector3(), 0, new Normal(0.1)));
filter.AddDisplacementSource(sourcemock.Object);
filter.CalculatePose(1);
filter.CalculatePose(2);
sourcemock.Verify(foo => foo.GetDisplacement(1, 2));
}
public void TestRealMarkerSensorData()
{
List <Measurement <Vector3> > oridata = new List <Measurement <Vector3> >();
List <Measurement <Vector3> > posdata = new List <Measurement <Vector3> >();
using (StreamReader sr = new StreamReader(TestContext.CurrentContext.TestDirectory + @"\RealMarkerOriData_Expected_9_10_3.dat"))
{
string line = sr.ReadLine();
while (line != null)
{
string[] unparsed = line.Split(',');
oridata.Add(new Measurement <Vector3>(
VectorMath.Normalize(new Vector3(float.Parse(unparsed[0]), float.Parse(unparsed[1]), float.Parse(unparsed[2]))),
0,
new Normal(0.1)));
line = sr.ReadLine();
}
}
using (StreamReader sr = new StreamReader(TestContext.CurrentContext.TestDirectory + @"\RealMarkerPosData_Expected_060_150_1.dat"))
{
string line = sr.ReadLine();
while (line != null)
{
string[] unparsed = line.Split(',');
posdata.Add(new Measurement <Vector3>(
new Vector3(float.Parse(unparsed[0]), float.Parse(unparsed[1]), float.Parse(unparsed[2])),
0,
new Normal(0.01)));
line = sr.ReadLine();
}
}
FieldSize fieldsize = new FieldSize {
Xmin = 0, Xmax = 4, Ymax = 2, Ymin = 0, Zmax = 4, Zmin = 0
};
IParticleGenerator particleGenerator = new RandomParticleGenerator(new ContinuousUniform());
IResampler resampler = new MultinomialResampler();
INoiseGenerator noiseGenerator = new RandomNoiseGenerator(new ContinuousUniform());
ISmoother smoother = new MovingAverageSmoother(200);
ParticleFilter filter = new ParticleFilter(250, 0.1f, fieldsize, particleGenerator, resampler, noiseGenerator, smoother);
int poscount = 0;
Mock <IPositionSource> possource = new Mock <IPositionSource>();
possource.Setup(foo => foo.GetPositionsClosestTo(It.IsAny <long>(), It.IsAny <long>())).
Returns(() => new List <Measurement <Vector3> > {
posdata[poscount]
}).
Callback(() => poscount++);
filter.AddPositionSource(possource.Object);
int oricount = 0;
Mock <IOrientationSource> orisource = new Mock <IOrientationSource>();
orisource.Setup(foo => foo.GetOrientationClosestTo(It.IsAny <long>(), It.IsAny <long>())).
Returns(() => new List <Measurement <Vector3> > {
oridata[oricount]
}).
Callback(() => oricount++);
filter.AddOrientationSource(orisource.Object);
this.writetofile = false;
StringBuilder res = new StringBuilder();
List <Pose> results = new List <Pose>();
long i = 0;
while ((poscount < posdata.Count / 10) && (oricount < oridata.Count / 10))
{
Pose pose = filter.CalculatePose(i);
if (this.writetofile)
{
res.AppendFormat($"{pose.Position.X},{pose.Position.Y},{pose.Position.Z},{pose.Orientation.X},{pose.Orientation.Y},{pose.Orientation.Z}" + Environment.NewLine);
}
i += 33;
results.Add(pose);
}
if (this.writetofile)
{
File.WriteAllText(TestContext.CurrentContext.TestDirectory + @"\RealMarkerResults.dat", res.ToString());
}
int startindex = 30;
Pose[] resultarray = new Pose[results.Count - startindex];
results.CopyTo(startindex, resultarray, 0, resultarray.Length);
Assert.AreEqual(posdata.GetRange(startindex, resultarray.Length).Select(m => m.Data.X).Average(), resultarray.Select(p => p.Position.X).Average(), 0.1);
Assert.AreEqual(posdata.GetRange(startindex, resultarray.Length).Select(m => m.Data.Y).Average(), resultarray.Select(p => p.Position.Y).Average(), 0.1);
Assert.AreEqual(posdata.GetRange(startindex, resultarray.Length).Select(m => m.Data.Z).Average(), resultarray.Select(p => p.Position.Z).Average(), 0.1);
Assert.AreEqual(oridata.GetRange(startindex, resultarray.Length).Select(m => m.Data.X).Average(), resultarray.Select(p => p.Orientation.X).Average(), 0.1);
Assert.AreEqual(oridata.GetRange(startindex, resultarray.Length).Select(m => m.Data.Y).Average(), resultarray.Select(p => p.Orientation.Y).Average(), 0.1);
Assert.AreEqual(oridata.GetRange(startindex, resultarray.Length).Select(m => m.Data.Z).Average(), resultarray.Select(p => p.Orientation.Z).Average(), 0.1);
}
