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 f69cbc7c..8f354797 100644 --- a/matching-core/src/main/java/com/graphhopper/matching/GPXExtension.java +++ b/matching-core/src/main/java/com/graphhopper/matching/GPXExtension.java @@ -19,36 +19,98 @@ 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; - private boolean directed; - public VirtualEdgeIteratorState incomingVirtualEdge; - public VirtualEdgeIteratorState outgoingVirtualEdge; + private final GPXEntry entry; + private final QueryResult queryResult; + private final boolean isDirected; + private final EdgeIteratorState incomingVirtualEdge; + private final EdgeIteratorState outgoingVirtualEdge; + /** + * Creates an undirected candidate for a real node. + */ public GPXExtension(GPXEntry entry, QueryResult queryResult) { - this.entry = entry; + this.entry = entry; this.queryResult = queryResult; - this.directed = false; + this.isDirected = false; + this.incomingVirtualEdge = null; + this.outgoingVirtualEdge = null; } - - public GPXExtension(GPXEntry entry, QueryResult queryResult, VirtualEdgeIteratorState incomingVirtualEdge, VirtualEdgeIteratorState outgoingVirtualEdge) { - this(entry, queryResult); + + /** + * 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; - this.directed = true; } + public GPXEntry getEntry() { + return entry; + } + + public QueryResult getQueryResult() { + return queryResult; + } + + /** + * 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 directed; + 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{" + @@ -59,12 +121,4 @@ public String toString() { ", outgoingEdge=" + outgoingVirtualEdge + '}'; } - - public QueryResult getQueryResult() { - return this.queryResult; - } - - public GPXEntry getEntry() { - return entry; - } } \ 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 3e1fa721..cda879c6 100644 --- a/matching-core/src/main/java/com/graphhopper/matching/MapMatching.java +++ b/matching-core/src/main/java/com/graphhopper/matching/MapMatching.java @@ -122,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(); @@ -130,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(); } @@ -173,19 +175,21 @@ public MatchResult doWork(List gpxList) { // 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"); + 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()); - List> queriesPerEntry = findGPXEntriesInGraph(filteredGPXEntries, edgeFilter); + List> queriesPerEntry = lookupGPXEntries(filteredGPXEntries, edgeFilter); // now look up the entries up in the graph: final QueryGraph queryGraph = new QueryGraph(routingGraph).setUseEdgeExplorerCache(true); - List allQueryResults = new ArrayList(); + List allQueryResults = new ArrayList<>(); for (List qrs: queriesPerEntry) - allQueryResults.addAll(qrs); + allQueryResults.addAll(qrs); queryGraph.lookup(allQueryResults); logger.debug("================= Query results ================="); @@ -201,8 +205,10 @@ public MatchResult doWork(List gpxList) { } } - // create candidates from the entries in the graph (a candidate is basically an entry + direction): - List> timeSteps = createTimeSteps(filteredGPXEntries, queriesPerEntry, queryGraph); + // 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) { @@ -212,7 +218,7 @@ public MatchResult doWork(List gpxList) { } } - // viterbify: + // Compute the most likely sequence of map matching candidates: List> seq = computeViterbiSequence(timeSteps, gpxList.size(), queryGraph); @@ -245,99 +251,110 @@ public MatchResult doWork(List gpxList) { * 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; - } - } - return filtered; + 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; + } + } + return filtered; } + /** * Find the possible locations of each qpxEntry in the graph. */ - private List> findGPXEntriesInGraph(List gpxList, EdgeFilter edgeFilter) { - - List> gpxEntryLocations = new ArrayList>(); - for (GPXEntry gpxEntry : gpxList) { - gpxEntryLocations.add(locationIndex.findNClosest(gpxEntry.lat, gpxEntry.lon, edgeFilter, measurementErrorSigma)); - } - return gpxEntryLocations; + private List> lookupGPXEntries(List gpxList, + EdgeFilter edgeFilter) { + + List> gpxEntryLocations = new ArrayList<>(); + for (GPXEntry gpxEntry : gpxList) { + gpxEntryLocations.add(locationIndex.findNClosest(gpxEntry.lat, gpxEntry.lon, edgeFilter, + measurementErrorSigma)); + } + return gpxEntryLocations; } - - + /** - * 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. + * 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 List> timeSteps = new ArrayList<>(); + 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."); + } - int n = filteredGPXEntries.size(); - assert queriesPerEntry.size() == n; + final List> timeSteps = new ArrayList<>(); for (int i = 0; i < n; i++) { - - GPXEntry gpxEntry = filteredGPXEntries.get(i); - List queryResults = queriesPerEntry.get(i); - - // as discussed in #51, if the closest node is virtual (i.e. inner-link) then we need to create two candidates: - // one for each direction of each virtual edge. For example, in A---X---B, we'd add the edges A->X and B->X. Note - // that we add the edges with an incoming direction (i.e. A->X not X->A). We can choose to enforce the incoming/outgoing - // direction with the third argument of queryGraph.enforceHeading - 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())) { - virtualEdges.add((VirtualEdgeIteratorState) queryGraph.getEdgeIteratorState(iter.getEdge(), iter.getAdjNode())); - } + + 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 " + 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 candidate for each of the two possible directions through the - // virtual node. This is needed to penalize U-turns at virtual nodes. - 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 { - // just add the real edge, undirected - GPXExtension candidate = new GPXExtension(gpxEntry, qr); - candidates.add(candidate); - } - } + // 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); @@ -345,6 +362,9 @@ private List> createTimeSteps(List> computeViterbiSequence( List> timeSteps, int originalGpxEntriesCount, QueryGraph queryGraph) { @@ -423,23 +443,30 @@ private void computeTransitionProbabilities(TimeStep> seq, - List gpxList, List> queriesPerEntry, + List gpxList, + List> queriesPerEntry, 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 (List queryResults: queriesPerEntry) { - for (QueryResult qr: queryResults) { - fillVirtualEdges(virtualEdgesMap, explorer, qr); - } - } - + final Map virtualEdgesMap = createVirtualEdgesMap( + queriesPerEntry, explorer); MatchResult matchResult = computeMatchedEdges(seq, virtualEdgesMap); computeGpxStats(gpxList, matchResult); @@ -586,28 +607,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) { @@ -638,8 +667,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 + "]"; } @@ -655,8 +684,8 @@ 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; @@ -672,10 +701,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); } @@ -691,7 +719,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()); @@ -700,7 +728,6 @@ public Path calcPath(MatchResult mr) { prevEdge = em.getEdgeState().getEdge(); } - // TODO p.setWeight(weight); p.setFound(true); return p; diff --git a/matching-web/src/main/java/com/graphhopper/matching/http/MatchServlet.java b/matching-web/src/main/java/com/graphhopper/matching/http/MatchServlet.java index 9b584449..b43d688b 100644 --- a/matching-web/src/main/java/com/graphhopper/matching/http/MatchServlet.java +++ b/matching-web/src/main/java/com/graphhopper/matching/http/MatchServlet.java @@ -64,7 +64,6 @@ public class MatchServlet extends GraphHopperServlet { public void doPost(HttpServletRequest httpReq, HttpServletResponse httpRes) throws ServletException, IOException { - logger.info("posted"); String infoStr = httpReq.getRemoteAddr() + " " + httpReq.getLocale(); String inType = "gpx"; String contentType = httpReq.getContentType();