private void assertCandidate(Tuple<MatcherCandidate, Double> candidate, Point sample) {
Polyline polyline = map.get(candidate.one().point().edge().id()).geometry();
double f = spatial.intercept(polyline, sample);
Point i = spatial.interpolate(polyline, f);
double l = spatial.distance(i, sample);
double sig2 = Math.pow(5d, 2);
double sqrt_2pi_sig2 = Math.sqrt(2d * Math.PI * sig2);
double p = 1 / sqrt_2pi_sig2 * Math.exp((-1) * l / (2 * sig2));
assertEquals(f, candidate.one().point().fraction(), 10E-6);
assertEquals(p, candidate.two(), 10E-6);
}
@Override
public void open() throws SourceException {
if (roads.isEmpty()) {
for (Entry entry : entries) {
Polyline geometry =
(Polyline)
GeometryEngine.geometryFromWkt(
entry.five(), WktImportFlags.wktImportDefaults, Type.Polyline);
roads.add(
new BaseRoad(
entry.one(),
entry.two(),
entry.three(),
entry.one(),
entry.four(),
(short) 0,
1.0f,
100.0f,
100.0f,
(float) spatial.length(geometry),
geometry));
}
}
iterator = roads.iterator();
}
@SuppressWarnings("unused")
private Set<Long> refset(Point sample, double radius) {
Set<Long> refset = new HashSet<Long>();
Iterator<Road> roads = map.edges();
while (roads.hasNext()) {
Road road = roads.next();
double f = spatial.intercept(road.geometry(), sample);
Point i = spatial.interpolate(road.geometry(), f);
double l = spatial.distance(i, sample);
if (l <= radius) {
refset.add(road.id());
}
}
return refset;
}
private void assertTransition(
Tuple<MatcherTransition, Double> transition,
Tuple<MatcherCandidate, MatcherSample> source,
Tuple<MatcherCandidate, MatcherSample> target,
double lambda) {
List<Road> edges = router.route(source.one().point(), target.one().point(), cost);
if (edges == null) {
// fail();
}
Route route = new Route(source.one().point(), target.one().point(), edges);
assertEquals(route.length(), transition.one().route().length(), 10E-6);
assertEquals(route.source().edge().id(), transition.one().route().source().edge().id());
assertEquals(route.target().edge().id(), transition.one().route().target().edge().id());
double beta =
lambda == 0 ? (2.0 * (target.two().time() - source.two().time()) / 1000) : 1 / lambda;
double base = 1.0 * spatial.distance(source.two().point(), target.two().point()) / 60;
double p =
(1 / beta) * Math.exp((-1.0) * Math.max(0, route.cost(new TimePriority()) - base) / beta);
assertEquals(transition.two(), p, 10E-6);
}
@Override
protected Set<Tuple<MatcherCandidate, Double>> candidates(
Set<MatcherCandidate> predecessors, MatcherSample sample) {
if (logger.isTraceEnabled()) {
logger.trace(
"finding candidates for sample {} {}",
new SimpleDateFormat("yyyy-MM-dd HH:mm:ssZ").format(sample.time()),
GeometryEngine.geometryToWkt(sample.point(), WktExportFlags.wktExportPoint));
}
Set<RoadPoint> points_ = map.spatial().radius(sample.point(), radius);
Set<RoadPoint> points = new HashSet<RoadPoint>(Minset.minimize(points_));
Map<Long, RoadPoint> map = new HashMap<Long, RoadPoint>();
for (RoadPoint point : points) {
map.put(point.edge().id(), point);
}
for (MatcherCandidate predecessor : predecessors) {
RoadPoint point = map.get(predecessor.point().edge().id());
if (point != null && point.fraction() < predecessor.point().fraction()) {
points.remove(point);
points.add(predecessor.point());
}
}
Set<Tuple<MatcherCandidate, Double>> candidates =
new HashSet<Tuple<MatcherCandidate, Double>>();
logger.debug("{} ({}) candidates", points.size(), points_.size());
for (RoadPoint point : points) {
double dz = spatial.distance(sample.point(), point.geometry());
double emission = 1 / sqrt_2pi_sig2 * Math.exp((-1) * dz / (2 * sig2));
MatcherCandidate candidate = new MatcherCandidate(point);
candidates.add(new Tuple<MatcherCandidate, Double>(candidate, emission));
logger.trace("{} {} {}", candidate.id(), dz, emission);
}
return candidates;
}
/**
* Matches a full sequence of samples, {@link MatcherSample} objects and returns state
* representation of the full matching which is a {@link KState} object.
*
* @param samples Sequence of samples, {@link MatcherSample} objects.
* @param minDistance Minimum distance in meters between subsequent samples as criterion to match
* a sample. (Avoids unnecessary matching where samples are more dense than necessary.)
* @param minInterval Minimum time interval in milliseconds between subsequent samples as
* criterion to match a sample. (Avoids unnecessary matching where samples are more dense than
* necessary.)
* @return State representation of the full matching which is a {@link KState} object.
*/
public MatcherKState mmatch(List<MatcherSample> samples, double minDistance, int minInterval) {
Collections.sort(
samples,
new Comparator<MatcherSample>() {
@Override
public int compare(MatcherSample left, MatcherSample right) {
return (int) (left.time() - right.time());
}
});
MatcherKState state = new MatcherKState();
for (MatcherSample sample : samples) {
if (state.sample() != null
&& (spatial.distance(sample.point(), state.sample().point()) < Math.max(0, minDistance)
|| (sample.time() - state.sample().time()) < Math.max(0, minInterval))) {
continue;
}
Set<MatcherCandidate> vector = execute(state.vector(), state.sample(), sample);
state.update(vector, sample);
}
return state;
}
@Override
protected Map<MatcherCandidate, Map<MatcherCandidate, Tuple<MatcherTransition, Double>>>
transitions(
final Tuple<MatcherSample, Set<MatcherCandidate>> predecessors,
final Tuple<MatcherSample, Set<MatcherCandidate>> candidates) {
if (logger.isTraceEnabled()) {
logger.trace(
"finding transitions for sample {} {} with {} x {} candidates",
new SimpleDateFormat("yyyy-MM-dd HH:mm:ssZ").format(candidates.one().time()),
GeometryEngine.geometryToWkt(candidates.one().point(), WktExportFlags.wktExportPoint),
predecessors.two().size(),
candidates.two().size());
}
Stopwatch sw = new Stopwatch();
sw.start();
final Set<RoadPoint> targets = new HashSet<RoadPoint>();
for (MatcherCandidate candidate : candidates.two()) {
targets.add(candidate.point());
}
final AtomicInteger count = new AtomicInteger();
final Map<MatcherCandidate, Map<MatcherCandidate, Tuple<MatcherTransition, Double>>>
transitions =
new ConcurrentHashMap<
MatcherCandidate, Map<MatcherCandidate, Tuple<MatcherTransition, Double>>>();
final double base =
1.0 * spatial.distance(predecessors.one().point(), candidates.one().point()) / 60;
final double bound =
Math.max(
1000d,
Math.min(
distance, ((candidates.one().time() - predecessors.one().time()) / 1000) * 100));
InlineScheduler scheduler = StaticScheduler.scheduler();
for (final MatcherCandidate predecessor : predecessors.two()) {
scheduler.spawn(
new Task() {
@Override
public void run() {
Map<MatcherCandidate, Tuple<MatcherTransition, Double>> map =
new HashMap<MatcherCandidate, Tuple<MatcherTransition, Double>>();
Stopwatch sw = new Stopwatch();
sw.start();
Map<RoadPoint, List<Road>> routes =
router.route(predecessor.point(), targets, cost, new Distance(), bound);
sw.stop();
logger.trace("{} routes ({} ms)", routes.size(), sw.ms());
for (MatcherCandidate candidate : candidates.two()) {
List<Road> edges = routes.get(candidate.point());
if (edges == null) {
continue;
}
Route route = new Route(predecessor.point(), candidate.point(), edges);
// According to Newson and Krumm 2009, transition probability is lambda *
// Math.exp((-1.0) * lambda * Math.abs(dt - route.length())), however, we
// experimentally choose lambda * Math.exp((-1.0) * lambda * Math.max(0,
// route.length() - dt)) to avoid unnecessary routes in case of u-turns.
double beta =
lambda == 0
? (2.0
* Math.max(1d, candidates.one().time() - predecessors.one().time())
/ 1000)
: 1 / lambda;
double transition =
(1 / beta)
* Math.exp(
(-1.0) * Math.max(0, route.cost(new TimePriority()) - base) / beta);
map.put(
candidate,
new Tuple<MatcherTransition, Double>(new MatcherTransition(route), transition));
logger.trace(
"{} -> {} {} {} {}",
predecessor.id(),
candidate.id(),
base,
route.length(),
transition);
count.incrementAndGet();
}
transitions.put(predecessor, map);
}
});
}
if (!scheduler.sync()) {
throw new RuntimeException();
}
sw.stop();
logger.trace("{} transitions ({} ms)", count.get(), sw.ms());
return transitions;
}
