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); }