Use Timestamp Information to Limit the Route Between States in the HMM

docs/meili/configuration.md

@@ -19,14 +19,15 @@ All transport modes can specify following parameters:
 
 Parameters                  | Description                                                                                                                        | Default
 ----------------------------|------------------------------------------------------------------------------------------------------------------------------------|-----
-`sigma_ z`                  | An non-negative value to specify the GPS accuracy (the variance of the normal distribution) of an incoming GPS sequence. It is also used to weight emission costs of measurements.  | 4.07
-`beta`                      | An non-negative emprical value to weight the transition cost of two successive candidates.                                                      | 3
-`max_route_distance_factor` | An non-negative value used to limit the routing search range which is the distance to next measurement multiplied by this factor.               | 3
-`breakage_distance`         | An non-negative value. If two successive measurements are far than this distance, then connectivity in between will not be considered.          | 2000 (meters)
+`sigma_ z`                  | A non-negative value to specify the GPS accuracy (the variance of the normal distribution) of an incoming GPS sequence. It is also used to weight emission costs of measurements.  | 4.07
+`beta`                      | A non-negative emprical value to weight the transition cost of two successive candidates.                                                      | 3
+`max_route_distance_factor` | A non-negative value used to limit the routing search range which is the distance to next measurement multiplied by this factor.               | 5
+`max_route_time_factor` | A non-negative value used to limit the routing search range which is the time to next measurement multiplied by this factor.               | 5
+`breakage_distance`         | A non-negative value. If two successive measurements are far than this distance, then connectivity in between will not be considered.          | 2000 (meters)
 `interpolation_distance`    | If two successive measurements are closer than this distance, then the later one will be interpolated into the matched route.                   | 10 (meters)
-`search_radius`             | An non-negative value to specify the search radius (in meters) within which to search road candidates for each measurement.                     | 40 (meters)
+`search_radius`             | A non-negative value to specify the search radius (in meters) within which to search road candidates for each measurement.                     | 50 (meters)
 `max_search_radius`         | Specify the upper bound of `search_radius`                                                                                                      | 100 (meters)
-`turn_penalty_factor`       | An non-negative value to penalize turns from one road segment to next.                                                                          | 0 (meters)
+`turn_penalty_factor`       | A non-negative value to penalize turns from one road segment to next.                                                                          | 0 (meters)
 
 ## Service Parameters
 

scripts/valhalla_build_config

@@ -74,13 +74,14 @@ config = {
   },
   'meili': {
     'mode': 'auto',
-    'customizable': ['mode', 'search_radius', 'turn_penalty_factor', 'gps_accuracy', 'sigma_z', 'beta'],
+    'customizable': ['mode', 'search_radius', 'turn_penalty_factor', 'gps_accuracy', 'sigma_z', 'beta', 'max_route_distance_factor', 'max_route_time_factor'],
     'verbose': False,
     'default': {
       'sigma_z': 4.07,
       'gps_accuracy': 5.0,
       'beta': 3,
       'max_route_distance_factor': 5,
+      'max_route_time_factor': 5,
       'breakage_distance': 2000,
       'interpolation_distance': 10,
       'search_radius': 50,
@@ -281,24 +282,25 @@ help_text = {
     'customizable': 'Specify which parameters are allowed to be customized by URL query parameters',
     'verbose': 'Control verbose output for debugging',
     'default': {
-      'sigma_z': 'An non-negative value to specify the GPS accuracy (the variance of the normal distribution) of an incoming GPS sequence. It is also used to weight emission costs of measurements',
+      'sigma_z': 'A non-negative value to specify the GPS accuracy (the variance of the normal distribution) of an incoming GPS sequence. It is also used to weight emission costs of measurements',
       'gps_accuracy': 'TODO: ',
-      'beta': 'An non-negative emprical value to weight the transition cost of two successive candidates',
-      'max_route_distance_factor': 'An non-negative value used to limit the routing search range which is the distance to next measurement multiplied by this factor',
-      'breakage_distance': 'An non-negative value. If two successive measurements are far than this distance, then connectivity in between will not be considered',
+      'beta': 'A non-negative emprical value to weight the transition cost of two successive candidates',
+      'max_route_distance_factor': 'A non-negative value used to limit the routing search range which is the distance to next measurement multiplied by this factor',
+      'max_route_time_factor': 'A non-negative value used to limit the routing search range which is the time to the next measurement multiplied by this factor',
+      'breakage_distance': 'A non-negative value. If two successive measurements are far than this distance, then connectivity in between will not be considered',
       'interpolation_distance': 'If two successive measurements are closer than this distance, then the later one will be interpolated into the matched route',
-      'search_radius': 'An non-negative value to specify the search radius (in meters) within which to search road candidates for each measurement',
+      'search_radius': 'A non-negative value to specify the search radius (in meters) within which to search road candidates for each measurement',
       'geometry': 'TODO: ',
       'route': 'TODO: ',
       'turn_penalty_factor': 'A non-negative value to penalize turns from one road segment to next'
     },
     'auto': {
       'turn_penalty_factor': 'A non-negative value to penalize turns from one road segment to next',
-      'search_radius': 'An non-negative value to specify the search radius (in meters) within which to search road candidates for each measurement'
+      'search_radius': 'A non-negative value to specify the search radius (in meters) within which to search road candidates for each measurement'
     },
     'pedestrian': {
       'turn_penalty_factor': 'A non-negative value to penalize turns from one road segment to next',
-      'search_radius': 'An non-negative value to specify the search radius (in meters) within which to search road candidates for each measurement'
+      'search_radius': 'A non-negative value to specify the search radius (in meters) within which to search road candidates for each measurement'
     },
     'bicycle': {
       'turn_penalty_factor': 'A non-negative value to penalize turns from one road segment to next'

src/meili/map_matcher.cc

@@ -24,16 +24,23 @@ GreatCircleDistance(const Measurement& left,
                     const Measurement& right)
 { return left.lnglat().Distance(right.lnglat()); }
 
+inline float
+ClockDistance(const Measurement& left,
+              const Measurement& right)
+{ return right.epoch_time() - left.epoch_time(); }
+
 
 struct Interpolation {
   midgard::PointLL projected;
   baldr::GraphId edgeid;
   float sq_distance;
   float route_distance;
+  float route_time;
   float edge_distance;
 
-  float sortcost(const MapMatching& mm, float mmt_dist) const
-  { return mm.CalculateTransitionCost(0.f, route_distance, mmt_dist) + mm.CalculateEmissionCost(sq_distance); }
+  float sortcost(const MapMatching& mm, float gc_dist, float clk_dist) const
+  { return mm.CalculateTransitionCost(0.f, route_distance, gc_dist, route_time, clk_dist) +
+      mm.CalculateEmissionCost(sq_distance); }
 };
 
 
@@ -45,7 +52,8 @@ InterpolateMeasurement(const MapMatching& mapmatching,
                        segment_iterator_t begin,
                        segment_iterator_t end,
                        const Measurement& measurement,
-                       float match_measurement_distance)
+                       float match_measurement_distance,
+                       float match_measurement_time)
 {
   const baldr::GraphTile* tile(nullptr);
   midgard::DistanceApproximator approximator(measurement.lnglat());
@@ -87,15 +95,19 @@ InterpolateMeasurement(const MapMatching& mapmatching,
     // beginning segment
     const auto route_distance = segment_begin_route_distance + distance_to_segment_ends;
 
-    Interpolation interp{projected_point, segment->edgeid, sq_distance, route_distance, offset};
-    const auto cost = interp.sortcost(mapmatching, match_measurement_distance);
+    // Get the amount of time spent on this segment
+    auto edge_percent = segment->target - segment->source;
+    auto route_time = mapmatching.costing()->EdgeCost(directededge).secs * edge_percent;
+
+    Interpolation interp{projected_point, segment->edgeid, sq_distance, route_distance, route_time, offset};
+    const auto cost = interp.sortcost(mapmatching, match_measurement_distance, match_measurement_time);
     if (cost < minimal_cost) {
       minimal_cost = cost;
       best_interp = std::move(interp);
     }
 
     // Assume segments are connected
-    segment_begin_route_distance += directededge->length() * (segment->target - segment->source);
+    segment_begin_route_distance += directededge->length() * edge_percent;
   }
 
   return best_interp;
@@ -130,8 +142,10 @@ InterpolateMeasurements(const MapMatching& mapmatching,
   for (const auto& measurement: interpolated_measurements) {
     const auto& match_measurement = mapmatching.measurement(state->time());
     const auto match_measurement_distance = GreatCircleDistance(measurement, match_measurement);
+    const auto match_measurement_time = ClockDistance(measurement, match_measurement);
     //interpolate this point along the route
-    const auto& interp = InterpolateMeasurement(mapmatching, route.begin(), route.end(), measurement, match_measurement_distance);
+    const auto& interp = InterpolateMeasurement(mapmatching, route.begin(), route.end(),
+        measurement, match_measurement_distance, match_measurement_time);
 
     //if it was able to do the interpolation
     if (interp.edgeid.Is_Valid()) {
@@ -347,8 +361,8 @@ MapMatcher::OfflineMatch(
     }
   }
 
-  const auto state_rbegin = mapmatching_.SearchPath(time),
-               state_rend = std::next(state_rbegin, match_count);
+  const auto state_rbegin = mapmatching_.SearchPath(time);
+  const auto state_rend = std::next(state_rbegin, match_count);
   const auto& results = FindMatchResults(mapmatching_, state_rbegin, state_rend, time);
 
   // Done if no measurements to interpolate

src/meili/map_matching.cc

@@ -13,6 +13,10 @@ GreatCircleDistance(const valhalla::meili::Measurement& left,
                     const valhalla::meili::Measurement& right)
 { return left.lnglat().Distance(right.lnglat()); }
 
+inline float ClockDistance(const valhalla::meili::Measurement& left,
+                    const valhalla::meili::Measurement& right)
+{ return right.epoch_time() - left.epoch_time(); }
+
 }
 
 
@@ -31,6 +35,7 @@ void
 State::route(const std::vector<const State*>& states,
              baldr::GraphReader& graphreader,
              float max_route_distance,
+             float max_route_time,
              const midgard::DistanceApproximator& approximator,
              const float search_radius,
              sif::cost_ptr_t costing,
@@ -46,11 +51,12 @@ State::route(const std::vector<const State*>& states,
   }
 
   // Route
-  labelset_ = std::make_shared<LabelSet>(std::ceil(max_route_distance));
+  labelset_ = std::make_shared<LabelSet>(max_route_distance);
   const auto& results = find_shortest_path(
       graphreader, locations, 0, labelset_,
       approximator, search_radius,
-      costing, edgelabel, turn_cost_table);
+      costing, edgelabel, turn_cost_table,
+      std::ceil(max_route_distance), std::ceil(max_route_time));
 
   // Cache results
   label_idx_.clear();
@@ -83,6 +89,7 @@ MapMatching::MapMatching(baldr::GraphReader& graphreader,
                          float beta,
                          float breakage_distance,
                          float max_route_distance_factor,
+                         float max_route_time_factor,
                          float turn_penalty_factor)
     : graphreader_(graphreader),
       mode_costing_(mode_costing),
@@ -95,6 +102,7 @@ MapMatching::MapMatching(baldr::GraphReader& graphreader,
       inv_beta_(1.f / beta_),
       breakage_distance_(breakage_distance),
       max_route_distance_factor_(max_route_distance_factor),
+      max_route_time_factor_(max_route_time_factor),
       turn_penalty_factor_(turn_penalty_factor),
       turn_cost_table_{0.f}
 {
@@ -127,6 +135,7 @@ MapMatching::MapMatching(baldr::GraphReader& graphreader,
                   config.get<float>("beta"),
                   config.get<float>("breakage_distance"),
                   config.get<float>("max_route_distance_factor"),
+                  config.get<float>("max_route_time_factor"),
                   config.get<float>("turn_penalty_factor")) {}
 
 
@@ -142,18 +151,16 @@ MapMatching::Clear()
   ViterbiSearch<State>::Clear();
 }
 
-
-inline float
-MapMatching::MaxRouteDistance(const State& left, const State& right) const
-{
-  const auto mmt_distance = GreatCircleDistance(measurement(left), measurement(right));
-  return std::min(mmt_distance * max_route_distance_factor_, breakage_distance_);
-}
-
-
 float
 MapMatching::TransitionCost(const State& left, const State& right) const
 {
+  // Get some basic info about difference between the two measurements
+  const auto& left_measurement = measurement(left);
+  const auto& right_measurement = measurement(right);
+  const auto gc_dist = GreatCircleDistance(left_measurement, right_measurement);
+  const auto clk_dist = ClockDistance(left_measurement, right_measurement);
+
+  // If we need to actually compute the route
   if (!left.routed()) {
     std::shared_ptr<const sif::EdgeLabel> edgelabel;
     const auto prev_stateid = predecessor(left.id());
@@ -181,18 +188,20 @@ MapMatching::TransitionCost(const State& left, const State& right) const
     // do not use it for purposes like getting transition cost of
     // two *arbitrary* states.
     left.route(unreached_states_[right.time()], graphreader_,
-               MaxRouteDistance(left, right),
+               std::min(gc_dist * max_route_distance_factor_, breakage_distance_),
+               clk_dist * max_route_time_factor_,
                approximator, measurement(right).search_radius(),
                costing(), edgelabel, turn_cost_table_);
   }
   // TODO: test it state.route(...); assert(state.routed());
 
+  // Compute the transition cost if we found a path
   const auto label = left.last_label(right);
   if (label) {
-    const auto mmt_distance = GreatCircleDistance(measurement(left), measurement(right));
-    return CalculateTransitionCost(label->turn_cost, label->cost.cost, mmt_distance);
+    return CalculateTransitionCost(label->turn_cost, label->cost.cost, gc_dist, label->cost.secs, clk_dist);
   }
 
+  // No path found
   return -1.f;
 }