Implement missing matching pieces
This commit is contained in:
parent
2259bce05f
commit
3a5e41ed91
@ -642,10 +642,6 @@ class StaticRTree
|
|||||||
return result_coordinate.is_valid();
|
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(
|
bool IncrementalFindPhantomNodeForCoordinate(
|
||||||
const FixedPointCoordinate &input_coordinate,
|
const FixedPointCoordinate &input_coordinate,
|
||||||
std::vector<PhantomNode> &result_phantom_node_vector,
|
std::vector<PhantomNode> &result_phantom_node_vector,
|
||||||
@ -797,9 +793,17 @@ class StaticRTree
|
|||||||
return !result_phantom_node_vector.empty();
|
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(
|
bool IncrementalFindPhantomNodeForCoordinateWithDistance(
|
||||||
const FixedPointCoordinate &input_coordinate,
|
const FixedPointCoordinate &input_coordinate,
|
||||||
std::vector<std::pair<PhantomNode, double>> &result_phantom_node_vector,
|
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_number_of_phantom_nodes,
|
||||||
const unsigned max_checked_elements = 4 * LEAF_NODE_SIZE)
|
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
|
// continue searching for the first segment from a big component
|
||||||
if (number_of_elements_from_big_cc == 0 &&
|
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())
|
current_segment.is_in_tiny_cc())
|
||||||
{
|
{
|
||||||
continue;
|
continue;
|
||||||
@ -900,6 +904,14 @@ class StaticRTree
|
|||||||
m_coordinate_list->at(current_segment.v), input_coordinate,
|
m_coordinate_list->at(current_segment.v), input_coordinate,
|
||||||
projected_coordinate, foot_point_coordinate_on_segment, current_ratio);
|
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
|
// store phantom node in result vector
|
||||||
result_phantom_node_vector.emplace_back(
|
result_phantom_node_vector.emplace_back(
|
||||||
PhantomNode( current_segment.forward_edge_based_node_id,
|
PhantomNode( current_segment.forward_edge_based_node_id,
|
||||||
@ -951,7 +963,6 @@ class StaticRTree
|
|||||||
return !result_phantom_node_vector.empty();
|
return !result_phantom_node_vector.empty();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
bool FindPhantomNodeForCoordinate(const FixedPointCoordinate &input_coordinate,
|
bool FindPhantomNodeForCoordinate(const FixedPointCoordinate &input_coordinate,
|
||||||
PhantomNode &result_phantom_node,
|
PhantomNode &result_phantom_node,
|
||||||
const unsigned zoom_level)
|
const unsigned zoom_level)
|
||||||
|
@ -1,5 +1,5 @@
|
|||||||
/*
|
/*
|
||||||
open source routing machine
|
open source routing machine
|
||||||
Copyright (C) Dennis Luxen, others 2010
|
Copyright (C) Dennis Luxen, others 2010
|
||||||
|
|
||||||
This program is free software; you can redistribute it and/or modify
|
This program is free software; you can redistribute it and/or modify
|
||||||
@ -29,6 +29,9 @@ or see http://www.gnu.org/licenses/agpl.txt.
|
|||||||
#include "../routing_algorithms/map_matching.hpp"
|
#include "../routing_algorithms/map_matching.hpp"
|
||||||
#include "../util/simple_logger.hpp"
|
#include "../util/simple_logger.hpp"
|
||||||
#include "../util/string_util.hpp"
|
#include "../util/string_util.hpp"
|
||||||
|
#include "../descriptors/descriptor_base.hpp"
|
||||||
|
#include "../descriptors/gpx_descriptor.hpp"
|
||||||
|
#include "../descriptors/json_descriptor.hpp"
|
||||||
|
|
||||||
#include <cstdlib>
|
#include <cstdlib>
|
||||||
|
|
||||||
@ -40,11 +43,16 @@ or see http://www.gnu.org/licenses/agpl.txt.
|
|||||||
template <class DataFacadeT> class MapMatchingPlugin : public BasePlugin
|
template <class DataFacadeT> class MapMatchingPlugin : public BasePlugin
|
||||||
{
|
{
|
||||||
private:
|
private:
|
||||||
|
std::unordered_map<std::string, unsigned> descriptor_table;
|
||||||
std::shared_ptr<SearchEngine<DataFacadeT>> search_engine_ptr;
|
std::shared_ptr<SearchEngine<DataFacadeT>> search_engine_ptr;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
MapMatchingPlugin(DataFacadeT *facade) : descriptor_string("match"), facade(facade)
|
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);
|
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
|
int HandleRequest(const RouteParameters &route_parameters, JSON::Object &json_result) final
|
||||||
{
|
{
|
||||||
// check number of parameters
|
// check number of parameters
|
||||||
|
|
||||||
SimpleLogger().Write() << "1";
|
|
||||||
if (!check_all_coordinates(route_parameters.coordinates))
|
if (!check_all_coordinates(route_parameters.coordinates))
|
||||||
{
|
{
|
||||||
return 400;
|
return 400;
|
||||||
}
|
}
|
||||||
|
|
||||||
SimpleLogger().Write() << "2";
|
|
||||||
|
|
||||||
InternalRouteResult raw_route;
|
InternalRouteResult raw_route;
|
||||||
Matching::CandidateLists candidate_lists;
|
Matching::CandidateLists candidate_lists;
|
||||||
candidate_lists.resize(route_parameters.coordinates.size());
|
|
||||||
|
|
||||||
SimpleLogger().Write() << "3";
|
double last_distance = coordinate_calculation::great_circle_distance(
|
||||||
// fetch 10 candidates for each given coordinate
|
route_parameters.coordinates[0],
|
||||||
for (const auto current_coordinate : osrm::irange<std::size_t>(0, candidate_lists.size()))
|
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(
|
if (!facade->IncrementalFindPhantomNodeForCoordinateWithDistance(
|
||||||
route_parameters.coordinates[current_coordinate],
|
route_parameters.coordinates[current_coordinate],
|
||||||
candidate_lists[current_coordinate],
|
candidates,
|
||||||
10))
|
last_distance,
|
||||||
|
5,
|
||||||
|
20))
|
||||||
{
|
{
|
||||||
return 400;
|
std::cout << "Nothing found for " << current_coordinate << std::endl;
|
||||||
|
continue;
|
||||||
}
|
}
|
||||||
|
|
||||||
while (candidate_lists[current_coordinate].size() < 10)
|
candidate_lists.push_back(candidates);
|
||||||
{
|
|
||||||
// TODO: add dummy candidates, if any are missing
|
std::cout << current_coordinate << " (" << (last_distance / 2.0) << ") : "
|
||||||
// TODO: add factory method to get an invalid PhantomNode/Distance pair
|
<< candidates.size() << std::endl;
|
||||||
}
|
|
||||||
|
BOOST_ASSERT(candidate_lists[current_coordinate].size() == 10);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (2 > candidate_lists.size())
|
||||||
|
{
|
||||||
|
return 400;
|
||||||
}
|
}
|
||||||
SimpleLogger().Write() << "4";
|
|
||||||
|
|
||||||
// call the actual map matching
|
// 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;
|
return 200;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -31,6 +31,38 @@ or see http://www.gnu.org/licenses/agpl.txt.
|
|||||||
#include <iomanip>
|
#include <iomanip>
|
||||||
#include <numeric>
|
#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
|
namespace Matching
|
||||||
{
|
{
|
||||||
typedef std::vector<std::pair<PhantomNode, double>> CandidateList;
|
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
|
const std::vector<FixedPointCoordinate> coordinate_list) const
|
||||||
{
|
{
|
||||||
std::vector<double> d_t_list, median_select_d_t_list;
|
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],
|
d_t_list.push_back(get_distance_difference(coordinate_list[t - 1],
|
||||||
coordinate_list[t],
|
coordinate_list[t],
|
||||||
@ -142,7 +174,7 @@ template <class DataFacadeT> class MapMatching final
|
|||||||
const PhantomNode &target_phantom) const
|
const PhantomNode &target_phantom) const
|
||||||
{
|
{
|
||||||
// great circle distance of two locations - median/avg dist table(candidate list1/2)
|
// 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 =
|
const auto great_circle_distance =
|
||||||
coordinate_calculation::great_circle_distance(location1, location2);
|
coordinate_calculation::great_circle_distance(location1, location2);
|
||||||
|
|
||||||
@ -153,7 +185,7 @@ template <class DataFacadeT> class MapMatching final
|
|||||||
return network_distance - great_circle_distance;
|
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
|
const PhantomNode &target_phantom) const
|
||||||
{
|
{
|
||||||
EdgeWeight upper_bound = INVALID_EDGE_WEIGHT;
|
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);
|
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:
|
public:
|
||||||
@ -218,97 +274,170 @@ template <class DataFacadeT> class MapMatching final
|
|||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
void operator()(const unsigned state_size,
|
// TODO optimize: a lot of copying that could probably be avoided
|
||||||
const Matching::CandidateLists ×tamp_list,
|
void expandCandidates(const Matching::CandidateLists &candidates_lists,
|
||||||
const std::vector<FixedPointCoordinate> coordinate_list,
|
Matching::CandidateLists &expanded_lists) const
|
||||||
InternalRouteResult &raw_route_data) const
|
|
||||||
{
|
{
|
||||||
BOOST_ASSERT(state_size != std::numeric_limits<unsigned>::max());
|
// expand list of PhantomNodes to be single-directional
|
||||||
BOOST_ASSERT(state_size != 0);
|
expanded_lists.resize(candidates_lists.size());
|
||||||
SimpleLogger().Write() << "matching starts with " << timestamp_list.size() << " locations";
|
for (const auto i : osrm::irange(0lu, candidates_lists.size()))
|
||||||
|
|
||||||
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)
|
|
||||||
{
|
{
|
||||||
SimpleLogger().Write() << "initializing s: " << s << "/" << state_size;
|
for (const auto& candidate : candidates_lists[i])
|
||||||
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;
|
|
||||||
}
|
|
||||||
SimpleLogger().Write() << "b";
|
|
||||||
|
|
||||||
// attention, this call is relatively expensive
|
|
||||||
const auto beta = get_beta(state_size, timestamp_list, coordinate_list);
|
|
||||||
|
|
||||||
for (auto t = 1; t < timestamp_list.size(); ++t)
|
|
||||||
{
|
|
||||||
// compute d_t for this timestamp and the next one
|
|
||||||
for (auto s = 0; s < state_size; ++s)
|
|
||||||
{
|
{
|
||||||
for (auto s_prime = 0; s_prime < state_size; ++s_prime)
|
// bi-directional edge, split phantom node
|
||||||
|
if (candidate.first.forward_node_id != SPECIAL_NODEID && candidate.first.reverse_node_id != SPECIAL_NODEID)
|
||||||
{
|
{
|
||||||
// how likely is candidate s_prime at time t to be emitted?
|
PhantomNode forward_node(candidate.first);
|
||||||
const double emission_pr = emission_probability(timestamp_list[t][s_prime].second);
|
PhantomNode reverse_node(candidate.first);
|
||||||
|
forward_node.reverse_node_id = SPECIAL_NODEID;
|
||||||
// get distance diff between loc1/2 and locs/s_prime
|
reverse_node.forward_node_id = SPECIAL_NODEID;
|
||||||
const auto d_t = get_distance_difference(coordinate_list[t-1],
|
expanded_lists[i].emplace_back(forward_node, candidate.second);
|
||||||
coordinate_list[t],
|
expanded_lists[i].emplace_back(reverse_node, candidate.second);
|
||||||
timestamp_list[t-1][s].first,
|
}
|
||||||
timestamp_list[t][s_prime].first);
|
else
|
||||||
|
{
|
||||||
// plug probabilities together. TODO: change to addition for logprobs
|
expanded_lists[i].push_back(candidate);
|
||||||
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])
|
|
||||||
{
|
|
||||||
viterbi[s_prime][t] = new_value;
|
|
||||||
parent[s_prime][t] = s;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
SimpleLogger().Write() << "c";
|
}
|
||||||
SimpleLogger().Write() << "timestamps: " << timestamp_list.size();
|
|
||||||
const auto number_of_timestamps = timestamp_list.size();
|
void operator()(const Matching::CandidateLists &candidates_lists,
|
||||||
const auto max_element_iter = std::max_element(viterbi[number_of_timestamps].begin(),
|
const std::vector<FixedPointCoordinate> coordinate_list,
|
||||||
viterbi[number_of_timestamps].end());
|
std::vector<PhantomNode>& matched_nodes,
|
||||||
auto parent_index = std::distance(max_element_iter, viterbi[number_of_timestamps].begin());
|
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 = 10.0;
|
||||||
|
|
||||||
|
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 = 0u; s < prev_viterbi.size(); ++s)
|
||||||
|
{
|
||||||
|
|
||||||
|
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 = 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(prev_coordinate,
|
||||||
|
current_coordinate,
|
||||||
|
prev_timestamps_list[s].first,
|
||||||
|
current_timestamps_list[s_prime].first);
|
||||||
|
|
||||||
|
// 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])
|
||||||
|
{
|
||||||
|
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]));
|
||||||
|
}
|
||||||
|
_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;
|
std::deque<std::size_t> reconstructed_indices;
|
||||||
|
|
||||||
SimpleLogger().Write() << "d";
|
for (auto i = timestamp_list.size() - 1u; i > 0u; --i)
|
||||||
|
|
||||||
for (auto i = number_of_timestamps - 1; i > 0; --i)
|
|
||||||
{
|
{
|
||||||
SimpleLogger().Write() << "[" << i << "] parent: " << parent_index ;
|
|
||||||
reconstructed_indices.push_front(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);
|
reconstructed_indices.push_front(parent_index);
|
||||||
|
|
||||||
SimpleLogger().Write() << "e";
|
JSON::Array _debug_chosen_candidates;
|
||||||
|
matched_nodes.resize(reconstructed_indices.size());
|
||||||
for (auto i = 0; i < reconstructed_indices.size(); ++i)
|
for (auto i = 0u; i < reconstructed_indices.size(); ++i)
|
||||||
{
|
{
|
||||||
auto location_index = reconstructed_indices[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);
|
||||||
}
|
}
|
||||||
|
_debug_info.values["chosen_candidates"] = _debug_chosen_candidates;
|
||||||
SimpleLogger().Write() << "f, done";
|
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;
|
||||||
}
|
}
|
||||||
};
|
};
|
||||||
|
|
||||||
|
@ -108,7 +108,9 @@ template <class EdgeDataT> class BaseDataFacade
|
|||||||
virtual bool
|
virtual bool
|
||||||
IncrementalFindPhantomNodeForCoordinateWithDistance(const FixedPointCoordinate &input_coordinate,
|
IncrementalFindPhantomNodeForCoordinateWithDistance(const FixedPointCoordinate &input_coordinate,
|
||||||
std::vector<std::pair<PhantomNode, double>> &resulting_phantom_node_vector,
|
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;
|
virtual unsigned GetCheckSum() const = 0;
|
||||||
|
|
||||||
|
@ -419,7 +419,9 @@ template <class EdgeDataT> class InternalDataFacade final : public BaseDataFacad
|
|||||||
bool
|
bool
|
||||||
IncrementalFindPhantomNodeForCoordinateWithDistance(const FixedPointCoordinate &input_coordinate,
|
IncrementalFindPhantomNodeForCoordinateWithDistance(const FixedPointCoordinate &input_coordinate,
|
||||||
std::vector<std::pair<PhantomNode, double>> &resulting_phantom_node_vector,
|
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())
|
if (!m_static_rtree.get())
|
||||||
{
|
{
|
||||||
@ -427,7 +429,7 @@ template <class EdgeDataT> class InternalDataFacade final : public BaseDataFacad
|
|||||||
}
|
}
|
||||||
|
|
||||||
return m_static_rtree->IncrementalFindPhantomNodeForCoordinateWithDistance(
|
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; }
|
unsigned GetCheckSum() const override final { return m_check_sum; }
|
||||||
|
@ -407,7 +407,9 @@ template <class EdgeDataT> class SharedDataFacade final : public BaseDataFacade<
|
|||||||
bool
|
bool
|
||||||
IncrementalFindPhantomNodeForCoordinateWithDistance(const FixedPointCoordinate &input_coordinate,
|
IncrementalFindPhantomNodeForCoordinateWithDistance(const FixedPointCoordinate &input_coordinate,
|
||||||
std::vector<std::pair<PhantomNode, double>> &resulting_phantom_node_vector,
|
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)
|
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(
|
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; }
|
unsigned GetCheckSum() const override final { return m_check_sum; }
|
||||||
|
Loading…
Reference in New Issue
Block a user