diff --git a/plugins/map_matching.hpp b/plugins/map_matching.hpp index 8a9988277..2dd9545b5 100644 --- a/plugins/map_matching.hpp +++ b/plugins/map_matching.hpp @@ -62,9 +62,6 @@ template class MapMatchingPlugin : public BasePlugin 0.7977883096366508) // valid apriori probability { descriptor_table.emplace("json", 0); - descriptor_table.emplace("gpx", 1); - // descriptor_table.emplace("geojson", 2); - // search_engine_ptr = std::make_shared>(facade); } @@ -93,12 +90,13 @@ template class MapMatchingPlugin : public BasePlugin return label_with_confidence; } - bool get_candiates(const std::vector& input_coords, double& trace_length, Matching::CandidateLists& candidates_lists) + bool get_candiates(const std::vector& input_coords, std::vector& sub_trace_lengths, Matching::CandidateLists& candidates_lists) { double last_distance = coordinate_calculation::great_circle_distance( input_coords[0], input_coords[1]); - trace_length = 0; + sub_trace_lengths.resize(input_coords.size()); + sub_trace_lengths[0] = 0; for (const auto current_coordinate : osrm::irange(0, input_coords.size())) { bool allow_uturn = false; @@ -107,7 +105,7 @@ template class MapMatchingPlugin : public BasePlugin last_distance = coordinate_calculation::great_circle_distance( input_coords[current_coordinate - 1], input_coords[current_coordinate]); - trace_length += last_distance; + sub_trace_lengths[current_coordinate] += sub_trace_lengths[current_coordinate-1] + last_distance; } if (input_coords.size()-1 > current_coordinate && 0 < current_coordinate) @@ -171,10 +169,10 @@ template class MapMatchingPlugin : public BasePlugin return 400; } - double trace_length; + std::vector sub_trace_lengths; Matching::CandidateLists candidates_lists; const auto& input_coords = route_parameters.coordinates; - bool found_candidates = get_candiates(input_coords, trace_length, candidates_lists); + bool found_candidates = get_candiates(input_coords, sub_trace_lengths, candidates_lists); if (!found_candidates) { return 400; @@ -182,72 +180,91 @@ template class MapMatchingPlugin : public BasePlugin // call the actual map matching JSON::Object debug_info; - float matched_length; - std::vector matched_nodes; - search_engine_ptr->map_matching(candidates_lists, input_coords, matched_nodes, matched_length, debug_info); + Matching::SubMatchingList sub_matchings; + search_engine_ptr->map_matching(candidates_lists, input_coords, sub_matchings, debug_info); - // classify result - TraceClassification classification = classify(trace_length, matched_length, input_coords.size() - matched_nodes.size()); - if (classification.first == ClassifierT::ClassLabel::POSITIVE) - { - json_result.values["confidence"] = classification.second; - } - else - { - json_result.values["confidence"] = 1-classification.second; - } - - // run shortest path routing to obtain geometry - InternalRouteResult raw_route; - PhantomNodes current_phantom_node_pair; - for (unsigned i = 0; i < matched_nodes.size() - 1; ++i) - { - current_phantom_node_pair.source_phantom = matched_nodes[i]; - current_phantom_node_pair.target_phantom = matched_nodes[i + 1]; - raw_route.segment_end_coordinates.emplace_back(current_phantom_node_pair); - } - - if (2 > matched_nodes.size()) + if (1 > sub_matchings.size()) { return 400; } - search_engine_ptr->shortest_path( - raw_route.segment_end_coordinates, - std::vector(raw_route.segment_end_coordinates.size(), true), - raw_route); - - DescriptorConfig descriptor_config; - - auto iter = descriptor_table.find(route_parameters.output_format); - unsigned descriptor_type = (iter != descriptor_table.end() ? iter->second : 0); - - descriptor_config.zoom_level = route_parameters.zoom_level; - descriptor_config.instructions = route_parameters.print_instructions; - descriptor_config.geometry = route_parameters.geometry; - descriptor_config.encode_geometry = route_parameters.compression; - - std::shared_ptr> descriptor; - switch (descriptor_type) + JSON::Array traces; + for (auto& sub : sub_matchings) { - // case 0: - // descriptor = std::make_shared>(); - // break; - case 1: - descriptor = std::make_shared>(facade); - break; - // case 2: - // descriptor = std::make_shared>(); - // break; - default: - descriptor = std::make_shared>(facade); - break; + // classify result + double trace_length = sub_trace_lengths[sub.end_idx-1] - sub_trace_lengths[sub.begin_idx]; + TraceClassification classification = classify(trace_length, + sub.length, + (sub.end_idx - sub.begin_idx) - sub.nodes.size()); + if (classification.first == ClassifierT::ClassLabel::POSITIVE) + { + sub.confidence = classification.second; + } + else + { + sub.confidence = 1-classification.second; + } + + // FIXME this is a pretty bad hack. Geometries should obtained directly + // from map_matching. + // run shortest path routing to obtain geometry + InternalRouteResult raw_route; + PhantomNodes current_phantom_node_pair; + for (unsigned i = 0; i < sub.nodes.size() - 1; ++i) + { + current_phantom_node_pair.source_phantom = sub.nodes[i]; + current_phantom_node_pair.target_phantom = sub.nodes[i + 1]; + raw_route.segment_end_coordinates.emplace_back(current_phantom_node_pair); + } + search_engine_ptr->shortest_path( + raw_route.segment_end_coordinates, + std::vector(raw_route.segment_end_coordinates.size(), true), + raw_route); + + + DescriptorConfig descriptor_config; + + auto iter = descriptor_table.find(route_parameters.output_format); + unsigned descriptor_type = (iter != descriptor_table.end() ? iter->second : 0); + + descriptor_config.zoom_level = route_parameters.zoom_level; + descriptor_config.instructions = false; + descriptor_config.geometry = route_parameters.geometry; + descriptor_config.encode_geometry = route_parameters.compression; + + std::shared_ptr> descriptor; + switch (descriptor_type) + { + // case 0: + // descriptor = std::make_shared>(); + // break; + case 1: + descriptor = std::make_shared>(facade); + break; + // case 2: + // descriptor = std::make_shared>(); + // break; + default: + descriptor = std::make_shared>(facade); + break; + } + + JSON::Object temp_result; + descriptor->SetConfig(descriptor_config); + descriptor->Run(raw_route, temp_result); + + JSON::Object subtrace; + // via_route compability + subtrace.values["route_geometry"] = temp_result.values["route_geometry"]; + subtrace.values["confidence"] = sub.confidence; + subtrace.values["via_indicies"] = temp_result.values["via_indicies"]; + subtrace.values["via_points"] = temp_result.values["via_points"]; + + traces.values.push_back(subtrace); } - descriptor->SetConfig(descriptor_config); - descriptor->Run(raw_route, json_result); - json_result.values["debug"] = debug_info; + json_result.values["traces"] = traces; return 200; } diff --git a/routing_algorithms/map_matching.hpp b/routing_algorithms/map_matching.hpp index 9cb19402b..92c05bc67 100644 --- a/routing_algorithms/map_matching.hpp +++ b/routing_algorithms/map_matching.hpp @@ -70,9 +70,19 @@ JSON::Array makeJSONArray(Args... args) namespace Matching { -typedef std::vector> CandidateList; -typedef std::vector CandidateLists; -typedef std::pair PhantomNodesWithProbability; + +struct SubMatching +{ + std::vector nodes; + unsigned begin_idx; + unsigned end_idx; + double length; + double confidence; +}; + +using CandidateList = std::vector>; +using CandidateLists = std::vector; +using SubMatchingList = std::vector; constexpr static const unsigned max_number_of_candidates = 20; } @@ -313,8 +323,7 @@ template class MapMatching final void operator()(const Matching::CandidateLists ×tamp_list, const std::vector coordinate_list, - std::vector& matched_nodes, - float& matched_length, + Matching::SubMatchingList& sub_matchings, JSON::Object& _debug_info) const { BOOST_ASSERT(timestamp_list.size() > 0); @@ -348,6 +357,7 @@ template class MapMatching final _debug_states.values.push_back(_debug_timestamps); } + std::vector split_points; std::vector prev_unbroken_timestamps; prev_unbroken_timestamps.reserve(timestamp_list.size()); prev_unbroken_timestamps.push_back(initial_timestamp); @@ -366,6 +376,8 @@ template class MapMatching final const auto& current_timestamps_list = timestamp_list[t]; const auto& current_coordinate = coordinate_list[t]; + std::cout << " # " << prev_unbroken_timestamp << " -> " << t << std::endl; + // compute d_t for this timestamp and the next one for (auto s = 0u; s < prev_viterbi.size(); ++s) { @@ -433,16 +445,24 @@ template class MapMatching final if (model.breakage[t]) { + std::cout << "Broken!" << std::endl; + // TODO we actually don't need to go to the beginning. + // with temporal information we can split after _n_ + // skipped states if (prev_unbroken_timestamps.size() > 1) { // remove both ends of the breakge prev_unbroken_timestamps.pop_back(); } - // we reached the beginning of the trace, discard the whole beginning + // we reached the beginning of the trace and it is still broken + // -> split the trace here else { + split_points.push_back(t); + // note this preserves everything before t model.clear(t); model.initialize(t); + prev_unbroken_timestamps.push_back(t); } } else @@ -451,41 +471,61 @@ template class MapMatching final } } - if (prev_unbroken_timestamps.size() < 1) + if (prev_unbroken_timestamps.size() > 1) { - return; + split_points.push_back(prev_unbroken_timestamps.back()+1); } - unsigned last_unbroken_timestamp = prev_unbroken_timestamps.back(); - - // loop through the columns, and only compare the last entry - auto max_element_iter = std::max_element(model.viterbi[last_unbroken_timestamp].begin(), - model.viterbi[last_unbroken_timestamp].end()); - auto parent_index = std::distance(model.viterbi[last_unbroken_timestamp].begin(), max_element_iter); - std::deque> reconstructed_indices; - - for (auto i = last_unbroken_timestamp; i > initial_timestamp; --i) + unsigned sub_matching_begin = initial_timestamp; + for (const unsigned sub_matching_end : split_points) { - if (model.breakage[i]) + Matching::SubMatching matching; + + // matchings that only consist of one candidate are invalid + if (sub_matching_end - sub_matching_begin < 2) + { + sub_matching_begin = sub_matching_end; continue; - reconstructed_indices.emplace_front(i, parent_index); - parent_index = model.parents[i][parent_index]; - } - reconstructed_indices.emplace_front(initial_timestamp, parent_index); + } - matched_length = 0.0f; - matched_nodes.resize(reconstructed_indices.size()); - for (auto i = 0u; i < reconstructed_indices.size(); ++i) - { - auto timestamp_index = reconstructed_indices[i].first; - auto location_index = reconstructed_indices[i].second; + std::cout << sub_matching_begin << " -> " << sub_matching_end << std::endl; - matched_nodes[i] = timestamp_list[timestamp_index][location_index].first; - matched_length += model.path_lengths[timestamp_index][location_index]; + matching.begin_idx = sub_matching_begin; + matching.end_idx = sub_matching_end; - _debug_states.values[timestamp_index] - .get().get().values[location_index] - .get().get().values["chosen"] = true; + // loop through the columns, and only compare the last entry + auto max_element_iter = std::max_element(model.viterbi[sub_matching_end-1].begin(), + model.viterbi[sub_matching_end-1].end()); + + auto parent_index = std::distance(model.viterbi[sub_matching_end-1].begin(), max_element_iter); + std::deque> reconstructed_indices; + for (auto i = sub_matching_end-1; i > sub_matching_begin; --i) + { + if (model.breakage[i]) + continue; + reconstructed_indices.emplace_front(i, parent_index); + parent_index = model.parents[i][parent_index]; + } + reconstructed_indices.emplace_front(initial_timestamp, parent_index); + + matching.length = 0.0f; + matching.nodes.resize(reconstructed_indices.size()); + for (auto i = 0u; i < reconstructed_indices.size(); ++i) + { + auto timestamp_index = reconstructed_indices[i].first; + auto location_index = reconstructed_indices[i].second; + + matching.nodes[i] = timestamp_list[timestamp_index][location_index].first; + matching.length += model.path_lengths[timestamp_index][location_index]; + + _debug_states.values[timestamp_index] + .get().get().values[location_index] + .get().get().values["chosen"] = true; + } + + sub_matchings.push_back(matching); + + sub_matching_begin = sub_matching_end; } JSON::Array _debug_breakage;