Skip to content

Commit

Permalink
Finish penalizing inner-link U-turns (graphhopper#70)
Browse files Browse the repository at this point in the history
  • Loading branch information
@stefanholder
stefanholder committed Dec 18, 2016
1 parent f0afa5f commit e2fe456
Show file tree
Hide file tree
Showing 3 changed files with 143 additions and 41 deletions.
5 changes: 3 additions & 2 deletions CONTRIBUTORS.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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).
10 changes: 7 additions & 3 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,8 +17,6 @@
*/
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 Down Expand Up @@ -53,7 +51,13 @@ public boolean isDirected() {

@Override
public String toString() {
return "entry:" + entry + ", query distance:" + queryResult.getQueryDistance();
return "GPXExtension{" +
"closest node=" + queryResult.getClosestNode() +
" at " + queryResult.getSnappedPoint().getLat() + "," +
queryResult.getSnappedPoint().getLon() +
", incomingEdge=" + incomingVirtualEdge +
", outgoingEdge=" + outgoingVirtualEdge +
'}';
}

public QueryResult getQueryResult() {
Expand Down
169 changes: 133 additions & 36 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,33 +17,29 @@
*/
package com.graphhopper.matching;

import com.graphhopper.routing.VirtualEdgeIteratorState;
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;

/**
Expand All @@ -66,6 +62,12 @@
*/
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;
Expand All @@ -76,6 +78,12 @@ public class MapMatching {
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());

Expand Down Expand Up @@ -170,24 +178,64 @@ public MatchResult doWork(List<GPXEntry> gpxList) {

// 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);
List<QueryResult> allQueryResults = new ArrayList<QueryResult>();
for (List<QueryResult> qrs: queriesPerEntry)
allQueryResults.addAll(qrs);
queryGraph.lookup(allQueryResults);

logger.debug("================= Query results =================");
int i = 1;
for (List<QueryResult> 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());
}
}

// 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);
logger.debug("=============== Time steps ===============");
i = 1;
for (TimeStep<GPXExtension, GPXEntry, Path> ts : timeSteps) {
logger.debug("Candidates for time step {}", i++);
for (GPXExtension candidate : ts.candidates) {
logger.debug(candidate.toString());
}
}

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

logger.debug("=============== Viterbi results =============== ");
i = 1;
for (SequenceState<GPXExtension, GPXEntry, Path> 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, filteredGPXEntries, queriesPerEntry, 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;
}
Expand Down Expand Up @@ -259,10 +307,14 @@ private List<TimeStep<GPXExtension, GPXEntry, Path>> createTimeSteps(List<GPXEnt
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.
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++) {
Expand All @@ -284,7 +336,7 @@ private List<TimeStep<GPXExtension, GPXEntry, Path>> createTimeSteps(List<GPXEnt
// 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);
Expand All @@ -294,15 +346,18 @@ private List<TimeStep<GPXExtension, GPXEntry, Path>> createTimeSteps(List<GPXEnt
}

private List<SequenceState<GPXExtension, GPXEntry, Path>> computeViterbiSequence(
List<TimeStep<GPXExtension, GPXEntry, Path>> timeSteps, List<GPXEntry> gpxList,
final QueryGraph queryGraph) {
List<TimeStep<GPXExtension, GPXEntry, Path>> timeSteps, int originalGpxEntriesCount,
QueryGraph queryGraph) {
final HmmProbabilities probabilities
= new HmmProbabilities(measurementErrorSigma, transitionProbabilityBeta);
final ViterbiAlgorithm<GPXExtension, GPXEntry, Path> viterbi = new ViterbiAlgorithm<>();

logger.debug("\n=============== Paths ===============");
int timeStepCounter = 0;
TimeStep<GPXExtension, GPXEntry, Path> prevTimeStep = null;
int i = 1;
for (TimeStep<GPXExtension, GPXEntry, Path> timeStep : timeSteps) {
logger.debug("\nPaths to time step {}", i++);
computeEmissionProbabilities(timeStep, probabilities);

if (prevTimeStep == null) {
Expand All @@ -328,9 +383,10 @@ private List<SequenceState<GPXExtension, GPXEntry, Path>> 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.");
}

Expand Down Expand Up @@ -361,28 +417,69 @@ private void computeTransitionProbabilities(TimeStep<GPXExtension, GPXEntry, Pat
// time difference in seconds
final double timeDiff
= (timeStep.observation.getTime() - prevTimeStep.observation.getTime()) / 1000.0;
logger.debug("Time difference: {} s", timeDiff);

for (GPXExtension from : prevTimeStep.candidates) {
for (GPXExtension to : timeStep.candidates) {
RoutingAlgorithm algo = algoFactory.createAlgo(queryGraph, algoOptions);
// enforce heading if required:
if (from.isDirected()) {
from.incomingVirtualEdge.setUnfavored(true);
}
queryGraph.unfavorVirtualEdgePair(from.getQueryResult().getClosestNode(),
from.incomingVirtualEdge.getEdge());
}
if (to.isDirected()) {
// unfavor the favour virtual edge
to.outgoingVirtualEdge.setUnfavored(true);
queryGraph.unfavorVirtualEdgePair(to.getQueryResult().getClosestNode(),
to.outgoingVirtualEdge.getEdge());
}
RoutingAlgorithm algo = algoFactory.createAlgo(queryGraph, algoOptions);
final Path path = algo.calcPath(from.getQueryResult().getClosestNode(), to.getQueryResult().getClosestNode());
queryGraph.clearUnfavoredStatus();

if (path.isFound()) {
timeStep.addRoadPath(from, to, path);

// The router considers unfavored virtual edges using edge penalties.
// However, if a virtual edge is included in the computed path,
// the path distance does not include any penalty distance. Hence, we
// need to add our own penalty distance for every unfavored edge in the path.
final double penalizedPathDistance = penalizedPathDistance(path,
queryGraph.getUnfavoredVirtualEdges());

logger.debug("Path from: {}, to: {}, penalized path length: {}",
from, to, penalizedPathDistance);

final double transitionLogProbability = probabilities
.transitionLogProbability(path.getDistance(), linearDistance, timeDiff);
.transitionLogProbability(penalizedPathDistance, linearDistance,
timeDiff);
timeStep.addTransitionLogProbability(from, to, transitionLogProbability);
} else {
logger.debug("No path found for from: {}, to: {}", from, to);
}
queryGraph.clearUnfavoredStatus();

}
}
}

/**
* Returns the path length plus a penalty if the starting/ending edge is unfavored.
*/
private double penalizedPathDistance(Path path,
Set<EdgeIteratorState> 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<EdgeIteratorState> 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<SequenceState<GPXExtension, GPXEntry, Path>> seq,
Expand Down Expand Up @@ -566,7 +663,7 @@ private void printMinDistances(List<TimeStep<GPXExtension, GPXEntry, Path>> time
}
}
}
System.out.println(index + ": " + Math.round(dist) + "m, minimum candidate: "
logger.debug(index + ": " + Math.round(dist) + "m, minimum candidate: "
+ Math.round(minCand) + "m");
index++;
}
Expand Down

0 comments on commit e2fe456

Please sign in to comment.