diff --git a/CONTRIBUTORS.md b/CONTRIBUTORS.md index e9312a35..84980bf5 100644 --- a/CONTRIBUTORS.md +++ b/CONTRIBUTORS.md @@ -4,7 +4,8 @@ * karussell, Peter Karich, GraphHopper GmbH, initial version * michaz, very important hidden markov improvement via hmm-lib, see #49 * rory, support milisecond gpx timestamps, see #4 - * stefanholder, Stefan Holder, BMW AG, creating and integrating the hmm-lib (!), see #49, #66 and #69 - * kodonnell, adding support for CH and other algorithms as per #60 + * stefanholder, Stefan Holder, BMW AG, creating and integrating the hmm-lib (#49, #66, #69) and + penalizing inner-link U-turns (#70) + * kodonnell, adding support for CH and other algorithms (#60) and penalizing inner-link U-turns (#70) For GraphHopper contributors see [here](https://github.com/graphhopper/graphhopper/blob/master/CONTRIBUTORS.md). diff --git a/map-data/issue-70.osm.gz b/map-data/issue-70.osm.gz new file mode 100644 index 00000000..88b1a3d5 Binary files /dev/null and b/map-data/issue-70.osm.gz differ diff --git a/matching-core/src/main/java/com/graphhopper/matching/EdgeMatch.java b/matching-core/src/main/java/com/graphhopper/matching/EdgeMatch.java index 5100a611..b477e4a2 100644 --- a/matching-core/src/main/java/com/graphhopper/matching/EdgeMatch.java +++ b/matching-core/src/main/java/com/graphhopper/matching/EdgeMatch.java @@ -61,8 +61,8 @@ public double getMinDistance() { double min = Double.MAX_VALUE; for (GPXExtension gpxExt : gpxExtensions) { - if (gpxExt.queryResult.getQueryDistance() < min) { - min = gpxExt.queryResult.getQueryDistance(); + if (gpxExt.getQueryResult().getQueryDistance() < min) { + min = gpxExt.getQueryResult().getQueryDistance(); } } return min; diff --git a/matching-core/src/main/java/com/graphhopper/matching/GPXExtension.java b/matching-core/src/main/java/com/graphhopper/matching/GPXExtension.java index 1c357c8e..8f354797 100644 --- a/matching-core/src/main/java/com/graphhopper/matching/GPXExtension.java +++ b/matching-core/src/main/java/com/graphhopper/matching/GPXExtension.java @@ -17,36 +17,108 @@ */ package com.graphhopper.matching; +import com.graphhopper.routing.VirtualEdgeIteratorState; import com.graphhopper.storage.index.QueryResult; +import com.graphhopper.util.EdgeIteratorState; import com.graphhopper.util.GPXEntry; /** + * During map matching this represents a map matching candidate, i.e. a potential snapped + * point of a GPX entry. After map matching, this represents the map matched point of + * an GPX entry. + *

+ * A GPXEntry can either be at an undirected real (tower) node or at a directed virtual node. + * If this is at a directed virtual node then incoming paths from any previous GPXExtension + * should arrive through {@link #getIncomingVirtualEdge()} and outgoing paths to any following + * GPXExtension should start with {@link #getOutgoingVirtualEdge()}. This is achieved by + * penalizing other edges for routing. Note that virtual nodes are always connected to their + * adjacent nodes via 2 virtual edges (not counting reverse virtual edges). * * @author Peter Karich + * @author kodonnell + * @author Stefan Holder */ public class GPXExtension { - final GPXEntry entry; - final QueryResult queryResult; - final int gpxListIndex; + private final GPXEntry entry; + private final QueryResult queryResult; + private final boolean isDirected; + private final EdgeIteratorState incomingVirtualEdge; + private final EdgeIteratorState outgoingVirtualEdge; - public GPXExtension(GPXEntry entry, QueryResult queryResult, int gpxListIndex) { + /** + * Creates an undirected candidate for a real node. + */ + public GPXExtension(GPXEntry entry, QueryResult queryResult) { this.entry = entry; this.queryResult = queryResult; - this.gpxListIndex = gpxListIndex; + this.isDirected = false; + this.incomingVirtualEdge = null; + this.outgoingVirtualEdge = null; } - @Override - public String toString() { - return "entry:" + entry - + ", query distance:" + queryResult.getQueryDistance() - + ", gpxListIndex:" + gpxListIndex; + /** + * Creates a directed candidate for a virtual node. + */ + public GPXExtension(GPXEntry entry, QueryResult queryResult, + VirtualEdgeIteratorState incomingVirtualEdge, + VirtualEdgeIteratorState outgoingVirtualEdge) { + this.entry = entry; + this.queryResult = queryResult; + this.isDirected = true; + this.incomingVirtualEdge = incomingVirtualEdge; + this.outgoingVirtualEdge = outgoingVirtualEdge; + } + + public GPXEntry getEntry() { + return entry; } public QueryResult getQueryResult() { - return this.queryResult; + return queryResult; } - public GPXEntry getEntry() { - return entry; + /** + * Returns whether this GPXExtension is directed. This is true if the snapped point + * is a virtual node, otherwise the snapped node is a real (tower) node and false is returned. + */ + public boolean isDirected() { + return isDirected; + } + + /** + * Returns the virtual edge that should be used by incoming paths. + * + * @throws IllegalStateException if this GPXExtension is not directed. + */ + public EdgeIteratorState getIncomingVirtualEdge() { + if (!isDirected) { + throw new IllegalStateException( + "This method may only be called for directed GPXExtensions"); + } + return incomingVirtualEdge; + } + + /** + * Returns the virtual edge that should be used by outgoing paths. + * + * @throws IllegalStateException if this GPXExtension is not directed. + */ + public EdgeIteratorState getOutgoingVirtualEdge() { + if (!isDirected) { + throw new IllegalStateException( + "This method may only be called for directed GPXExtensions"); + } + return outgoingVirtualEdge; + } + + @Override + public String toString() { + return "GPXExtension{" + + "closest node=" + queryResult.getClosestNode() + + " at " + queryResult.getSnappedPoint().getLat() + "," + + queryResult.getSnappedPoint().getLon() + + ", incomingEdge=" + incomingVirtualEdge + + ", outgoingEdge=" + outgoingVirtualEdge + + '}'; } -} +} \ No newline at end of file diff --git a/matching-core/src/main/java/com/graphhopper/matching/MapMatching.java b/matching-core/src/main/java/com/graphhopper/matching/MapMatching.java index 08387cbc..f2b53a2f 100644 --- a/matching-core/src/main/java/com/graphhopper/matching/MapMatching.java +++ b/matching-core/src/main/java/com/graphhopper/matching/MapMatching.java @@ -17,32 +17,29 @@ */ package com.graphhopper.matching; +import com.bmw.hmm.SequenceState; +import com.bmw.hmm.ViterbiAlgorithm; import com.graphhopper.GraphHopper; import com.graphhopper.matching.util.HmmProbabilities; import com.graphhopper.matching.util.TimeStep; -import com.graphhopper.routing.weighting.Weighting; -import com.bmw.hmm.SequenceState; -import com.bmw.hmm.ViterbiAlgorithm; -import com.graphhopper.routing.AlgorithmOptions; -import com.graphhopper.routing.Path; -import com.graphhopper.routing.QueryGraph; -import com.graphhopper.routing.RoutingAlgorithm; -import com.graphhopper.routing.RoutingAlgorithmFactory; +import com.graphhopper.routing.*; import com.graphhopper.routing.ch.CHAlgoFactoryDecorator; import com.graphhopper.routing.ch.PrepareContractionHierarchies; -import com.graphhopper.routing.util.*; +import com.graphhopper.routing.util.DefaultEdgeFilter; +import com.graphhopper.routing.util.EdgeFilter; +import com.graphhopper.routing.util.HintsMap; import com.graphhopper.routing.weighting.FastestWeighting; +import com.graphhopper.routing.weighting.Weighting; import com.graphhopper.storage.CHGraph; import com.graphhopper.storage.Graph; import com.graphhopper.storage.index.LocationIndexTree; import com.graphhopper.storage.index.QueryResult; import com.graphhopper.util.*; import com.graphhopper.util.shapes.GHPoint; -import java.util.ArrayList; -import java.util.Collection; -import java.util.HashMap; -import java.util.List; -import java.util.Map; +import org.slf4j.Logger; +import org.slf4j.LoggerFactory; + +import java.util.*; import java.util.Map.Entry; /** @@ -65,16 +62,28 @@ */ public class MapMatching { + private final Logger logger = LoggerFactory.getLogger(getClass()); + + // Penalty in m for each U-turn performed at the beginning or end of a path between two + // subsequent candidates. + private double uTurnDistancePenalty; + private final Graph routingGraph; private final LocationIndexMatch locationIndex; private double measurementErrorSigma = 50.0; - private double transitionProbabilityBeta = 0.00959442; + private double transitionProbabilityBeta = 2.0; private final int nodeCount; private DistanceCalc distanceCalc = new DistancePlaneProjection(); private final RoutingAlgorithmFactory algoFactory; private final AlgorithmOptions algoOptions; public MapMatching(GraphHopper hopper, AlgorithmOptions algoOptions) { + // Convert heading penalty [s] into U-turn penalty [m] + final double PENALTY_CONVERSION_VELOCITY = 5; // [m/s] + final double headingTimePenalty = algoOptions.getHints().getDouble( + Parameters.Routing.HEADING_PENALTY, Parameters.Routing.DEFAULT_HEADING_PENALTY); + uTurnDistancePenalty = headingTimePenalty * PENALTY_CONVERSION_VELOCITY; + this.locationIndex = new LocationIndexMatch(hopper.getGraphHopperStorage(), (LocationIndexTree) hopper.getLocationIndex()); @@ -113,7 +122,8 @@ public MapMatching(GraphHopper hopper, AlgorithmOptions algoOptions) { boolean forceFlexibleMode = hints.getBool(Parameters.CH.DISABLE, false); if (chFactoryDecorator.isEnabled() && !forceFlexibleMode) { if (!(algoFactory instanceof PrepareContractionHierarchies)) { - throw new IllegalStateException("Although CH was enabled a non-CH algorithm factory was returned " + algoFactory); + throw new IllegalStateException("Although CH was enabled a non-CH algorithm " + + "factory was returned " + algoFactory); } weighting = ((PrepareContractionHierarchies) algoFactory).getWeighting(); @@ -121,7 +131,8 @@ public MapMatching(GraphHopper hopper, AlgorithmOptions algoOptions) { } else { weighting = algoOptions.hasWeighting() ? algoOptions.getWeighting() - : new FastestWeighting(hopper.getEncodingManager().getEncoder(vehicle), algoOptions.getHints()); + : new FastestWeighting(hopper.getEncodingManager().getEncoder(vehicle), + algoOptions.getHints()); this.routingGraph = hopper.getGraphHopperStorage(); } @@ -161,84 +172,214 @@ public MatchResult doWork(List gpxList) { + gpxList.size() + "). Correct format?"); } + // filter the entries: + List filteredGPXEntries = filterGPXEntries(gpxList); + if (filteredGPXEntries.size() < 2) { + throw new IllegalStateException("Only " + filteredGPXEntries.size() + + " filtered GPX entries (from " + gpxList.size() + + "), but two or more are needed"); + } + + // now find each of the entries in the graph: final EdgeFilter edgeFilter = new DefaultEdgeFilter(algoOptions.getWeighting().getFlagEncoder()); - // Compute all candidates first. - // TODO: Generate candidates on-the-fly within computeViterbiSequence() if this does not - // degrade performance. - final List allCandidates = new ArrayList<>(); - List> timeSteps = createTimeSteps(gpxList, - edgeFilter, allCandidates); + List> queriesPerEntry = lookupGPXEntries(filteredGPXEntries, edgeFilter); - if (allCandidates.size() < 2) { - throw new IllegalArgumentException("Too few matching coordinates (" - + allCandidates.size() + "). Wrong region imported?"); - } - if (timeSteps.size() < 2) { - throw new IllegalStateException("Coordinates produced too few time steps " - + timeSteps.size() + ", gpxList:" + gpxList.size()); + // now look up the entries up in the graph: + final QueryGraph queryGraph = new QueryGraph(routingGraph).setUseEdgeExplorerCache(true); + List allQueryResults = new ArrayList<>(); + for (List qrs: queriesPerEntry) + allQueryResults.addAll(qrs); + queryGraph.lookup(allQueryResults); + + logger.debug("================= Query results ================="); + int i = 1; + for (List entries : queriesPerEntry) { + logger.debug("Query results for GPX entry {}", i++); + for (QueryResult qr : entries) { + logger.debug("Node id: {}, virtual: {}, snapped on: {}, pos: {},{}, " + + "query distance: {}", qr.getClosestNode(), + isVirtualNode(qr.getClosestNode()), qr.getSnappedPosition(), + qr.getSnappedPoint().getLat(), qr.getSnappedPoint().getLon(), + qr.getQueryDistance()); + } } - final QueryGraph queryGraph = new QueryGraph(routingGraph).setUseEdgeExplorerCache(true); - queryGraph.lookup(allCandidates); + // Creates candidates from the QueryResults of all GPX entries (a candidate is basically a + // QueryResult + direction). + List> timeSteps = + createTimeSteps(filteredGPXEntries, queriesPerEntry, queryGraph); + logger.debug("=============== Time steps ==============="); + i = 1; + for (TimeStep ts : timeSteps) { + logger.debug("Candidates for time step {}", i++); + for (GPXExtension candidate : ts.candidates) { + logger.debug(candidate.toString()); + } + } + // Compute the most likely sequence of map matching candidates: List> seq = computeViterbiSequence(timeSteps, - gpxList, queryGraph); + gpxList.size(), queryGraph); + + logger.debug("=============== Viterbi results =============== "); + i = 1; + for (SequenceState ss : seq) { + logger.debug("{}: {}, path: {}", i, ss.state, + ss.transitionDescriptor != null ? ss.transitionDescriptor.calcEdges() : null); + i++; + } + // finally, extract the result: final EdgeExplorer explorer = queryGraph.createEdgeExplorer(edgeFilter); - MatchResult matchResult = computeMatchResult(seq, gpxList, allCandidates, explorer); + + // Needs original gpxList to compute stats. + MatchResult matchResult = computeMatchResult(seq, gpxList, queriesPerEntry, explorer); + + logger.debug("=============== Matched real edges =============== "); + i = 1; + for (EdgeMatch em : matchResult.getEdgeMatches()) { + logger.debug("{}: {}", i, em.getEdgeState()); + i++; + } return matchResult; } + + /** + * Filters GPX entries to only those which will be used for map matching (i.e. those which + * are separated by at least 2 * measurementErrorSigman + */ + private List filterGPXEntries(List gpxList) { + List filtered = new ArrayList<>(); + GPXEntry prevEntry = null; + int last = gpxList.size() - 1; + for (int i = 0; i <= last; i++) { + GPXEntry gpxEntry = gpxList.get(i); + if (i == 0 || i == last || distanceCalc.calcDist( + prevEntry.getLat(), prevEntry.getLon(), + gpxEntry.getLat(), gpxEntry.getLon()) > 2 * measurementErrorSigma) { + filtered.add(gpxEntry); + prevEntry = gpxEntry; + } else { + logger.debug("Filter out GPX entry: {}", i + 1); + } + } + return filtered; + } /** - * Creates TimeSteps for the GPX entries but does not create emission or - * transition probabilities. - * - * @param outAllCandidates output parameter for all candidates, must be an - * empty list. + * Find the possible locations of each qpxEntry in the graph. */ - private List> createTimeSteps(List gpxList, - EdgeFilter edgeFilter, List outAllCandidates) { - int indexGPX = 0; - TimeStep prevTimeStep = null; - final List> timeSteps = new ArrayList<>(); + private List> lookupGPXEntries(List gpxList, + EdgeFilter edgeFilter) { + List> gpxEntryLocations = new ArrayList<>(); for (GPXEntry gpxEntry : gpxList) { - if (prevTimeStep == null - || distanceCalc.calcDist( - prevTimeStep.observation.getLat(), prevTimeStep.observation.getLon(), - gpxEntry.getLat(), gpxEntry.getLon()) > 2 * measurementErrorSigma - // always include last point - || indexGPX == gpxList.size() - 1) { - final List queryResults = locationIndex.findNClosest( - gpxEntry.lat, gpxEntry.lon, - edgeFilter, measurementErrorSigma); - outAllCandidates.addAll(queryResults); - final List candidates = new ArrayList<>(); - for (QueryResult candidate : queryResults) { - candidates.add(new GPXExtension(gpxEntry, candidate, indexGPX)); + gpxEntryLocations.add(locationIndex.findNClosest(gpxEntry.lat, gpxEntry.lon, edgeFilter, + measurementErrorSigma)); + } + return gpxEntryLocations; + } + + /** + * Creates TimeSteps with candidates for the GPX entries but does not create emission or + * transition probabilities. Creates directed candidates for virtual nodes and undirected + * candidates for real nodes. + */ + private List> createTimeSteps( + List filteredGPXEntries, List> queriesPerEntry, + QueryGraph queryGraph) { + final int n = filteredGPXEntries.size(); + if (queriesPerEntry.size() != n) { + throw new IllegalArgumentException( + "filteredGPXEntries and queriesPerEntry must have same size."); + } + + final List> timeSteps = new ArrayList<>(); + for (int i = 0; i < n; i++) { + + GPXEntry gpxEntry = filteredGPXEntries.get(i); + List queryResults = queriesPerEntry.get(i); + + List candidates = new ArrayList<>(); + for (QueryResult qr: queryResults) { + int closestNode = qr.getClosestNode(); + if (queryGraph.isVirtualNode(closestNode)) { + // get virtual edges: + List virtualEdges = new ArrayList<>(); + EdgeIterator iter = queryGraph.createEdgeExplorer().setBaseNode(closestNode); + while (iter.next()) { + if (!queryGraph.isVirtualEdge(iter.getEdge())) { + throw new RuntimeException("Virtual nodes must only have virtual edges " + + "to adjacent nodes."); + } + virtualEdges.add((VirtualEdgeIteratorState) + queryGraph.getEdgeIteratorState(iter.getEdge(), iter.getAdjNode())); + } + if( virtualEdges.size() != 2) { + throw new RuntimeException("Each virtual node must have exactly 2 " + + "virtual edges (reverse virtual edges are not returned by the " + + "EdgeIterator"); + } + + // Create a directed candidate for each of the two possible directions through + // the virtual node. This is needed to penalize U-turns at virtual nodes + // (see also #51). We need to add candidates for both directions because + // we don't know yet which is the correct one. This will be figured + // out by the Viterbi algorithm. + // + // Adding further candidates to explicitly allow U-turns through setting + // incomingVirtualEdge==outgoingVirtualEdge doesn't make sense because this + // would actually allow to perform a U-turn without a penalty by going to and + // from the virtual node through the other virtual edge or its reverse edge. + VirtualEdgeIteratorState e1 = virtualEdges.get(0); + VirtualEdgeIteratorState e2 = virtualEdges.get(1); + for (int j = 0; j < 2; j++) { + // get favored/unfavored edges: + VirtualEdgeIteratorState incomingVirtualEdge = j == 0 ? e1 : e2; + VirtualEdgeIteratorState outgoingVirtualEdge = j == 0 ? e2 : e1; + // create candidate + QueryResult vqr = new QueryResult(qr.getQueryPoint().lat, qr.getQueryPoint().lon); + vqr.setQueryDistance(qr.getQueryDistance()); + vqr.setClosestNode(qr.getClosestNode()); + vqr.setWayIndex(qr.getWayIndex()); + vqr.setSnappedPosition(qr.getSnappedPosition()); + vqr.setClosestEdge(qr.getClosestEdge()); + vqr.calcSnappedPoint(distanceCalc); + GPXExtension candidate = new GPXExtension(gpxEntry, vqr, incomingVirtualEdge, + outgoingVirtualEdge); + candidates.add(candidate); + } + } else { + // Create an undirected candidate for the real node. + GPXExtension candidate = new GPXExtension(gpxEntry, qr); + candidates.add(candidate); } - final TimeStep timeStep - = new TimeStep<>(gpxEntry, candidates); - timeSteps.add(timeStep); - prevTimeStep = timeStep; } - indexGPX++; + + final TimeStep timeStep = new TimeStep<>(gpxEntry, candidates); + timeSteps.add(timeStep); } return timeSteps; } + /** + * Computes the most likely candidate sequence for the GPX entries. + */ private List> computeViterbiSequence( - List> timeSteps, List gpxList, - final QueryGraph queryGraph) { + List> timeSteps, int originalGpxEntriesCount, + QueryGraph queryGraph) { final HmmProbabilities probabilities = new HmmProbabilities(measurementErrorSigma, transitionProbabilityBeta); final ViterbiAlgorithm viterbi = new ViterbiAlgorithm<>(); + logger.debug("\n=============== Paths ==============="); int timeStepCounter = 0; TimeStep prevTimeStep = null; + int i = 1; for (TimeStep timeStep : timeSteps) { + logger.debug("\nPaths to time step {}", i++); computeEmissionProbabilities(timeStep, probabilities); if (prevTimeStep == null) { @@ -264,9 +405,10 @@ private List> computeViterbiSequence } throw new RuntimeException("Sequence is broken for submitted track at time step " - + timeStepCounter + " (" + gpxList.size() + " points). " + likelyReasonStr - + "observation:" + timeStep.observation + ", " - + timeStep.candidates.size() + " candidates: " + getSnappedCandidates(timeStep.candidates) + + timeStepCounter + " (" + originalGpxEntriesCount + " points). " + + likelyReasonStr + "observation:" + timeStep.observation + ", " + + timeStep.candidates.size() + " candidates: " + + getSnappedCandidates(timeStep.candidates) + ". If a match is expected consider increasing max_visited_nodes."); } @@ -297,33 +439,83 @@ private void computeTransitionProbabilities(TimeStep> seq, - List gpxList, List allCandidates, - EdgeExplorer explorer) { - // every virtual edge maps to its real edge where the orientation is already correct! - // TODO use traversal key instead of string! - final Map virtualEdgesMap = new HashMap<>(); - for (QueryResult candidate : allCandidates) { - fillVirtualEdges(virtualEdgesMap, explorer, candidate); + /** + * Returns the path length plus a penalty if the starting/ending edge is unfavored. + */ + private double penalizedPathDistance(Path path, + Set penalizedVirtualEdges) { + double totalPenalty = 0; + + // Unfavored edges in the middle of the path should not be penalized because we are + // only concerned about the direction at the start/end. + final List edges = path.calcEdges(); + if (!edges.isEmpty()) { + if (penalizedVirtualEdges.contains(edges.get(0))) { + totalPenalty += uTurnDistancePenalty; + } } + if (edges.size() > 1) { + if (penalizedVirtualEdges.contains(edges.get(edges.size() - 1))) { + totalPenalty += uTurnDistancePenalty; + } + } + return path.getDistance() + totalPenalty; + } + private MatchResult computeMatchResult(List> seq, + List gpxList, + List> queriesPerEntry, + EdgeExplorer explorer) { + final Map virtualEdgesMap = createVirtualEdgesMap( + queriesPerEntry, explorer); MatchResult matchResult = computeMatchedEdges(seq, virtualEdgesMap); computeGpxStats(gpxList, matchResult); @@ -416,28 +608,36 @@ private boolean isVirtualNode(int node) { } /** - * Fills the minFactorMap with weights for the virtual edges. + * Returns a map where every virtual edge maps to its real edge with correct orientation. */ - private void fillVirtualEdges(Map virtualEdgesMap, - EdgeExplorer explorer, QueryResult qr) { - if (isVirtualNode(qr.getClosestNode())) { - EdgeIterator iter = explorer.setBaseNode(qr.getClosestNode()); - while (iter.next()) { - int node = traverseToClosestRealAdj(explorer, iter); - if (node == qr.getClosestEdge().getAdjNode()) { - virtualEdgesMap.put(virtualEdgesMapKey(iter), - qr.getClosestEdge().detach(false)); - virtualEdgesMap.put(reverseVirtualEdgesMapKey(iter), - qr.getClosestEdge().detach(true)); - } else if (node == qr.getClosestEdge().getBaseNode()) { - virtualEdgesMap.put(virtualEdgesMapKey(iter), qr.getClosestEdge().detach(true)); - virtualEdgesMap.put(reverseVirtualEdgesMapKey(iter), - qr.getClosestEdge().detach(false)); - } else { - throw new RuntimeException(); + private Map createVirtualEdgesMap( + List> queriesPerEntry, EdgeExplorer explorer) { + // TODO For map key, use the traversal key instead of string! + Map virtualEdgesMap = new HashMap<>(); + for (List queryResults: queriesPerEntry) { + for (QueryResult qr: queryResults) { + if (isVirtualNode(qr.getClosestNode())) { + EdgeIterator iter = explorer.setBaseNode(qr.getClosestNode()); + while (iter.next()) { + int node = traverseToClosestRealAdj(explorer, iter); + if (node == qr.getClosestEdge().getAdjNode()) { + virtualEdgesMap.put(virtualEdgesMapKey(iter), + qr.getClosestEdge().detach(false)); + virtualEdgesMap.put(reverseVirtualEdgesMapKey(iter), + qr.getClosestEdge().detach(true)); + } else if (node == qr.getClosestEdge().getBaseNode()) { + virtualEdgesMap.put(virtualEdgesMapKey(iter), + qr.getClosestEdge().detach(true)); + virtualEdgesMap.put(reverseVirtualEdgesMapKey(iter), + qr.getClosestEdge().detach(false)); + } else { + throw new RuntimeException(); + } + } } } } + return virtualEdgesMap; } private String virtualEdgesMapKey(EdgeIteratorState iter) { @@ -468,8 +668,8 @@ private String getSnappedCandidates(Collection candidates) { if (!str.isEmpty()) { str += ", "; } - str += "distance: " + gpxe.queryResult.getQueryDistance() + " to " - + gpxe.queryResult.getSnappedPoint(); + str += "distance: " + gpxe.getQueryResult().getQueryDistance() + " to " + + gpxe.getQueryResult().getSnappedPoint(); } return "[" + str + "]"; } @@ -485,15 +685,15 @@ private void printMinDistances(List> time double minCand = Double.POSITIVE_INFINITY; for (GPXExtension prevGPXE : prevStep.candidates) { for (GPXExtension gpxe : ts.candidates) { - GHPoint psp = prevGPXE.queryResult.getSnappedPoint(); - GHPoint sp = gpxe.queryResult.getSnappedPoint(); + GHPoint psp = prevGPXE.getQueryResult().getSnappedPoint(); + GHPoint sp = gpxe.getQueryResult().getSnappedPoint(); double tmpDist = distanceCalc.calcDist(psp.lat, psp.lon, sp.lat, sp.lon); if (tmpDist < minCand) { minCand = tmpDist; } } } - System.out.println(index + ": " + Math.round(dist) + "m, minimum candidate: " + logger.debug(index + ": " + Math.round(dist) + "m, minimum candidate: " + Math.round(minCand) + "m"); index++; } @@ -502,10 +702,9 @@ private void printMinDistances(List> time } } - // TODO: Make setFromNode and processEdge public in Path and then remove this. - private static class MyPath extends Path { + private static class MapMatchedPath extends Path { - public MyPath(Graph graph, Weighting weighting) { + public MapMatchedPath(Graph graph, Weighting weighting) { super(graph, weighting); } @@ -521,7 +720,7 @@ public void processEdge(int edgeId, int adjNode, int prevEdgeId) { } public Path calcPath(MatchResult mr) { - MyPath p = new MyPath(routingGraph, algoOptions.getWeighting()); + MapMatchedPath p = new MapMatchedPath(routingGraph, algoOptions.getWeighting()); if (!mr.getEdgeMatches().isEmpty()) { int prevEdge = EdgeIterator.NO_EDGE; p.setFromNode(mr.getEdgeMatches().get(0).getEdgeState().getBaseNode()); @@ -530,7 +729,6 @@ public Path calcPath(MatchResult mr) { prevEdge = em.getEdgeState().getEdge(); } - // TODO p.setWeight(weight); p.setFound(true); return p; @@ -538,4 +736,4 @@ public Path calcPath(MatchResult mr) { return p; } } -} +} \ No newline at end of file diff --git a/matching-core/src/main/java/com/graphhopper/matching/MapMatchingMain.java b/matching-core/src/main/java/com/graphhopper/matching/MapMatchingMain.java index 7f17fda1..49479090 100644 --- a/matching-core/src/main/java/com/graphhopper/matching/MapMatchingMain.java +++ b/matching-core/src/main/java/com/graphhopper/matching/MapMatchingMain.java @@ -89,7 +89,8 @@ private void start(CmdArgs args) { hints(new HintsMap().put("weighting", "fastest").put("vehicle", firstEncoder.toString())). build(); MapMatching mapMatching = new MapMatching(hopper, opts); - mapMatching.setTransitionProbabilityBeta(args.getDouble("transition_probability_beta", 0.00959442)); + mapMatching.setTransitionProbabilityBeta(args.getDouble + ("transition_probability_beta", 2.0)); mapMatching.setMeasurementErrorSigma(gpsAccuracy); // do the actual matching, get the GPX entries from a file or via stream diff --git a/matching-core/src/main/java/com/graphhopper/matching/util/HmmProbabilities.java b/matching-core/src/main/java/com/graphhopper/matching/util/HmmProbabilities.java index 70f499ea..9aa52573 100644 --- a/matching-core/src/main/java/com/graphhopper/matching/util/HmmProbabilities.java +++ b/matching-core/src/main/java/com/graphhopper/matching/util/HmmProbabilities.java @@ -26,23 +26,11 @@ public class HmmProbabilities { private final double sigma; private final double beta; - /** - * Sets default values for sigma and beta. - */ - public HmmProbabilities() { - /* - * Sigma taken from Newson&Krumm. - * Beta empirically computed from the Microsoft ground truth data for shortest route - * lengths and 60 s sampling interval but also works for other sampling intervals. - */ - this(4.07, 0.00959442); - } - /** * @param sigma standard deviation of the normal distribution [m] used for * modeling the GPS error - * @param beta beta parameter of the exponential distribution for 1 s - * sampling interval, used for modeling transition probabilities + * @param beta beta parameter of the exponential distribution used for modeling + * transition probabilities */ public HmmProbabilities(double sigma, double beta) { this.sigma = sigma; @@ -67,33 +55,12 @@ public double emissionLogProbability(double distance) { * consecutive map matching candidates. * @param linearDistance Linear distance [m] between two consecutive GPS * measurements. - * @param timeDiff time difference [s] between two consecutive GPS - * measurements. */ - public double transitionLogProbability(double routeLength, double linearDistance, - double timeDiff) { - if (timeDiff == 0) { - return 0; - } - Double transitionMetric = normalizedTransitionMetric(routeLength, linearDistance, timeDiff); - return Distributions.logExponentialDistribution(beta, transitionMetric); - } + public double transitionLogProbability(double routeLength, double linearDistance) { + // Transition metric taken from Newson & Krumm. + Double transitionMetric = Math.abs(linearDistance - routeLength); - /** - * Returns a transition metric for the transition between two consecutive - * map matching candidates. - * - * In contrast to Newson & Krumm the absolute distance difference is divided - * by the quadratic time difference to make the beta parameter of the - * exponential distribution independent of the sampling interval. - */ - private double normalizedTransitionMetric(double routeLength, double linearDistance, - double timeDiff) { - if (timeDiff < 0.0) { - throw new IllegalStateException( - "Time difference between subsequent location measurements must be >= 0."); - } - return Math.abs(linearDistance - routeLength) / (timeDiff * timeDiff); + return Distributions.logExponentialDistribution(beta, transitionMetric); } } diff --git a/matching-core/src/test/java/com/graphhopper/matching/MapMatching2Test.java b/matching-core/src/test/java/com/graphhopper/matching/MapMatching2Test.java index bb093459..f2c83630 100644 --- a/matching-core/src/test/java/com/graphhopper/matching/MapMatching2Test.java +++ b/matching-core/src/test/java/com/graphhopper/matching/MapMatching2Test.java @@ -62,4 +62,26 @@ public void testIssue13() { assertEquals(mr.getGpxEntriesLength(), mr.getMatchLength(), 2.5); assertEquals(28790, mr.getMatchMillis(), 50); } + + @Test + public void testIssue70() { + CarFlagEncoder encoder = new CarFlagEncoder(); + TestGraphHopper hopper = new TestGraphHopper(); + hopper.setDataReaderFile("../map-data/issue-70.osm.gz"); + hopper.setGraphHopperLocation("../target/mapmatchingtest-70"); + hopper.setEncodingManager(new EncodingManager(encoder)); + hopper.importOrLoad(); + + AlgorithmOptions opts = AlgorithmOptions.start().build(); + MapMatching mapMatching = new MapMatching(hopper, opts); + + List inputGPXEntries = new GPXFile(). + doImport("./src/test/resources/issue-70.gpx").getEntries(); + MatchResult mr = mapMatching.doWork(inputGPXEntries); + + assertEquals(Arrays.asList("Милана Видака", "Милана Видака", "Милана Видака", + "Бранка Радичевића", "Бранка Радичевића", "Здравка Челара"), + fetchStreets(mr.getEdgeMatches())); + // TODO: length/time + } } diff --git a/matching-core/src/test/java/com/graphhopper/matching/MapMatchingTest.java b/matching-core/src/test/java/com/graphhopper/matching/MapMatchingTest.java index df5c6c5f..b398596f 100644 --- a/matching-core/src/test/java/com/graphhopper/matching/MapMatchingTest.java +++ b/matching-core/src/test/java/com/graphhopper/matching/MapMatchingTest.java @@ -61,17 +61,13 @@ public class MapMatchingTest { public final static TranslationMap SINGLETON = new TranslationMap().doImport(); + // non-CH / CH test parameters + private final String parameterName; private final TestGraphHopper hopper; private final AlgorithmOptions algoOptions; - public MapMatchingTest(String name, TestGraphHopper hopper, AlgorithmOptions algoOption) { - this.algoOptions = algoOption; - this.hopper = hopper; - } - @Parameterized.Parameters(name = "{0}") public static Collection algoOptions() { - // create hopper instance with CH enabled CarFlagEncoder encoder = new CarFlagEncoder(); TestGraphHopper hopper = new TestGraphHopper(); @@ -82,15 +78,15 @@ public static Collection algoOptions() { // force CH AlgorithmOptions chOpts = AlgorithmOptions.start() - .maxVisitedNodes(40) + .maxVisitedNodes(1000) .hints(new PMap().put(Parameters.CH.DISABLE, false)) .build(); // flexible should fall back to defaults - AlgorithmOptions flexibleOpts = AlgorithmOptions.start(). + AlgorithmOptions flexibleOpts = AlgorithmOptions.start() // TODO: fewer nodes than for CH are possible (short routes & different finish condition & higher degree graph) - // maxVisitedNodes(20). - build(); + // .maxVisitedNodes(20) + .build(); return Arrays.asList(new Object[][]{ {"non-CH", hopper, flexibleOpts}, @@ -98,6 +94,13 @@ public static Collection algoOptions() { }); } + public MapMatchingTest(String parameterName, TestGraphHopper hopper, + AlgorithmOptions algoOption) { + this.parameterName = parameterName; + this.algoOptions = algoOption; + this.hopper = hopper; + } + /** * TODO: split this test up into smaller units with better names? */ @@ -179,8 +182,9 @@ public void testDistantPoints() { new GHPoint(51.23, 12.18), new GHPoint(51.45, 12.59)); MatchResult mr = mapMatching.doWork(inputGPXEntries); - assertEquals(mr.getMatchLength(), 57653, 1); - assertEquals(mr.getMatchMillis(), 2748186, 1); + + assertEquals(57650, mr.getMatchLength(), 1); + assertEquals(2747796, mr.getMatchMillis(), 1); // not OK when we only allow a small number of visited nodes: AlgorithmOptions opts = AlgorithmOptions.start(algoOptions).maxVisitedNodes(1).build(); @@ -208,7 +212,7 @@ public void testSmallSeparatedSearchDistance() { MatchResult mr = mapMatching.doWork(inputGPXEntries); assertEquals(Arrays.asList("Weinligstraße", "Weinligstraße", "Weinligstraße", "Fechnerstraße", "Fechnerstraße"), fetchStreets(mr.getEdgeMatches())); - assertEquals(mr.getGpxEntriesLength(), mr.getMatchLength(), 11); + assertEquals(mr.getGpxEntriesLength(), mr.getMatchLength(), 11); // TODO: this should be around 300m according to Google ... need to check assertEquals(mr.getGpxEntriesMillis(), mr.getMatchMillis(), 3000); } @@ -219,6 +223,10 @@ public void testSmallSeparatedSearchDistance() { @Test public void testLoop() { MapMatching mapMatching = new MapMatching(hopper, algoOptions); + + // Need to reduce GPS accuracy because too many GPX are filtered out otherwise. + mapMatching.setMeasurementErrorSigma(40); + List inputGPXEntries = new GPXFile() .doImport("./src/test/resources/tour2-with-loop.gpx").getEntries(); MatchResult mr = mapMatching.doWork(inputGPXEntries); @@ -258,6 +266,16 @@ public void testLoop2() { */ @Test public void testUTurns() { + // This test requires changing the default heading penalty, which does not work for CH. + if (parameterName.equals("CH")) { + return; + } + + final AlgorithmOptions algoOptions = AlgorithmOptions.start() + // Reduce penalty to allow U-turns + .hints(new PMap().put(Parameters.Routing.HEADING_PENALTY, 50)) + .build(); + MapMatching mapMatching = new MapMatching(hopper, algoOptions); List inputGPXEntries = new GPXFile() .doImport("./src/test/resources/tour4-with-uturn.gpx").getEntries(); @@ -265,12 +283,14 @@ public void testUTurns() { // with large measurement error, we expect no U-turn mapMatching.setMeasurementErrorSigma(50); MatchResult mr = mapMatching.doWork(inputGPXEntries); + assertEquals(Arrays.asList("Gustav-Adolf-Straße", "Gustav-Adolf-Straße", "Funkenburgstraße", "Funkenburgstraße"), fetchStreets(mr.getEdgeMatches())); // with small measurement error, we expect the U-turn mapMatching.setMeasurementErrorSigma(10); mr = mapMatching.doWork(inputGPXEntries); + assertEquals( Arrays.asList("Gustav-Adolf-Straße", "Gustav-Adolf-Straße", "Funkenburgstraße", "Funkenburgstraße", "Funkenburgstraße", "Funkenburgstraße"), diff --git a/matching-core/src/test/java/com/graphhopper/matching/util/HmmProbabilitiesTest.java b/matching-core/src/test/java/com/graphhopper/matching/util/HmmProbabilitiesTest.java deleted file mode 100644 index ae990f0b..00000000 --- a/matching-core/src/test/java/com/graphhopper/matching/util/HmmProbabilitiesTest.java +++ /dev/null @@ -1,18 +0,0 @@ -package com.graphhopper.matching.util; - -import org.junit.Test; -import static org.junit.Assert.*; - -/** - * - * @author Peter Karich - */ -public class HmmProbabilitiesTest { - - @Test - public void testTransitionLogProbability() { - HmmProbabilities instance = new HmmProbabilities(); - // see #13 for the real world problem - assertEquals(0, instance.transitionLogProbability(1, 1, 0), 0.001); - } -} diff --git a/matching-core/src/test/resources/issue-70.gpx b/matching-core/src/test/resources/issue-70.gpx new file mode 100644 index 00000000..ce8c4faa --- /dev/null +++ b/matching-core/src/test/resources/issue-70.gpx @@ -0,0 +1,25 @@ + + + + converted track + + 382 + 382 + 382 + 382 + 382 + 382 + 382 + 382 + 382 + 382 + 382 + 382 + 382 + 382 + 382 + 382 + 382 + + + \ No newline at end of file diff --git a/matching-web/src/test/java/com/graphhopper/matching/http/MatchResultToJsonTest.java b/matching-web/src/test/java/com/graphhopper/matching/http/MatchResultToJsonTest.java index 2adda29a..2d993e9a 100644 --- a/matching-web/src/test/java/com/graphhopper/matching/http/MatchResultToJsonTest.java +++ b/matching-web/src/test/java/com/graphhopper/matching/http/MatchResultToJsonTest.java @@ -58,8 +58,8 @@ public GHPoint3D getSnappedPoint() { } }; - list.add(new GPXExtension(new GPXEntry(-3.4446, -38.9996, 100000), queryResult1, 1)); - list.add(new GPXExtension(new GPXEntry(-3.4448, -38.9999, 100001), queryResult2, 1)); + list.add(new GPXExtension(new GPXEntry(-3.4446, -38.9996, 100000), queryResult1)); + list.add(new GPXExtension(new GPXEntry(-3.4448, -38.9999, 100001), queryResult2)); return list; }