From c30f43b148ddcfc8c2626dde29f718a0e9e0eb9f Mon Sep 17 00:00:00 2001 From: Kajari Ghosh Date: Tue, 1 Nov 2016 17:13:10 -0400 Subject: [PATCH] added max_speed to the profiles (#3089) --- features/step_definitions/matching.js | 104 +++---- features/support/data_classes.js | 1 + features/testbot/matching.feature | 44 ++- include/engine/datafacade/datafacade_base.hpp | 2 + .../engine/datafacade/internal_datafacade.hpp | 5 + .../engine/datafacade/shared_datafacade.hpp | 5 + .../routing_algorithms/map_matching.hpp | 259 +++++++++--------- include/extractor/profile_properties.hpp | 15 +- profiles/bicycle.lua | 3 +- profiles/car.lua | 1 + profiles/foot.lua | 1 + profiles/testbot.lua | 1 + src/engine/plugins/match.cpp | 1 + src/extractor/scripting_environment_lua.cpp | 3 + unit_tests/mocks/mock_datafacade.hpp | 1 + 15 files changed, 266 insertions(+), 180 deletions(-) diff --git a/features/step_definitions/matching.js b/features/step_definitions/matching.js index f2e18ab14..1c67c09ca 100644 --- a/features/step_definitions/matching.js +++ b/features/step_definitions/matching.js @@ -1,5 +1,6 @@ +'use strict'; + var util = require('util'); -var d3 = require('d3-queue'); var polyline = require('polyline'); module.exports = function () { @@ -44,14 +45,25 @@ module.exports = function () { if (res.statusCode === 200) { if (headers.has('matchings')) { subMatchings = []; - var sub = [json.tracepoints[0].location]; - for(var i = 1; i < json.tracepoints.length; i++){ - if(json.tracepoints[i-1].matchings_index === json.tracepoints[i].matchings_index) { - sub.push(json.tracepoints[i].location); - } else { - subMatchings.push(sub); - sub = [json.tracepoints[i].location]; + + // find the first matched + let start_index = 0; + while (start_index < json.tracepoints.length && json.tracepoints[start_index] === null) start_index++; + + var sub = []; + let prev_index = null; + for(var i = start_index; i < json.tracepoints.length; i++){ + if (json.tracepoints[i] === null) continue; + + let current_index = json.tracepoints[i].matchings_index; + + if(prev_index !== current_index) { + if (sub.length > 0) subMatchings.push(sub); + sub = []; + prev_index = current_index; } + + sub.push(json.tracepoints[i].location); } subMatchings.push(sub); } @@ -82,7 +94,7 @@ module.exports = function () { } if (headers.has('OSM IDs')) { - if (json.matchings.length != 1) throw new Error('*** CHecking annotation only supported for matchings with one subtrace'); + if (json.matchings.length != 1) throw new Error('*** Checking annotation only supported for matchings with one subtrace'); OSMIDs = this.OSMIDList(json.matchings[0]); } } @@ -118,59 +130,53 @@ module.exports = function () { var encodedResult = '', extendedTarget = ''; - var q = d3.queue(); + var testSubMatching = (sub, si) => { + var testSubNode = (ni) => { + var node = this.findNodeByName(sub[ni]), + outNode = subMatchings[si][ni]; - var testSubMatching = (sub, si, scb) => { - if (si >= subMatchings.length) { - ok = false; - q.abort(); - scb(); - } else { - var sq = d3.queue(); - - var testSubNode = (ni, ncb) => { - var node = this.findNodeByName(sub[ni]), - outNode = subMatchings[si][ni]; - - if (this.FuzzyMatch.matchLocation(outNode, node)) { - encodedResult += sub[ni]; - extendedTarget += sub[ni]; - } else { + if (this.FuzzyMatch.matchLocation(outNode, node)) { + encodedResult += sub[ni]; + extendedTarget += sub[ni]; + } else { + if (outNode != null) { encodedResult += util.format('? [%s,%s]', outNode[0], outNode[1]); - extendedTarget += util.format('%s [%d,%d]', node.lat, node.lon); - ok = false; + } else { + encodedResult += '?'; } - ncb(); - }; - - for (var i=0; i { - q.defer(testSubMatching, sub, si); + testSubMatching(sub, si); }); - q.awaitAll(() => { - if (ok) { - if (headers.has('matchings')) { - got.matchings = row.matchings; - } - - if (headers.has('timestamps')) { - got.timestamps = row.timestamps; - } - } else { - got.matchings = encodedResult; - row.matchings = extendedTarget; + if (ok) { + if (headers.has('matchings')) { + got.matchings = row.matchings; } - cb(null, got); - }); + if (headers.has('timestamps')) { + got.timestamps = row.timestamps; + } + } else { + got.matchings = encodedResult; + row.matchings = extendedTarget; + } + + cb(null, got); }; if (row.request) { diff --git a/features/support/data_classes.js b/features/support/data_classes.js index a17141e5e..2ea8d4e09 100644 --- a/features/support/data_classes.js +++ b/features/support/data_classes.js @@ -107,6 +107,7 @@ module.exports = { } matchLocation (got, want) { + if (got == null || want == null) return false; return this.match(got[0], util.format('%d ~0.0025%', want.lon)) && this.match(got[1], util.format('%d ~0.0025%', want.lat)); } diff --git a/features/testbot/matching.feature b/features/testbot/matching.feature index 72550deb8..b63c485d5 100644 --- a/features/testbot/matching.feature +++ b/features/testbot/matching.feature @@ -9,10 +9,11 @@ Feature: Basic Map Matching | geometries | geojson | Scenario: Testbot - Map matching with outlier that has no candidate - Given a grid size of 10 meters + Given a grid size of 100 meters Given the node map """ a b c d + 1 """ @@ -22,7 +23,7 @@ Feature: Basic Map Matching When I match I should get | trace | timestamps | matchings | - | ab1d | 0 1 2 3 | abcd | + | ab1d | 0 1 2 3 | ad | Scenario: Testbot - Map matching with trace splitting Given the node map @@ -169,3 +170,42 @@ Feature: Basic Map Matching When I match I should get | trace | matchings | geometry | | abd | abd | 1,1,1.000089,1,1.000089,1,1.000089,0.99991 | + + Scenario: Testbot - Speed greater than speed threshhold, should split -- returns trace as abcd but should be split into ab,cd + Given a grid size of 10 meters + Given the query options + | geometries | geojson | + + Given the node map + """ + a b ---- x + | + | + y --- c d + """ + + And the ways + | nodes | oneway | + | abxycd | no | + + When I match I should get + | trace | timestamps | matchings | + | abcd | 0 1 2 3 | ab,cd | + + Scenario: Testbot - Speed less than speed threshhold, should not split + Given a grid size of 10 meters + Given the query options + | geometries | geojson | + + Given the node map + """ + a b c d + """ + + And the ways + | nodes | oneway | + | abcd | no | + + When I match I should get + | trace | timestamps | matchings | + | abcd | 0 1 2 3 | abcd | diff --git a/include/engine/datafacade/datafacade_base.hpp b/include/engine/datafacade/datafacade_base.hpp index 331786612..61df241af 100644 --- a/include/engine/datafacade/datafacade_base.hpp +++ b/include/engine/datafacade/datafacade_base.hpp @@ -173,6 +173,8 @@ class BaseDataFacade virtual bool GetContinueStraightDefault() const = 0; + virtual double GetMapMatchingMaxSpeed() const = 0; + virtual BearingClassID GetBearingClassID(const NodeID id) const = 0; virtual util::guidance::TurnBearing PreTurnBearing(const EdgeID eid) const = 0; diff --git a/include/engine/datafacade/internal_datafacade.hpp b/include/engine/datafacade/internal_datafacade.hpp index c43d258e5..82a5729e7 100644 --- a/include/engine/datafacade/internal_datafacade.hpp +++ b/include/engine/datafacade/internal_datafacade.hpp @@ -899,6 +899,11 @@ class InternalDataFacade final : public BaseDataFacade return m_profile_properties.continue_straight_at_waypoint; } + double GetMapMatchingMaxSpeed() const override final + { + return m_profile_properties.max_speed_for_map_matching; + } + BearingClassID GetBearingClassID(const NodeID nid) const override final { return m_bearing_class_id_table.at(nid); diff --git a/include/engine/datafacade/shared_datafacade.hpp b/include/engine/datafacade/shared_datafacade.hpp index 006daf74e..ea3b56f23 100644 --- a/include/engine/datafacade/shared_datafacade.hpp +++ b/include/engine/datafacade/shared_datafacade.hpp @@ -931,6 +931,11 @@ class SharedDataFacade final : public BaseDataFacade return m_profile_properties->continue_straight_at_waypoint; } + double GetMapMatchingMaxSpeed() const override final + { + return m_profile_properties->max_speed_for_map_matching; + } + BearingClassID GetBearingClassID(const NodeID id) const override final { return m_bearing_class_id_table.at(id); diff --git a/include/engine/routing_algorithms/map_matching.hpp b/include/engine/routing_algorithms/map_matching.hpp index a47290d7d..971b4b019 100644 --- a/include/engine/routing_algorithms/map_matching.hpp +++ b/include/engine/routing_algorithms/map_matching.hpp @@ -7,6 +7,7 @@ #include "engine/map_matching/matching_confidence.hpp" #include "engine/map_matching/sub_matching.hpp" +#include "extractor/profile_properties.hpp" #include "util/coordinate_calculation.hpp" #include "util/for_each_pair.hpp" @@ -32,7 +33,6 @@ using HMM = map_matching::HiddenMarkovModel; using SubMatchingList = std::vector; constexpr static const unsigned MAX_BROKEN_STATES = 10; -constexpr static const double MAX_SPEED = 180 / 3.6; // 180km -> m/s static const constexpr double MATCHING_BETA = 10; constexpr static const double MAX_DISTANCE_DELTA = 2000.; @@ -46,6 +46,7 @@ class MapMatching final : public BasicRoutingInterface ×tamps) const { @@ -98,7 +99,7 @@ class MapMatching final : public BasicRoutingInterface + max_broken_time; + } + else + { + return t - prev_unbroken_timestamps.back() > MAX_BROKEN_STATES; + } + }(); + + if (!gap_in_trace) + { + BOOST_ASSERT(!prev_unbroken_timestamps.empty()); + const std::size_t prev_unbroken_timestamp = prev_unbroken_timestamps.back(); + + const auto &prev_viterbi = model.viterbi[prev_unbroken_timestamp]; + const auto &prev_pruned = model.pruned[prev_unbroken_timestamp]; + const auto &prev_unbroken_timestamps_list = + candidates_list[prev_unbroken_timestamp]; + const auto &prev_coordinate = trace_coordinates[prev_unbroken_timestamp]; + + auto ¤t_viterbi = model.viterbi[t]; + auto ¤t_pruned = model.pruned[t]; + auto ¤t_parents = model.parents[t]; + auto ¤t_lengths = model.path_distances[t]; + const auto ¤t_timestamps_list = candidates_list[t]; + const auto ¤t_coordinate = trace_coordinates[t]; + + const auto haversine_distance = util::coordinate_calculation::haversineDistance( + prev_coordinate, current_coordinate); + // assumes minumum of 0.1 m/s + const int duration_upper_bound = + ((haversine_distance + max_distance_delta) * 0.25) * 10; + + // compute d_t for this timestamp and the next one + for (const auto s : util::irange(0UL, prev_viterbi.size())) + { + if (prev_pruned[s]) + { + continue; + } + + for (const auto s_prime : + util::irange(0UL, current_viterbi.size())) + { + const double emission_pr = emission_log_probabilities[t][s_prime]; + double new_value = prev_viterbi[s] + emission_pr; + if (current_viterbi[s_prime] > new_value) + { + continue; + } + + forward_heap.Clear(); + reverse_heap.Clear(); + + double network_distance; + if (facade.GetCoreSize() > 0) + { + forward_core_heap.Clear(); + reverse_core_heap.Clear(); + network_distance = super::GetNetworkDistanceWithCore( + facade, + forward_heap, + reverse_heap, + forward_core_heap, + reverse_core_heap, + prev_unbroken_timestamps_list[s].phantom_node, + current_timestamps_list[s_prime].phantom_node, + duration_upper_bound); + } + else + { + network_distance = super::GetNetworkDistance( + facade, + forward_heap, + reverse_heap, + prev_unbroken_timestamps_list[s].phantom_node, + current_timestamps_list[s_prime].phantom_node); + } + + // get distance diff between loc1/2 and locs/s_prime + const auto d_t = std::abs(network_distance - haversine_distance); + + // very low probability transition -> prune + if (d_t >= max_distance_delta) + { + continue; + } + + const double transition_pr = transition_log_probability(d_t); + new_value += transition_pr; + + if (new_value > current_viterbi[s_prime]) + { + current_viterbi[s_prime] = new_value; + current_parents[s_prime] = std::make_pair(prev_unbroken_timestamp, s); + current_lengths[s_prime] = network_distance; + current_pruned[s_prime] = false; + model.breakage[t] = false; + } + } + } + + if (model.breakage[t]) + { + // save start of breakage -> we need this as split point + if (t < breakage_begin) + { + breakage_begin = t; + } + + BOOST_ASSERT(prev_unbroken_timestamps.size() > 0); + // remove both ends of the breakage + prev_unbroken_timestamps.pop_back(); + } + else + { + prev_unbroken_timestamps.push_back(t); + } + } + // breakage recover has removed all previous good points - bool trace_split = prev_unbroken_timestamps.empty(); + const bool trace_split = prev_unbroken_timestamps.empty(); - // use temporal information if available to determine a split - if (use_timestamps) - { - trace_split = - trace_split || - (trace_timestamps[t] - trace_timestamps[prev_unbroken_timestamps.back()] > - max_broken_time); - } - else - { - trace_split = - trace_split || (t - prev_unbroken_timestamps.back() > MAX_BROKEN_STATES); - } - - if (trace_split) + if (trace_split || gap_in_trace) { std::size_t split_index = t; if (breakage_begin != map_matching::INVALID_STATE) @@ -213,114 +325,9 @@ class MapMatching final : public BasicRoutingInterface new_start >= breakge_begin // we can only reset trace_coordindates.size() times. - t = new_start + 1; - } - - BOOST_ASSERT(!prev_unbroken_timestamps.empty()); - const std::size_t prev_unbroken_timestamp = prev_unbroken_timestamps.back(); - - const auto &prev_viterbi = model.viterbi[prev_unbroken_timestamp]; - const auto &prev_pruned = model.pruned[prev_unbroken_timestamp]; - const auto &prev_unbroken_timestamps_list = candidates_list[prev_unbroken_timestamp]; - const auto &prev_coordinate = trace_coordinates[prev_unbroken_timestamp]; - - auto ¤t_viterbi = model.viterbi[t]; - auto ¤t_pruned = model.pruned[t]; - auto ¤t_parents = model.parents[t]; - auto ¤t_lengths = model.path_distances[t]; - const auto ¤t_timestamps_list = candidates_list[t]; - const auto ¤t_coordinate = trace_coordinates[t]; - - const auto haversine_distance = util::coordinate_calculation::haversineDistance( - prev_coordinate, current_coordinate); - // assumes minumum of 0.1 m/s - const int duration_uppder_bound = - ((haversine_distance + max_distance_delta) * 0.25) * 10; - - // compute d_t for this timestamp and the next one - for (const auto s : util::irange(0UL, prev_viterbi.size())) - { - if (prev_pruned[s]) - { - continue; - } - - for (const auto s_prime : util::irange(0UL, current_viterbi.size())) - { - - const double emission_pr = emission_log_probabilities[t][s_prime]; - double new_value = prev_viterbi[s] + emission_pr; - if (current_viterbi[s_prime] > new_value) - { - continue; - } - - forward_heap.Clear(); - reverse_heap.Clear(); - - double network_distance; - if (facade.GetCoreSize() > 0) - { - forward_core_heap.Clear(); - reverse_core_heap.Clear(); - network_distance = super::GetNetworkDistanceWithCore( - facade, - forward_heap, - reverse_heap, - forward_core_heap, - reverse_core_heap, - prev_unbroken_timestamps_list[s].phantom_node, - current_timestamps_list[s_prime].phantom_node, - duration_uppder_bound); - } - else - { - network_distance = super::GetNetworkDistance( - facade, - forward_heap, - reverse_heap, - prev_unbroken_timestamps_list[s].phantom_node, - current_timestamps_list[s_prime].phantom_node); - } - - // get distance diff between loc1/2 and locs/s_prime - const auto d_t = std::abs(network_distance - haversine_distance); - - // very low probability transition -> prune - if (d_t >= max_distance_delta) - { - continue; - } - - const double transition_pr = transition_log_probability(d_t); - new_value += transition_pr; - - if (new_value > current_viterbi[s_prime]) - { - current_viterbi[s_prime] = new_value; - current_parents[s_prime] = std::make_pair(prev_unbroken_timestamp, s); - current_lengths[s_prime] = network_distance; - current_pruned[s_prime] = false; - model.breakage[t] = false; - } - } - } - - if (model.breakage[t]) - { - // save start of breakage -> we need this as split point - if (t < breakage_begin) - { - breakage_begin = t; - } - - BOOST_ASSERT(prev_unbroken_timestamps.size() > 0); - // remove both ends of the breakage - prev_unbroken_timestamps.pop_back(); - } - else - { - prev_unbroken_timestamps.push_back(t); + t = new_start; + // note: the head of the loop will call ++t, hence the next + // iteration will actually be on new_start+1 } } diff --git a/include/extractor/profile_properties.hpp b/include/extractor/profile_properties.hpp index 16e2d3640..d53bb04f3 100644 --- a/include/extractor/profile_properties.hpp +++ b/include/extractor/profile_properties.hpp @@ -8,11 +8,14 @@ namespace osrm namespace extractor { +const constexpr auto DEFAULT_MAX_SPEED = 180 / 3.6; // 180kmph -> m/s + struct ProfileProperties { ProfileProperties() - : traffic_signal_penalty(0), u_turn_penalty(0), continue_straight_at_waypoint(true), - use_turn_restrictions(false), left_hand_driving(false) + : traffic_signal_penalty(0), u_turn_penalty(0), + max_speed_for_map_matching(DEFAULT_MAX_SPEED), continue_straight_at_waypoint(true), + use_turn_restrictions(false), left_hand_driving(false) { } @@ -30,10 +33,18 @@ struct ProfileProperties traffic_signal_penalty = boost::numeric_cast(traffic_signal_penalty_ * 10.); } + double GetMaxSpeedForMapMatching() const { return max_speed_for_map_matching; } + + void SetMaxSpeedForMapMatching(const double max_speed_for_map_matching_) + { + max_speed_for_map_matching = max_speed_for_map_matching_; + } + //! penalty to cross a traffic light in deci-seconds int traffic_signal_penalty; //! penalty to do a uturn in deci-seconds int u_turn_penalty; + double max_speed_for_map_matching; bool continue_straight_at_waypoint; bool use_turn_restrictions; bool left_hand_driving; diff --git a/profiles/bicycle.lua b/profiles/bicycle.lua index c49a11381..055f7798e 100644 --- a/profiles/bicycle.lua +++ b/profiles/bicycle.lua @@ -93,8 +93,9 @@ surface_speeds = { -- these need to be global because they are accesed externaly properties.traffic_signal_penalty = 2 -properties.use_turn_restrictions = false properties.u_turn_penalty = 20 +properties.max_speed_for_map_matching = 110/3.6 -- kmph -> m/s +properties.use_turn_restrictions = false properties.continue_straight_at_waypoint = false local obey_oneway = true diff --git a/profiles/car.lua b/profiles/car.lua index c2746bba1..f0b361e21 100644 --- a/profiles/car.lua +++ b/profiles/car.lua @@ -147,6 +147,7 @@ maxspeed_table = { -- set profile properties properties.u_turn_penalty = 20 properties.traffic_signal_penalty = 2 +properties.max_speed_for_map_matching = 180/3.6 -- 180kmph -> m/s properties.use_turn_restrictions = true properties.continue_straight_at_waypoint = true properties.left_hand_driving = false diff --git a/profiles/foot.lua b/profiles/foot.lua index cf9c8f5d8..54564d3f9 100644 --- a/profiles/foot.lua +++ b/profiles/foot.lua @@ -66,6 +66,7 @@ leisure_speeds = { properties.traffic_signal_penalty = 2 properties.u_turn_penalty = 2 +properties.max_speed_for_map_matching = 40/3.6 -- kmph -> m/s properties.use_turn_restrictions = false properties.continue_straight_at_waypoint = false diff --git a/profiles/testbot.lua b/profiles/testbot.lua index 57ae76302..dd69e0ae1 100644 --- a/profiles/testbot.lua +++ b/profiles/testbot.lua @@ -20,6 +20,7 @@ properties.continue_straight_at_waypoint = true properties.use_turn_restrictions = true properties.traffic_signal_penalty = 7 -- seconds properties.u_turn_penalty = 20 +properties.max_speed_for_map_matching = 30/3.6 --km -> m/s function limit_speed(speed, limits) -- don't use ipairs(), since it stops at the first nil value diff --git a/src/engine/plugins/match.cpp b/src/engine/plugins/match.cpp index fd814c616..4fa6a97fe 100644 --- a/src/engine/plugins/match.cpp +++ b/src/engine/plugins/match.cpp @@ -4,6 +4,7 @@ #include "engine/api/match_api.hpp" #include "engine/api/match_parameters.hpp" #include "engine/map_matching/bayes_classifier.hpp" +#include "engine/map_matching/sub_matching.hpp" #include "util/coordinate_calculation.hpp" #include "util/integer_range.hpp" #include "util/json_util.hpp" diff --git a/src/extractor/scripting_environment_lua.cpp b/src/extractor/scripting_environment_lua.cpp index 299c3b640..283d8b47b 100644 --- a/src/extractor/scripting_environment_lua.cpp +++ b/src/extractor/scripting_environment_lua.cpp @@ -131,6 +131,9 @@ void LuaScriptingEnvironment::InitContext(LuaScriptingContext &context) .property("u_turn_penalty", &ProfileProperties::GetUturnPenalty, &ProfileProperties::SetUturnPenalty) + .property("max_speed_for_map_matching", + &ProfileProperties::GetMaxSpeedForMapMatching, + &ProfileProperties::SetMaxSpeedForMapMatching) .def_readwrite("use_turn_restrictions", &ProfileProperties::use_turn_restrictions) .def_readwrite("continue_straight_at_waypoint", &ProfileProperties::continue_straight_at_waypoint) diff --git a/unit_tests/mocks/mock_datafacade.hpp b/unit_tests/mocks/mock_datafacade.hpp index ca448954c..6eb1d8a8f 100644 --- a/unit_tests/mocks/mock_datafacade.hpp +++ b/unit_tests/mocks/mock_datafacade.hpp @@ -206,6 +206,7 @@ class MockDataFacade final : public engine::datafacade::BaseDataFacade std::size_t GetCoreSize() const override { return 0; } std::string GetTimestamp() const override { return ""; } bool GetContinueStraightDefault() const override { return true; } + double GetMapMatchingMaxSpeed() const override { return 180 / 3.6; } BearingClassID GetBearingClassID(const NodeID /*id*/) const override { return 0; } EntryClassID GetEntryClassID(const EdgeID /*id*/) const override { return 0; }