Open-Harbor/osrm-backend
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 3 Wiki Activity

Implement missing matching pieces

Browse Source
This commit is contained in:
Patrick Niklaus 2014-12-08 14:46:31 -08:00
parent 2259bce05f
commit 3a5e41ed91
6 changed files with 317 additions and 112 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
data_structures/static_rtree.hpp
View File

@ -642,10 +642,6 @@ class StaticRTree
return result_coordinate.is_valid();
}
// implementation of the Hjaltason/Samet query [3], a BFS traversal of the tree
// - searches for k elements nearest elements
// - continues to find the k+1st element from a big component if k elements
// come from tiny components
bool IncrementalFindPhantomNodeForCoordinate(
const FixedPointCoordinate &input_coordinate,
std::vector<PhantomNode> &result_phantom_node_vector,
@ -797,9 +793,17 @@ class StaticRTree
return !result_phantom_node_vector.empty();
}
/**
* Returns elements within max_distance.
* If the minium of elements could not be found in the search radius, widen
* it until the minimum can be satisfied.
* At the number of returned nodes is capped at the given maximum.
*/
bool IncrementalFindPhantomNodeForCoordinateWithDistance(
const FixedPointCoordinate &input_coordinate,
std::vector<std::pair<PhantomNode, double>> &result_phantom_node_vector,
const double max_distance,
const unsigned min_number_of_phantom_nodes,
const unsigned max_number_of_phantom_nodes,
const unsigned max_checked_elements = 4 * LEAF_NODE_SIZE)
{
@ -884,7 +888,7 @@ class StaticRTree
// continue searching for the first segment from a big component
if (number_of_elements_from_big_cc == 0 &&
number_of_elements_from_tiny_cc >= max_number_of_phantom_nodes &&
number_of_elements_from_tiny_cc >= max_number_of_phantom_nodes-1 &&
current_segment.is_in_tiny_cc())
{
continue;
@ -900,6 +904,14 @@ class StaticRTree
m_coordinate_list->at(current_segment.v), input_coordinate,
projected_coordinate, foot_point_coordinate_on_segment, current_ratio);
if (number_of_elements_from_big_cc > 0
&& (number_of_elements_from_tiny_cc + number_of_elements_from_tiny_cc >= max_number_of_phantom_nodes
|| current_perpendicular_distance >= max_distance))
{
traversal_queue = std::priority_queue<IncrementalQueryCandidate>{};
continue;
}
// store phantom node in result vector
result_phantom_node_vector.emplace_back(
PhantomNode( current_segment.forward_edge_based_node_id,
@ -951,7 +963,6 @@ class StaticRTree
return !result_phantom_node_vector.empty();
}
bool FindPhantomNodeForCoordinate(const FixedPointCoordinate &input_coordinate,
PhantomNode &result_phantom_node,
const unsigned zoom_level)

101
plugins/map_matching.hpp
View File

@ -29,6 +29,9 @@ or see http://www.gnu.org/licenses/agpl.txt.
#include "../routing_algorithms/map_matching.hpp"
#include "../util/simple_logger.hpp"
#include "../util/string_util.hpp"
#include "../descriptors/descriptor_base.hpp"
#include "../descriptors/gpx_descriptor.hpp"
#include "../descriptors/json_descriptor.hpp"
#include <cstdlib>
@ -40,11 +43,16 @@ or see http://www.gnu.org/licenses/agpl.txt.
template <class DataFacadeT> class MapMatchingPlugin : public BasePlugin
{
private:
std::unordered_map<std::string, unsigned> descriptor_table;
std::shared_ptr<SearchEngine<DataFacadeT>> search_engine_ptr;
public:
MapMatchingPlugin(DataFacadeT *facade) : descriptor_string("match"), facade(facade)
{
descriptor_table.emplace("json", 0);
descriptor_table.emplace("gpx", 1);
// descriptor_table.emplace("geojson", 2);
//
search_engine_ptr = std::make_shared<SearchEngine<DataFacadeT>>(facade);
}
@ -55,47 +63,98 @@ template <class DataFacadeT> class MapMatchingPlugin : public BasePlugin
int HandleRequest(const RouteParameters &route_parameters, JSON::Object &json_result) final
{
// check number of parameters
SimpleLogger().Write() << "1";
if (!check_all_coordinates(route_parameters.coordinates))
{
return 400;
}
SimpleLogger().Write() << "2";
InternalRouteResult raw_route;
Matching::CandidateLists candidate_lists;
candidate_lists.resize(route_parameters.coordinates.size());
SimpleLogger().Write() << "3";
// fetch 10 candidates for each given coordinate
for (const auto current_coordinate : osrm::irange<std::size_t>(0, candidate_lists.size()))
double last_distance = coordinate_calculation::great_circle_distance(
route_parameters.coordinates[0],
route_parameters.coordinates[1]);
for (const auto current_coordinate : osrm::irange<std::size_t>(0, route_parameters.coordinates.size()))
{
if (0 < current_coordinate)
last_distance = coordinate_calculation::great_circle_distance(
route_parameters.coordinates[current_coordinate - 1],
route_parameters.coordinates[current_coordinate]);
std::cout << "Searching: " << current_coordinate << std::endl;
std::vector<std::pair<PhantomNode, double>> candidates;
if (!facade->IncrementalFindPhantomNodeForCoordinateWithDistance(
route_parameters.coordinates[current_coordinate],
candidate_lists[current_coordinate],
10))
candidates,
last_distance,
5,
20))
{
std::cout << "Nothing found for " << current_coordinate << std::endl;
continue;
}
candidate_lists.push_back(candidates);
std::cout << current_coordinate << " (" << (last_distance / 2.0) << ") : "
<< candidates.size() << std::endl;
BOOST_ASSERT(candidate_lists[current_coordinate].size() == 10);
}
if (2 > candidate_lists.size())
{
return 400;
}
while (candidate_lists[current_coordinate].size() < 10)
{
// TODO: add dummy candidates, if any are missing
// TODO: add factory method to get an invalid PhantomNode/Distance pair
}
}
SimpleLogger().Write() << "4";
// call the actual map matching
search_engine_ptr->map_matching(10, candidate_lists, route_parameters.coordinates, raw_route);
std::vector<PhantomNode> matched_nodes;
JSON::Object debug_info;
search_engine_ptr->map_matching(candidate_lists, route_parameters.coordinates, matched_nodes, debug_info);
if (INVALID_EDGE_WEIGHT == raw_route.shortest_path_length)
PhantomNodes current_phantom_node_pair;
for (unsigned i = 0; i < matched_nodes.size() - 1; ++i)
{
SimpleLogger().Write(logDEBUG) << "Error occurred, single path not found";
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);
}
search_engine_ptr->shortest_path(
raw_route.segment_end_coordinates, route_parameters.uturns, 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<BaseDescriptor<DataFacadeT>> descriptor;
switch (descriptor_type)
{
// case 0:
// descriptor = std::make_shared<JSONDescriptor<DataFacadeT>>();
// break;
case 1:
descriptor = std::make_shared<GPXDescriptor<DataFacadeT>>(facade);
break;
// case 2:
// descriptor = std::make_shared<GEOJSONDescriptor<DataFacadeT>>();
// break;
default:
descriptor = std::make_shared<JSONDescriptor<DataFacadeT>>(facade);
break;
}
descriptor->SetConfig(descriptor_config);
descriptor->Run(raw_route, json_result);
json_result.values["debug"] = debug_info;
return 200;
}

263
routing_algorithms/map_matching.hpp
View File

@ -31,6 +31,38 @@ or see http://www.gnu.org/licenses/agpl.txt.
#include <iomanip>
#include <numeric>
#include <fstream>
template<typename T>
T makeJSONSave(T d)
{
if (std::isnan(d) || std::numeric_limits<T>::infinity() == d) {
return std::numeric_limits<T>::max();
}
if (-std::numeric_limits<T>::infinity() == d) {
return -std::numeric_limits<T>::max();
}
return d;
}
void appendToJSONArray(JSON::Array& a) { }
template<typename T, typename... Args>
void appendToJSONArray(JSON::Array& a, T value, Args... args)
{
a.values.emplace_back(value);
appendToJSONArray(a, args...);
}
template<typename... Args>
JSON::Array makeJSONArray(Args... args)
{
JSON::Array a;
appendToJSONArray(a, args...);
return a;
}
namespace Matching
{
typedef std::vector<std::pair<PhantomNode, double>> CandidateList;
@ -115,9 +147,9 @@ template <class DataFacadeT> class MapMatching final
const std::vector<FixedPointCoordinate> coordinate_list) const
{
std::vector<double> d_t_list, median_select_d_t_list;
for (auto t = 1; t < timestamp_list.size(); ++t)
for (auto t = 1u; t < timestamp_list.size(); ++t)
{
for (auto s = 0; s < state_size; ++s)
for (auto s = 0u; s < state_size; ++s)
{
d_t_list.push_back(get_distance_difference(coordinate_list[t - 1],
coordinate_list[t],
@ -142,7 +174,7 @@ template <class DataFacadeT> class MapMatching final
const PhantomNode &target_phantom) const
{
// great circle distance of two locations - median/avg dist table(candidate list1/2)
const EdgeWeight network_distance = get_network_distance(source_phantom, target_phantom);
const auto network_distance = get_network_distance(source_phantom, target_phantom);
const auto great_circle_distance =
coordinate_calculation::great_circle_distance(location1, location2);
@ -153,7 +185,7 @@ template <class DataFacadeT> class MapMatching final
return network_distance - great_circle_distance;
}
EdgeWeight get_network_distance(const PhantomNode &source_phantom,
double get_network_distance(const PhantomNode &source_phantom,
const PhantomNode &target_phantom) const
{
EdgeWeight upper_bound = INVALID_EDGE_WEIGHT;
@ -209,7 +241,31 @@ template <class DataFacadeT> class MapMatching final
reverse_heap, forward_heap, &middle_node, &upper_bound, edge_offset, false);
}
}
return upper_bound;
double distance = std::numeric_limits<double>::max();
if (upper_bound != INVALID_EDGE_WEIGHT)
{
std::vector<NodeID> packed_leg;
super::RetrievePackedPathFromHeap(forward_heap, reverse_heap, middle_node, packed_leg);
std::vector<PathData> unpacked_path;
PhantomNodes nodes;
nodes.source_phantom = source_phantom;
nodes.target_phantom = target_phantom;
super::UnpackPath(packed_leg, nodes, unpacked_path);
FixedPointCoordinate previous_coordinate = source_phantom.location;
FixedPointCoordinate current_coordinate;
distance = 0;
for (const auto& p : unpacked_path)
{
current_coordinate = super::facade->GetCoordinateOfNode(p.node);
distance += coordinate_calculation::great_circle_distance(previous_coordinate, current_coordinate);
previous_coordinate = current_coordinate;
}
distance += coordinate_calculation::great_circle_distance(previous_coordinate, target_phantom.location);
}
return distance;
}
public:
@ -218,97 +274,170 @@ template <class DataFacadeT> class MapMatching final
{
}
void operator()(const unsigned state_size,
const Matching::CandidateLists &timestamp_list,
const std::vector<FixedPointCoordinate> coordinate_list,
InternalRouteResult &raw_route_data) const
// TODO optimize: a lot of copying that could probably be avoided
void expandCandidates(const Matching::CandidateLists &candidates_lists,
Matching::CandidateLists &expanded_lists) const
{
BOOST_ASSERT(state_size != std::numeric_limits<unsigned>::max());
BOOST_ASSERT(state_size != 0);
SimpleLogger().Write() << "matching starts with " << timestamp_list.size() << " locations";
SimpleLogger().Write() << "state_size: " << state_size;
std::vector<std::vector<double>> viterbi(state_size,
std::vector<double>(timestamp_list.size() + 1, 0));
std::vector<std::vector<std::size_t>> parent(
state_size, std::vector<std::size_t>(timestamp_list.size() + 1, 0));
SimpleLogger().Write() << "a";
for (auto s = 0; s < state_size; ++s)
// expand list of PhantomNodes to be single-directional
expanded_lists.resize(candidates_lists.size());
for (const auto i : osrm::irange(0lu, candidates_lists.size()))
{
SimpleLogger().Write() << "initializing s: " << s << "/" << state_size;
SimpleLogger().Write()
<< " distance: " << timestamp_list[0][s].second << " at "
<< timestamp_list[0][s].first.location << " prob " << std::setprecision(10)
<< emission_probability(timestamp_list[0][s].second) << " logprob "
<< log_probability(emission_probability(timestamp_list[0][s].second));
// TODO: implement
const double emission_pr = 0.;
viterbi[s][0] = emission_pr;
parent[s][0] = s;
for (const auto& candidate : candidates_lists[i])
{
// bi-directional edge, split phantom node
if (candidate.first.forward_node_id != SPECIAL_NODEID && candidate.first.reverse_node_id != SPECIAL_NODEID)
{
PhantomNode forward_node(candidate.first);
PhantomNode reverse_node(candidate.first);
forward_node.reverse_node_id = SPECIAL_NODEID;
reverse_node.forward_node_id = SPECIAL_NODEID;
expanded_lists[i].emplace_back(forward_node, candidate.second);
expanded_lists[i].emplace_back(reverse_node, candidate.second);
}
SimpleLogger().Write() << "b";
else
{
expanded_lists[i].push_back(candidate);
}
}
}
}
void operator()(const Matching::CandidateLists &candidates_lists,
const std::vector<FixedPointCoordinate> coordinate_list,
std::vector<PhantomNode>& matched_nodes,
JSON::Object& _debug_info) const
{
BOOST_ASSERT(candidates_lists.size() == coordinate_list.size());
Matching::CandidateLists timestamp_list;
expandCandidates(candidates_lists, timestamp_list);
BOOST_ASSERT(timestamp_list.size() > 0);
// TODO for the viterbi values we actually only need the current and last row
std::vector<std::vector<double>> viterbi;
std::vector<std::vector<std::size_t>> parents;
for (const auto& l : timestamp_list)
{
viterbi.emplace_back(l.size(), -std::numeric_limits<double>::infinity());
parents.emplace_back(l.size(), 0);
}
JSON::Array _debug_viterbi;
JSON::Array _debug_initial_viterbi;
for (auto s = 0u; s < viterbi[0].size(); ++s)
{
// this might need to be squared as pi_s is also defined as the emission
// probability in the paper.
viterbi[0][s] = log_probability(emission_probability(timestamp_list[0][s].second));
parents[0][s] = s;
_debug_initial_viterbi.values.push_back(makeJSONSave(viterbi[0][s]));
}
_debug_viterbi.values.push_back(_debug_initial_viterbi);
// attention, this call is relatively expensive
const auto beta = get_beta(state_size, timestamp_list, coordinate_list);
//const auto beta = get_beta(state_size, timestamp_list, coordinate_list);
const auto beta = 10.0;
for (auto t = 1; t < timestamp_list.size(); ++t)
JSON::Array _debug_timestamps;
for (auto t = 1u; t < timestamp_list.size(); ++t)
{
const auto& prev_viterbi = viterbi[t-1];
const auto& prev_timestamps_list = timestamp_list[t-1];
const auto& prev_coordinate = coordinate_list[t-1];
auto& current_viterbi = viterbi[t];
auto& current_parents = parents[t];
const auto& current_timestamps_list = timestamp_list[t];
const auto& current_coordinate = coordinate_list[t];
JSON::Array _debug_transition_rows;
// compute d_t for this timestamp and the next one
for (auto s = 0; s < state_size; ++s)
for (auto s = 0u; s < prev_viterbi.size(); ++s)
{
for (auto s_prime = 0; s_prime < state_size; ++s_prime)
JSON::Array _debug_row;
for (auto s_prime = 0u; s_prime < current_viterbi.size(); ++s_prime)
{
// how likely is candidate s_prime at time t to be emitted?
const double emission_pr = emission_probability(timestamp_list[t][s_prime].second);
const double emission_pr = log_probability(emission_probability(timestamp_list[t][s_prime].second));
// get distance diff between loc1/2 and locs/s_prime
const auto d_t = get_distance_difference(coordinate_list[t-1],
coordinate_list[t],
timestamp_list[t-1][s].first,
timestamp_list[t][s_prime].first);
const auto d_t = get_distance_difference(prev_coordinate,
current_coordinate,
prev_timestamps_list[s].first,
current_timestamps_list[s_prime].first);
// plug probabilities together. TODO: change to addition for logprobs
const double transition_pr = transition_probability(beta, d_t);
const double new_value = viterbi[s][t] * emission_pr * transition_pr;
if (new_value > viterbi[s_prime][t])
// plug probabilities together
const double transition_pr = log_probability(transition_probability(d_t, beta));
const double new_value = prev_viterbi[s] + emission_pr + transition_pr;
JSON::Array _debug_element = makeJSONArray(
makeJSONSave(prev_viterbi[s]),
makeJSONSave(emission_pr),
makeJSONSave(transition_pr),
get_network_distance(prev_timestamps_list[s].first, current_timestamps_list[s_prime].first),
coordinate_calculation::great_circle_distance(prev_coordinate, current_coordinate)
);
_debug_row.values.push_back(_debug_element);
if (new_value > current_viterbi[s_prime])
{
viterbi[s_prime][t] = new_value;
parent[s_prime][t] = s;
current_viterbi[s_prime] = new_value;
current_parents[s_prime] = s;
}
}
_debug_transition_rows.values.push_back(_debug_row);
}
_debug_timestamps.values.push_back(_debug_transition_rows);
JSON::Array _debug_viterbi_col;
for (auto s_prime = 0u; s_prime < current_timestamps_list.size(); ++s_prime)
{
_debug_viterbi_col.values.push_back(makeJSONSave(current_viterbi[s_prime]));
}
SimpleLogger().Write() << "c";
SimpleLogger().Write() << "timestamps: " << timestamp_list.size();
const auto number_of_timestamps = timestamp_list.size();
const auto max_element_iter = std::max_element(viterbi[number_of_timestamps].begin(),
viterbi[number_of_timestamps].end());
auto parent_index = std::distance(max_element_iter, viterbi[number_of_timestamps].begin());
_debug_viterbi.values.push_back(_debug_viterbi_col);
}
_debug_info.values["transitions"] = _debug_timestamps;
_debug_info.values["viterbi"] = _debug_viterbi;
_debug_info.values["beta"] = beta;
// loop through the columns, and only compare the last entry
auto max_element_iter = std::max_element(viterbi.back().begin(), viterbi.back().end());
auto parent_index = std::distance(viterbi.back().begin(), max_element_iter);
std::deque<std::size_t> reconstructed_indices;
SimpleLogger().Write() << "d";
for (auto i = number_of_timestamps - 1; i > 0; --i)
for (auto i = timestamp_list.size() - 1u; i > 0u; --i)
{
SimpleLogger().Write() << "[" << i << "] parent: " << parent_index ;
reconstructed_indices.push_front(parent_index);
parent_index = parent[parent_index][i];
parent_index = parents[i][parent_index];
}
SimpleLogger().Write() << "[0] parent: " << parent_index;
reconstructed_indices.push_front(parent_index);
SimpleLogger().Write() << "e";
for (auto i = 0; i < reconstructed_indices.size(); ++i)
JSON::Array _debug_chosen_candidates;
matched_nodes.resize(reconstructed_indices.size());
for (auto i = 0u; i < reconstructed_indices.size(); ++i)
{
auto location_index = reconstructed_indices[i];
SimpleLogger().Write() << std::setprecision(8) << "location " << coordinate_list[i] << " to " << timestamp_list[i][location_index].first.location;
matched_nodes[i] = timestamp_list[i][location_index].first;
_debug_chosen_candidates.values.push_back(location_index);
}
SimpleLogger().Write() << "f, done";
_debug_info.values["chosen_candidates"] = _debug_chosen_candidates;
JSON::Array _debug_expanded_candidates;
for (const auto& l : timestamp_list) {
JSON::Array _debug_expanded_candidates_col;
for (const auto& pair : l) {
const auto& coord = pair.first.location;
_debug_expanded_candidates_col.values.push_back(makeJSONArray(coord.lat / COORDINATE_PRECISION,
coord.lon / COORDINATE_PRECISION));
}
_debug_expanded_candidates.values.push_back(_debug_expanded_candidates_col);
}
_debug_info.values["expanded_candidates"] = _debug_expanded_candidates;
}
};

4
server/data_structures/datafacade_base.hpp
View File

@ -108,7 +108,9 @@ template <class EdgeDataT> class BaseDataFacade
virtual bool
IncrementalFindPhantomNodeForCoordinateWithDistance(const FixedPointCoordinate &input_coordinate,
std::vector<std::pair<PhantomNode, double>> &resulting_phantom_node_vector,
const unsigned number_of_results) = 0;
const double max_distance,
const unsigned min_number_of_phantom_nodes,
const unsigned max_number_of_phantom_nodes) = 0;
virtual unsigned GetCheckSum() const = 0;

6
server/data_structures/internal_datafacade.hpp
View File

@ -419,7 +419,9 @@ template <class EdgeDataT> class InternalDataFacade final : public BaseDataFacad
bool
IncrementalFindPhantomNodeForCoordinateWithDistance(const FixedPointCoordinate &input_coordinate,
std::vector<std::pair<PhantomNode, double>> &resulting_phantom_node_vector,
const unsigned number_of_results) final
const double max_distance,
const unsigned min_number_of_phantom_nodes,
const unsigned max_number_of_phantom_nodes) final
{
if (!m_static_rtree.get())
{
@ -427,7 +429,7 @@ template <class EdgeDataT> class InternalDataFacade final : public BaseDataFacad
}
return m_static_rtree->IncrementalFindPhantomNodeForCoordinateWithDistance(
input_coordinate, resulting_phantom_node_vector, number_of_results);
input_coordinate, resulting_phantom_node_vector, max_distance, min_number_of_phantom_nodes, max_number_of_phantom_nodes);
}
unsigned GetCheckSum() const override final { return m_check_sum; }

6
server/data_structures/shared_datafacade.hpp
View File

@ -407,7 +407,9 @@ template <class EdgeDataT> class SharedDataFacade final : public BaseDataFacade<
bool
IncrementalFindPhantomNodeForCoordinateWithDistance(const FixedPointCoordinate &input_coordinate,
std::vector<std::pair<PhantomNode, double>> &resulting_phantom_node_vector,
const unsigned number_of_results) final
const double max_distance,
const unsigned min_number_of_phantom_nodes,
const unsigned max_number_of_phantom_nodes) final
{
if (!m_static_rtree.get() || CURRENT_TIMESTAMP != m_static_rtree->first)
{
@ -415,7 +417,7 @@ template <class EdgeDataT> class SharedDataFacade final : public BaseDataFacade<
}
return m_static_rtree->second->IncrementalFindPhantomNodeForCoordinateWithDistance(
input_coordinate, resulting_phantom_node_vector, number_of_results);
input_coordinate, resulting_phantom_node_vector, max_distance, min_number_of_phantom_nodes, max_number_of_phantom_nodes);
}
unsigned GetCheckSum() const override final { return m_check_sum; }