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);
}
private void init()
{
var path = Path.Combine(Directory.GetParent(Environment.CurrentDirectory).FullName, "Resources", "PrototypeTemplatesBW");
var generatedScales = EnumerableExtensions.GetRange(60, 170, 3);
var generatedOrientations = EnumerableExtensions.GetRange(-90, +90, (int)((180f / GlobalParameters.NUM_OF_QUNATIZED_ORIENTATIONS) / 2 / 2 /*user factor - last "2"*/));
Console.WriteLine("Generating templates.... Please wait!");
var templates = OpenHandTemplate.CreateRange(path, "*.bmp", generatedScales, generatedOrientations);
ModelRepository.Initialize(templates);
//drawTemplatesToFiles(templates, "C:/generatedTemplates");
Console.WriteLine("Initializing particle filter!");
particleFilter = ParticleFilter.UnifromParticleSpreadInitializer(
NUMBER_OF_PARTICLES,
new DoubleRange[]
{
//template type
new DoubleRange(0, ModelRepository.PrototypeCount - 1),
//scale
new DoubleRange(70, 150),
//rotation
new DoubleRange(-15, 15)
},
ModelParticle.FromParameters).ToList();
initialParticles = particleFilter.Select(x => (ModelParticle)x.Clone()).ToList();
resetClock = new Stopwatch(); resetClock.Start();
}
/// <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);
}
private void update()
{
particleFilter = Enumerable.ToList(particleFilter.Update
(
//measure
p => updateParticleWeight(p),
//normalize weights
particles => ParticleFilter.SimpleNormalizer(particles),
//resample (if necessary)
(particles, normalizedWeights) => ParticleFilter.SimpleResampler(particles.ToList(), normalizedWeights.ToList())
));
}
private void init()
{
particleFilter = ParticleFilter.UnifromParticleSpreadInitializer <ColorParticle>
(
//particles' count
1000,
//position range
new DoubleRange[]
{
new DoubleRange(0, imgSize.Width),
new DoubleRange(0, imgSize.Height)
},
//convert arr => position (create from array)
ColorParticle.FromArray
)
.ToList();
}
private void update()
{
particleFilter = particleFilter.Update
(
//measure
particles => measure(linPyr, particles.ToList()),
//normalize
particles => ParticleFilter.SimpleNormalizer(particles),
//re-sample
(particles, normalizedWeights) =>
{
var sampledParticles = ParticleFilter.SimpleResampler(particles.ToList(),
normalizedWeights.ToList()); //particles' weight are all equal after re-sampling
return(sampledParticles);
}
).ToList();
}
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));
}
private void OnLoaded(object sender, RoutedEventArgs e)
{
World = new Map(129, 129);
World.AddSquare(51, 1, 26, 33, 0);
World.AddSquare(92, 79, 36, 14, 0);
//World.AddSquare(133, 96, 16, 24, 0);
//World.AddSquare(160, 27, 36, 14, 0);
//filter = new ParticleFilter(N, World, 108, 57, 90);
filter = new ParticleFilter(N, World);
int fromX = 0, fromY = 0, toX = 19, toY = 19;
var roadMap = World.GetAStarRoadMap(fromX, fromY, toX, toY);
Astar = new Astar(roadMap, 25, 20, 15);
OriginalBitmap = new Bitmap(130, 130);
Image.Source = OriginalBitmap.DrawObjects(roadMap);
BitmapClone = (Bitmap)OriginalBitmap.Clone();
Image.Source = BitmapClone.Drawparticles(filter.ParticleSet);
}
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);
}
