Bugfixes: emission oscillation filter and computation of emission pro…

…bability. Change-Id: Id6f2be5cd156445a2d6e20de80ec9a3e4821792c
bmwcarit · Jun 7, 2017 · 104b436 · 104b436
1 parent 7163f9f
commit 104b436
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 12 deletions.
diff --git a/src/main/java/com/bmwcarit/barefoot/matcher/Matcher.java b/src/main/java/com/bmwcarit/barefoot/matcher/Matcher.java
@@ -29,6 +29,7 @@
 
 import com.bmwcarit.barefoot.markov.Filter;
 import com.bmwcarit.barefoot.markov.KState;
+import com.bmwcarit.barefoot.road.Heading;
 import com.bmwcarit.barefoot.roadmap.Distance;
 import com.bmwcarit.barefoot.roadmap.Road;
 import com.bmwcarit.barefoot.roadmap.RoadMap;
@@ -178,23 +179,26 @@ protected Set<Tuple<MatcherCandidate, Double>> candidates(Set<MatcherCandidate>
         for (RoadPoint point : points) {
             map.put(point.edge().id(), point);
         }
-
         for (MatcherCandidate predecessor : predecessors) {
             RoadPoint point = map.get(predecessor.point().edge().id());
-            if (point != null && point.fraction() < predecessor.point().fraction()) {
+            if (point != null && point.edge() != null
+                    && spatial.distance(point.geometry(),
+                            predecessor.point().geometry()) < getSigma()
+                    && ((point.edge().heading() == Heading.forward
+                            && point.fraction() < predecessor.point().fraction())
+                            || (point.edge().heading() == Heading.backward
+                                    && point.fraction() > predecessor.point().fraction()))) {
                 points.remove(point);
                 points.add(predecessor.point());
             }
         }
 
-        Set<Tuple<MatcherCandidate, Double>> candidates =
-                new HashSet<>();
-
+        Set<Tuple<MatcherCandidate, Double>> candidates = new HashSet<>();
         logger.debug("{} ({}) candidates", points.size(), points_.size());
 
         for (RoadPoint point : points) {
             double dz = spatial.distance(sample.point(), point.geometry());
-            double emission = 1 / sqrt_2pi_sig2 * Math.exp((-1) * dz / (2 * sig2));
+            double emission = 1 / sqrt_2pi_sig2 * Math.exp((-1) * dz * dz / (2 * sig2));
             if (!Double.isNaN(sample.azimuth())) {
                 double da = sample.azimuth() > point.azimuth()
                         ? Math.min(sample.azimuth() - point.azimuth(),
@@ -204,7 +208,6 @@ protected Set<Tuple<MatcherCandidate, Double>> candidates(Set<MatcherCandidate>
                 emission *= Math.max(1E-2, 1 / sqrt_2pi_sigA * Math.exp((-1) * da / (2 * sigA)));
             }
 
-
             MatcherCandidate candidate = new MatcherCandidate(point);
             candidates.add(new Tuple<>(candidate, emission));
 
@@ -256,8 +259,7 @@ protected Map<MatcherCandidate, Map<MatcherCandidate, Tuple<MatcherTransition, D
             scheduler.spawn(new Task() {
                 @Override
                 public void run() {
-                    Map<MatcherCandidate, Tuple<MatcherTransition, Double>> map =
-                            new HashMap<>();
+                    Map<MatcherCandidate, Tuple<MatcherTransition, Double>> map = new HashMap<>();
                     Stopwatch sw = new Stopwatch();
                     sw.start();
                     Map<RoadPoint, List<Road>> routes =
@@ -288,8 +290,7 @@ public void run() {
                         double transition = (1 / beta) * Math.exp(
                                 (-1.0) * Math.max(0, route.cost(new TimePriority()) - base) / beta);
 
-                        map.put(candidate, new Tuple<>(
-                                new MatcherTransition(route), transition));
+                        map.put(candidate, new Tuple<>(new MatcherTransition(route), transition));
 
                         logger.trace("{} -> {} {} {} {}", predecessor.id(), candidate.id(), base,
                                 route.length(), transition);

diff --git a/src/test/java/com/bmwcarit/barefoot/matcher/MatcherTest.java b/src/test/java/com/bmwcarit/barefoot/matcher/MatcherTest.java
@@ -123,7 +123,7 @@ private void assertCandidate(Tuple<MatcherCandidate, Double> candidate, Point sa
         double l = spatial.distance(i, sample);
         double sig2 = Math.pow(5d, 2);
         double sqrt_2pi_sig2 = Math.sqrt(2d * Math.PI * sig2);
-        double p = 1 / sqrt_2pi_sig2 * Math.exp((-1) * l / (2 * sig2));
+        double p = 1 / sqrt_2pi_sig2 * Math.exp((-1) * l * l / (2 * sig2));
 
         assertEquals(f, candidate.one().point().fraction(), 10E-6);
         assertEquals(p, candidate.two(), 10E-6);