Skip to content

Commit

Permalink
WIP: playing round with directed candidates for graphhopper#70
Browse files Browse the repository at this point in the history
  • Loading branch information
@kodonnell @stefanholder
kodonnell authored and stefanholder committed Dec 11, 2016
1 parent cd388f3 commit f0afa5f
Show file tree
Hide file tree
Showing 8 changed files with 201 additions and 66 deletions.
Binary file added map-data/issue-70.osm.gz
View file
Binary file not shown.
30 changes: 22 additions & 8 deletions matching-core/src/main/java/com/graphhopper/matching/GPXExtension.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,9 @@
*/
package com.graphhopper.matching;

import java.util.List;

import com.graphhopper.routing.VirtualEdgeIteratorState;
import com.graphhopper.storage.index.QueryResult;
import com.graphhopper.util.GPXEntry;

Expand All @@ -27,19 +30,30 @@
public class GPXExtension {
final GPXEntry entry;
final QueryResult queryResult;
final int gpxListIndex;
private boolean directed;
public VirtualEdgeIteratorState incomingVirtualEdge;
public VirtualEdgeIteratorState outgoingVirtualEdge;

public GPXExtension(GPXEntry entry, QueryResult queryResult, int gpxListIndex) {
this.entry = entry;
public GPXExtension(GPXEntry entry, QueryResult queryResult) {
this.entry = entry;
this.queryResult = queryResult;
this.gpxListIndex = gpxListIndex;
this.directed = false;
}

public GPXExtension(GPXEntry entry, QueryResult queryResult, VirtualEdgeIteratorState incomingVirtualEdge, VirtualEdgeIteratorState outgoingVirtualEdge) {
this(entry, queryResult);
this.incomingVirtualEdge = incomingVirtualEdge;
this.outgoingVirtualEdge = outgoingVirtualEdge;
this.directed = true;
}

public boolean isDirected() {
return directed;
}

@Override
public String toString() {
return "entry:" + entry
+ ", query distance:" + queryResult.getQueryDistance()
+ ", gpxListIndex:" + gpxListIndex;
return "entry:" + entry + ", query distance:" + queryResult.getQueryDistance();
}

public QueryResult getQueryResult() {
Expand All @@ -49,4 +63,4 @@ public QueryResult getQueryResult() {
public GPXEntry getEntry() {
return entry;
}
}
}
181 changes: 127 additions & 54 deletions matching-core/src/main/java/com/graphhopper/matching/MapMatching.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@
*/
package com.graphhopper.matching;

import com.graphhopper.routing.VirtualEdgeIteratorState;
import com.graphhopper.GraphHopper;
import com.graphhopper.matching.util.HmmProbabilities;
import com.graphhopper.matching.util.TimeStep;
Expand Down Expand Up @@ -161,70 +162,133 @@ public MatchResult doWork(List<GPXEntry> gpxList) {
+ gpxList.size() + "). Correct format?");
}

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<QueryResult> allCandidates = new ArrayList<>();
List<TimeStep<GPXExtension, GPXEntry, Path>> timeSteps = createTimeSteps(gpxList,
edgeFilter, allCandidates);

if (allCandidates.size() < 2) {
throw new IllegalArgumentException("Too few matching coordinates ("
+ allCandidates.size() + "). Wrong region imported?");
// filter the entries:
List<GPXEntry> 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");
}
if (timeSteps.size() < 2) {
throw new IllegalStateException("Coordinates produced too few time steps "
+ timeSteps.size() + ", gpxList:" + gpxList.size());
}


// now find each of the entries in the graph:
final EdgeFilter edgeFilter = new DefaultEdgeFilter(algoOptions.getWeighting().getFlagEncoder());
List<List<QueryResult>> queriesPerEntry = findGPXEntriesInGraph(filteredGPXEntries, edgeFilter);

// now look up the entries up in the graph:
final QueryGraph queryGraph = new QueryGraph(routingGraph).setUseEdgeExplorerCache(true);
queryGraph.lookup(allCandidates);
List<QueryResult> allQueryResults = new ArrayList<QueryResult>();
for (List<QueryResult> qrs: queriesPerEntry)
allQueryResults.addAll(qrs);
queryGraph.lookup(allQueryResults);

// create candidates from the entries in the graph (a candidate is basically an entry + direction):
List<TimeStep<GPXExtension, GPXEntry, Path>> timeSteps = createTimeSteps(filteredGPXEntries, queriesPerEntry, queryGraph);

List<SequenceState<GPXExtension, GPXEntry, Path>> seq = computeViterbiSequence(timeSteps,
gpxList, queryGraph);
// viterbify:
List<SequenceState<GPXExtension, GPXEntry, Path>> seq = computeViterbiSequence(timeSteps, gpxList, queryGraph);

// finally, extract the result:
final EdgeExplorer explorer = queryGraph.createEdgeExplorer(edgeFilter);
MatchResult matchResult = computeMatchResult(seq, gpxList, allCandidates, explorer);
MatchResult matchResult = computeMatchResult(seq, filteredGPXEntries, queriesPerEntry, explorer);

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<GPXEntry> filterGPXEntries(List<GPXEntry> gpxList) {
List<GPXEntry> filtered = new ArrayList<GPXEntry>();
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<List<QueryResult>> findGPXEntriesInGraph(List<GPXEntry> gpxList, EdgeFilter edgeFilter) {

List<List<QueryResult>> gpxEntryLocations = new ArrayList<List<QueryResult>>();
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.
*/
private List<TimeStep<GPXExtension, GPXEntry, Path>> createTimeSteps(List<GPXEntry> gpxList,
EdgeFilter edgeFilter, List<QueryResult> outAllCandidates) {
int indexGPX = 0;
TimeStep<GPXExtension, GPXEntry, Path> prevTimeStep = null;
private List<TimeStep<GPXExtension, GPXEntry, Path>> createTimeSteps(List<GPXEntry> filteredGPXEntries,
List<List<QueryResult>> queriesPerEntry, QueryGraph queryGraph) {

final List<TimeStep<GPXExtension, GPXEntry, Path>> timeSteps = 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<QueryResult> queryResults = locationIndex.findNClosest(
gpxEntry.lat, gpxEntry.lon,
edgeFilter, measurementErrorSigma);
outAllCandidates.addAll(queryResults);
final List<GPXExtension> candidates = new ArrayList<>();
for (QueryResult candidate : queryResults) {
candidates.add(new GPXExtension(gpxEntry, candidate, indexGPX));
}
final TimeStep<GPXExtension, GPXEntry, Path> timeStep
= new TimeStep<>(gpxEntry, candidates);
timeSteps.add(timeStep);
prevTimeStep = timeStep;
}
indexGPX++;
int n = filteredGPXEntries.size();
assert queriesPerEntry.size() == n;
for (int i = 0; i < n; i++) {

GPXEntry gpxEntry = filteredGPXEntries.get(i);
List<QueryResult> 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<GPXExtension> candidates = new ArrayList<GPXExtension>();
for (QueryResult qr: queryResults) {
int closestNode = qr.getClosestNode();
if (queryGraph.isVirtualNode(closestNode)) {
// get virtual edges:
List<VirtualEdgeIteratorState> virtualEdges = new ArrayList<VirtualEdgeIteratorState>();
EdgeIterator iter = queryGraph.createEdgeExplorer().setBaseNode(closestNode);
while (iter.next()) {
if (queryGraph.isVirtualEdge(iter.getEdge())) {
virtualEdges.add((VirtualEdgeIteratorState) queryGraph.getEdgeIteratorState(iter.getEdge(), iter.getAdjNode()));
}
}
assert virtualEdges.size() == 2;

// create a candidate for each: the candidate being the querypoint plus the virtual edge to favour. Note
// that we favour the virtual edge by *unfavoring* the rest, so we need to record these.
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);
}
}

final TimeStep<GPXExtension, GPXEntry, Path> timeStep = new TimeStep<>(gpxEntry, candidates);
timeSteps.add(timeStep);
}
return timeSteps;
}
Expand Down Expand Up @@ -301,9 +365,16 @@ private void computeTransitionProbabilities(TimeStep<GPXExtension, GPXEntry, Pat
for (GPXExtension from : prevTimeStep.candidates) {
for (GPXExtension to : timeStep.candidates) {
RoutingAlgorithm algo = algoFactory.createAlgo(queryGraph, algoOptions);
// System.out.println("algo " + algo.getName());
final Path path = algo.calcPath(from.getQueryResult().getClosestNode(),
to.getQueryResult().getClosestNode());
// enforce heading if required:
if (from.isDirected()) {
from.incomingVirtualEdge.setUnfavored(true);
}
if (to.isDirected()) {
// unfavor the favour virtual edge
to.outgoingVirtualEdge.setUnfavored(true);
}
final Path path = algo.calcPath(from.getQueryResult().getClosestNode(), to.getQueryResult().getClosestNode());
queryGraph.clearUnfavoredStatus();
if (path.isFound()) {
timeStep.addRoadPath(from, to, path);
final double transitionLogProbability = probabilities
Expand All @@ -315,13 +386,15 @@ private void computeTransitionProbabilities(TimeStep<GPXExtension, GPXEntry, Pat
}

private MatchResult computeMatchResult(List<SequenceState<GPXExtension, GPXEntry, Path>> seq,
List<GPXEntry> gpxList, List<QueryResult> allCandidates,
List<GPXEntry> gpxList, List<List<QueryResult>> 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<String, EdgeIteratorState> virtualEdgesMap = new HashMap<>();
for (QueryResult candidate : allCandidates) {
fillVirtualEdges(virtualEdgesMap, explorer, candidate);
for (List<QueryResult> queryResults: queriesPerEntry) {
for (QueryResult qr: queryResults) {
fillVirtualEdges(virtualEdgesMap, explorer, qr);
}
}

MatchResult matchResult = computeMatchedEdges(seq, virtualEdgesMap);
Expand Down Expand Up @@ -538,4 +611,4 @@ public Path calcPath(MatchResult mr) {
return p;
}
}
}
}
22 changes: 22 additions & 0 deletions matching-core/src/test/java/com/graphhopper/matching/MapMatching2Test.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<GPXEntry> 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
}
}
4 changes: 2 additions & 2 deletions matching-core/src/test/java/com/graphhopper/matching/MapMatchingTest.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -82,7 +82,7 @@ public static Collection<Object[]> algoOptions() {

// force CH
AlgorithmOptions chOpts = AlgorithmOptions.start()
.maxVisitedNodes(40)
.maxVisitedNodes(1000)
.hints(new PMap().put(Parameters.CH.DISABLE, false))
.build();

Expand Down Expand Up @@ -208,7 +208,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);
}

Expand Down
25 changes: 25 additions & 0 deletions matching-core/src/test/resources/issue-70.gpx
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,25 @@
<?xml version="1.0" encoding="UTF-8" standalone="no" ?>
<gpx xmlns="http://www.topografix.com/GPX/1/1" creator="MapSource 6.16.3" version="1.1" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:schemaLocation="http://www.topografix.com/GPX/1/1 http://www.topografix.com/GPX/1/1/gpx.xsd">
<trk>
<name>converted track</name>
<trkseg>
<trkpt lat="45.24443688057394" lon="19.70705632120371"><ele>382</ele><time>2010-01-01T01:00:53Z</time></trkpt>
<trkpt lat="45.244557744418756" lon="19.70795754343271"><ele>382</ele><time>2010-01-01T01:01:53Z</time></trkpt>
<trkpt lat="45.24481457923517" lon="19.709523953497406"><ele>382</ele><time>2010-01-01T01:02:53Z</time></trkpt>
<trkpt lat="45.24516205978563" lon="19.71006039530039"><ele>382</ele><time>2010-01-01T01:03:53Z</time></trkpt>
<trkpt lat="45.24513184416995" lon="19.710961617529392"><ele>382</ele><time>2010-01-01T01:04:53Z</time></trkpt>
<trkpt lat="45.24540378413238" lon="19.71250656992197"><ele>382</ele><time>2010-01-01T01:05:53Z</time></trkpt>
<trkpt lat="45.245690830458216" lon="19.713021554052826"><ele>382</ele><time>2010-01-01T01:06:53Z</time></trkpt>
<trkpt lat="45.24563039977332" lon="19.713944233953953"><ele>382</ele><time>2010-01-01T01:07:53Z</time></trkpt>
<trkpt lat="45.24517716758746" lon="19.71370819956064"><ele>382</ele><time>2010-01-01T01:08:53Z</time></trkpt>
<trkpt lat="45.24482968712937" lon="19.71398714929819"><ele>382</ele><time>2010-01-01T01:09:53Z</time></trkpt>
<trkpt lat="45.24458796033977" lon="19.713622368872166"><ele>382</ele><time>2010-01-01T01:10:53Z</time></trkpt>
<trkpt lat="45.244210260171826" lon="19.713515080511566"><ele>382</ele><time>2010-01-01T01:11:53Z</time></trkpt>
<trkpt lat="45.244557744418756" lon="19.71428755670786"><ele>382</ele><time>2010-01-01T01:12:53Z</time></trkpt>
<trkpt lat="45.2443764485551" lon="19.714502133429047"><ele>382</ele><time>2010-01-01T01:13:53Z</time></trkpt>
<trkpt lat="45.244708823863164" lon="19.71527460962534"><ele>382</ele><time>2010-01-01T01:14:53Z</time></trkpt>
<trkpt lat="45.24460306829425" lon="19.71548918634653"><ele>382</ele><time>2010-01-01T01:15:53Z</time></trkpt>
<trkpt lat="45.244905226540226" lon="19.716197289526463"><ele>382</ele><time>2010-01-01T01:16:53Z</time></trkpt>
</trkseg>
</trk>
</gpx>
1 change: 1 addition & 0 deletions matching-web/src/main/java/com/graphhopper/matching/http/MatchServlet.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,6 +64,7 @@ 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();
Expand Down
4 changes: 2 additions & 2 deletions matching-web/src/test/java/com/graphhopper/matching/http/MatchResultToJsonTest.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

Expand Down

0 comments on commit f0afa5f

Please sign in to comment.