use of com.bmw.hmm.SequenceState in project graphhopper by graphhopper.
the class MapMatching method computeViterbiSequence.
/**
* Computes the most likely state sequence for the observations.
*/
private List<SequenceState<State, Observation, Path>> computeViterbiSequence(List<ObservationWithCandidateStates> timeSteps) {
final HmmProbabilities probabilities = new HmmProbabilities(measurementErrorSigma, transitionProbabilityBeta);
final ViterbiAlgorithm<State, Observation, Path> viterbi = new ViterbiAlgorithm<>();
int timeStepCounter = 0;
ObservationWithCandidateStates prevTimeStep = null;
for (ObservationWithCandidateStates timeStep : timeSteps) {
final Map<State, Double> emissionLogProbabilities = new HashMap<>();
Map<Transition<State>, Double> transitionLogProbabilities = new HashMap<>();
Map<Transition<State>, Path> roadPaths = new HashMap<>();
for (State candidate : timeStep.candidates) {
// distance from observation to road in meters
final double distance = candidate.getSnap().getQueryDistance();
emissionLogProbabilities.put(candidate, probabilities.emissionLogProbability(distance));
}
if (prevTimeStep == null) {
viterbi.startWithInitialObservation(timeStep.observation, timeStep.candidates, emissionLogProbabilities);
} else {
final double linearDistance = distanceCalc.calcDist(prevTimeStep.observation.getPoint().lat, prevTimeStep.observation.getPoint().lon, timeStep.observation.getPoint().lat, timeStep.observation.getPoint().lon);
for (State from : prevTimeStep.candidates) {
for (State to : timeStep.candidates) {
final Path path = createRouter().calcPath(from.getSnap().getClosestNode(), to.getSnap().getClosestNode(), from.isOnDirectedEdge() ? from.getOutgoingVirtualEdge().getEdge() : EdgeIterator.ANY_EDGE, to.isOnDirectedEdge() ? to.getIncomingVirtualEdge().getEdge() : EdgeIterator.ANY_EDGE);
if (path.isFound()) {
double transitionLogProbability = probabilities.transitionLogProbability(path.getDistance(), linearDistance);
Transition<State> transition = new Transition<>(from, to);
roadPaths.put(transition, path);
transitionLogProbabilities.put(transition, transitionLogProbability);
}
}
}
viterbi.nextStep(timeStep.observation, timeStep.candidates, emissionLogProbabilities, transitionLogProbabilities, roadPaths);
}
if (viterbi.isBroken()) {
fail(timeStepCounter, prevTimeStep, timeStep);
}
timeStepCounter++;
prevTimeStep = timeStep;
}
return viterbi.computeMostLikelySequence();
}
use of com.bmw.hmm.SequenceState in project graphhopper by graphhopper.
the class MapMatching method prepareEdgeMatches.
private List<EdgeMatch> prepareEdgeMatches(List<SequenceState<State, Observation, Path>> seq) {
// This creates a list of directed edges (EdgeIteratorState instances turned the right way),
// each associated with 0 or more of the observations.
// These directed edges are edges of the real street graph, where nodes are intersections.
// So in _this_ representation, the path that you get when you just look at the edges goes from
// an intersection to an intersection.
// Implementation note: We have to look at both states _and_ transitions, since we can have e.g. just one state,
// or two states with a transition that is an empty path (observations snapped to the same node in the query graph),
// but these states still happen on an edge, and for this representation, we want to have that edge.
// (Whereas in the ResponsePath representation, we would just see an empty path.)
// Note that the result can be empty, even when the input is not. Observations can be on nodes as well as on
// edges, and when all observations are on the same node, we get no edge at all.
// But apart from that corner case, all observations that go in here are also in the result.
// (Consider totally forbidding candidate states to be snapped to a point, and make them all be on directed
// edges, then that corner case goes away.)
List<EdgeMatch> edgeMatches = new ArrayList<>();
List<State> states = new ArrayList<>();
EdgeIteratorState currentDirectedRealEdge = null;
for (SequenceState<State, Observation, Path> transitionAndState : seq) {
// transition (except before the first state)
if (transitionAndState.transitionDescriptor != null) {
for (EdgeIteratorState edge : transitionAndState.transitionDescriptor.calcEdges()) {
EdgeIteratorState newDirectedRealEdge = resolveToRealEdge(edge);
if (currentDirectedRealEdge != null) {
if (!equalEdges(currentDirectedRealEdge, newDirectedRealEdge)) {
EdgeMatch edgeMatch = new EdgeMatch(currentDirectedRealEdge, states);
edgeMatches.add(edgeMatch);
states = new ArrayList<>();
}
}
currentDirectedRealEdge = newDirectedRealEdge;
}
}
// state
if (transitionAndState.state.isOnDirectedEdge()) {
// as opposed to on a node
EdgeIteratorState newDirectedRealEdge = resolveToRealEdge(transitionAndState.state.getOutgoingVirtualEdge());
if (currentDirectedRealEdge != null) {
if (!equalEdges(currentDirectedRealEdge, newDirectedRealEdge)) {
EdgeMatch edgeMatch = new EdgeMatch(currentDirectedRealEdge, states);
edgeMatches.add(edgeMatch);
states = new ArrayList<>();
}
}
currentDirectedRealEdge = newDirectedRealEdge;
}
states.add(transitionAndState.state);
}
if (currentDirectedRealEdge != null) {
EdgeMatch edgeMatch = new EdgeMatch(currentDirectedRealEdge, states);
edgeMatches.add(edgeMatch);
}
return edgeMatches;
}
Aggregations