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

Add skeleton code for matching

Browse Source
This commit is contained in:
Dennis Luxen 2014-09-23 18:46:14 +02:00 committed by Patrick Niklaus
parent 7e00a86bb4
commit 2259bce05f
8 changed files with 552 additions and 90 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
data_structures/search_engine.hpp
View File

@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "search_engine_data.hpp"
#include "../routing_algorithms/alternative_path.hpp"
#include "../routing_algorithms/many_to_many.hpp"
#include "../routing_algorithms/map_matching.hpp"
#include "../routing_algorithms/shortest_path.hpp"
#include <type_traits>
@ -45,10 +46,14 @@ template <class DataFacadeT> class SearchEngine
ShortestPathRouting<DataFacadeT> shortest_path;
AlternativeRouting<DataFacadeT> alternative_path;
ManyToManyRouting<DataFacadeT> distance_table;
MapMatching<DataFacadeT> map_matching;
explicit SearchEngine(DataFacadeT *facade)
: facade(facade), shortest_path(facade, engine_working_data),
alternative_path(facade, engine_working_data), distance_table(facade, engine_working_data)
: facade(facade),
shortest_path(facade, engine_working_data),
alternative_path(facade, engine_working_data),
distance_table(facade, engine_working_data),
map_matching(facade, engine_working_data)
{
static_assert(!std::is_pointer<DataFacadeT>::value, "don't instantiate with ptr type");
static_assert(std::is_object<DataFacadeT>::value,

172
data_structures/static_rtree.hpp
View File

@ -745,6 +745,7 @@ class StaticRTree
// check if it is smaller than what we had before
float current_ratio = 0.f;
FixedPointCoordinate foot_point_coordinate_on_segment;
// const float current_perpendicular_distance =
coordinate_calculation::perpendicular_distance_from_projected_coordinate(
m_coordinate_list->at(current_segment.u),
@ -796,20 +797,24 @@ class StaticRTree
return !result_phantom_node_vector.empty();
}
// implementation of the Hjaltason/Samet query [3], a BFS traversal of the tree
bool IncrementalFindPhantomNodeForCoordinateWithDistance(
const FixedPointCoordinate &input_coordinate,
std::vector<std::pair<PhantomNode, double>> &result_phantom_node_vector,
const unsigned number_of_results,
const unsigned max_checked_segments = 4 * LEAF_NODE_SIZE)
const unsigned max_number_of_phantom_nodes,
const unsigned max_checked_elements = 4 * LEAF_NODE_SIZE)
{
std::vector<float> min_found_distances(number_of_results,
std::numeric_limits<float>::max());
unsigned inspected_elements = 0;
unsigned number_of_elements_from_big_cc = 0;
unsigned number_of_elements_from_tiny_cc = 0;
unsigned number_of_results_found_in_big_cc = 0;
unsigned number_of_results_found_in_tiny_cc = 0;
unsigned pruned_elements = 0;
unsigned inspected_segments = 0;
std::pair<double, double> projected_coordinate = {
mercator::lat2y(input_coordinate.lat / COORDINATE_PRECISION),
input_coordinate.lon / COORDINATE_PRECISION};
// upper bound pruning technique
upper_bound<float> pruning_bound(max_number_of_phantom_nodes);
// initialize queue with root element
std::priority_queue<IncrementalQueryCandidate> traversal_queue;
@ -820,42 +825,43 @@ class StaticRTree
const IncrementalQueryCandidate current_query_node = traversal_queue.top();
traversal_queue.pop();
const float current_min_dist = min_found_distances[number_of_results - 1];
if (current_query_node.min_dist > current_min_dist)
{
continue;
}
if (current_query_node.RepresentsTreeNode())
{
if (current_query_node.node.template is<TreeNode>())
{ // current object is a tree node
const TreeNode &current_tree_node =
current_query_node.node.template get<TreeNode>();
if (current_tree_node.child_is_on_disk)
{
LeafNode current_leaf_node;
LoadLeafFromDisk(current_tree_node.children[0], current_leaf_node);
// Add all objects from leaf into queue
for (uint32_t i = 0; i < current_leaf_node.object_count; ++i)
// current object represents a block on disk
for (const auto i : osrm::irange(0u, current_leaf_node.object_count))
{
const auto &current_edge = current_leaf_node.objects[i];
const float current_perpendicular_distance =
coordinate_calculation::perpendicular_distance(
const float current_perpendicular_distance = coordinate_calculation::
perpendicular_distance_from_projected_coordinate(
m_coordinate_list->at(current_edge.u),
m_coordinate_list->at(current_edge.v), input_coordinate);
m_coordinate_list->at(current_edge.v), input_coordinate,
projected_coordinate);
// distance must be non-negative
BOOST_ASSERT(0. <= current_perpendicular_distance);
BOOST_ASSERT(0.f <= current_perpendicular_distance);
if (current_perpendicular_distance < current_min_dist)
if (pruning_bound.get() >= current_perpendicular_distance ||
current_edge.is_in_tiny_cc())
{
pruning_bound.insert(current_perpendicular_distance);
traversal_queue.emplace(current_perpendicular_distance, current_edge);
}
else
{
++pruned_elements;
}
}
}
else
{
// for each child mbr
for (uint32_t i = 0; i < current_tree_node.child_count; ++i)
// for each child mbr get a lower bound and enqueue it
for (const auto i : osrm::irange(0u, current_tree_node.child_count))
{
const int32_t child_id = current_tree_node.children[i];
const TreeNode &child_tree_node = m_search_tree[child_id];
@ -863,99 +869,89 @@ class StaticRTree
child_tree_node.minimum_bounding_rectangle;
const float lower_bound_to_element =
child_rectangle.GetMinDist(input_coordinate);
BOOST_ASSERT(0.f <= lower_bound_to_element);
// TODO - enough elements found, i.e. nearest distance > maximum distance?
// ie. some measure of 'confidence of accuracy'
// check if it needs to be explored by mindist
if (lower_bound_to_element < current_min_dist)
{
traversal_queue.emplace(lower_bound_to_element, child_tree_node);
}
traversal_queue.emplace(lower_bound_to_element, child_tree_node);
}
// SimpleLogger().Write(logDEBUG) << "added " << current_tree_node.child_count
// << " mbrs into queue of " << traversal_queue.size();
}
}
else
{
++inspected_segments;
{ // current object is a leaf node
++inspected_elements;
// inspecting an actual road segment
const EdgeDataT &current_segment =
current_query_node.node.template get<EdgeDataT>();
// don't collect too many results from small components
if (number_of_results_found_in_big_cc == number_of_results &&
!current_segment.is_in_tiny_cc)
{
continue;
}
// don't collect too many results from big components
if (number_of_results_found_in_tiny_cc == number_of_results &&
current_segment.is_in_tiny_cc)
// 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 &&
current_segment.is_in_tiny_cc())
{
continue;
}
// check if it is smaller than what we had before
float current_ratio = 0.;
float current_ratio = 0.f;
FixedPointCoordinate foot_point_coordinate_on_segment;
const float current_perpendicular_distance =
coordinate_calculation::perpendicular_distance(
m_coordinate_list->at(current_segment.u),
m_coordinate_list->at(current_segment.v), input_coordinate,
foot_point_coordinate_on_segment, current_ratio);
coordinate_calculation::perpendicular_distance_from_projected_coordinate(
m_coordinate_list->at(current_segment.u),
m_coordinate_list->at(current_segment.v), input_coordinate,
projected_coordinate, foot_point_coordinate_on_segment, current_ratio);
BOOST_ASSERT(0. <= current_perpendicular_distance);
// store phantom node in result vector
result_phantom_node_vector.emplace_back(
PhantomNode( current_segment.forward_edge_based_node_id,
current_segment.reverse_edge_based_node_id, current_segment.name_id,
current_segment.forward_weight, current_segment.reverse_weight,
current_segment.forward_offset, current_segment.reverse_offset,
current_segment.packed_geometry_id, current_segment.component_id,
foot_point_coordinate_on_segment, current_segment.fwd_segment_position,
current_segment.forward_travel_mode, current_segment.backward_travel_mode),
current_perpendicular_distance);
if ((current_perpendicular_distance < current_min_dist) &&
!osrm::epsilon_compare(current_perpendicular_distance, current_min_dist))
{
// store phantom node in result vector
result_phantom_node_vector.emplace_back(current_segment,
foot_point_coordinate_on_segment,
current_perpendicular_distance);
// Hack to fix rounding errors and wandering via nodes.
FixUpRoundingIssue(input_coordinate, result_phantom_node_vector.back().first);
// Hack to fix rounding errors and wandering via nodes.
FixUpRoundingIssue(input_coordinate, result_phantom_node_vector.back());
// set forward and reverse weights on the phantom node
SetForwardAndReverseWeightsOnPhantomNode(current_segment,
result_phantom_node_vector.back().first);
// set forward and reverse weights on the phantom node
SetForwardAndReverseWeightsOnPhantomNode(current_segment,
result_phantom_node_vector.back());
// do we have results only in a small scc
if (current_segment.is_in_tiny_cc)
{
++number_of_results_found_in_tiny_cc;
}
else
{
// found an element in a large component
min_found_distances[number_of_results_found_in_big_cc] =
current_perpendicular_distance;
++number_of_results_found_in_big_cc;
// SimpleLogger().Write(logDEBUG) << std::setprecision(8) <<
// foot_point_coordinate_on_segment << " at " <<
// current_perpendicular_distance;
}
// update counts on what we found from which result class
if (current_segment.is_in_tiny_cc())
{ // found an element in tiny component
++number_of_elements_from_tiny_cc;
}
else
{ // found an element in a big component
++number_of_elements_from_big_cc;
}
}
// TODO add indicator to prune if maxdist > threshold
if (number_of_results == number_of_results_found_in_big_cc ||
inspected_segments >= max_checked_segments)
// stop the search by flushing the queue
if ((result_phantom_node_vector.size() >= max_number_of_phantom_nodes &&
number_of_elements_from_big_cc > 0) ||
inspected_elements >= max_checked_elements)
{
// SimpleLogger().Write(logDEBUG) << "flushing queue of " << traversal_queue.size()
// << " elements";
// work-around for traversal_queue.clear();
traversal_queue = std::priority_queue<IncrementalQueryCandidate>{};
}
}
// SimpleLogger().Write() << "result_phantom_node_vector.size(): " <<
// result_phantom_node_vector.size();
// SimpleLogger().Write() << "max_number_of_phantom_nodes: " << max_number_of_phantom_nodes;
// SimpleLogger().Write() << "number_of_elements_from_big_cc: " <<
// number_of_elements_from_big_cc;
// SimpleLogger().Write() << "number_of_elements_from_tiny_cc: " <<
// number_of_elements_from_tiny_cc;
// SimpleLogger().Write() << "inspected_elements: " << inspected_elements;
// SimpleLogger().Write() << "max_checked_elements: " << max_checked_elements;
// SimpleLogger().Write() << "pruned_elements: " << pruned_elements;
return !result_phantom_node_vector.empty();
}
bool FindPhantomNodeForCoordinate(const FixedPointCoordinate &input_coordinate,
PhantomNode &result_phantom_node,
const unsigned zoom_level)

2
library/osrm_impl.cpp
View File

@ -42,6 +42,7 @@ class named_mutex;
#include "../plugins/nearest.hpp"
#include "../plugins/timestamp.hpp"
#include "../plugins/viaroute.hpp"
#include "../plugins/map_matching.hpp"
#include "../server/data_structures/datafacade_base.hpp"
#include "../server/data_structures/internal_datafacade.hpp"
#include "../server/data_structures/shared_barriers.hpp"
@ -81,6 +82,7 @@ OSRM_impl::OSRM_impl(libosrm_config &lib_config)
RegisterPlugin(new HelloWorldPlugin());
RegisterPlugin(new LocatePlugin<BaseDataFacade<QueryEdge::EdgeData>>(query_data_facade));
RegisterPlugin(new NearestPlugin<BaseDataFacade<QueryEdge::EdgeData>>(query_data_facade));
RegisterPlugin(new MapMatchingPlugin<BaseDataFacade<QueryEdge::EdgeData>>(query_data_facade));
RegisterPlugin(new TimestampPlugin<BaseDataFacade<QueryEdge::EdgeData>>(query_data_facade));
RegisterPlugin(new ViaRoutePlugin<BaseDataFacade<QueryEdge::EdgeData>>(query_data_facade));
}

107
plugins/map_matching.hpp Normal file
View File

@ -0,0 +1,107 @@
/*
open source routing machine
Copyright (C) Dennis Luxen, others 2010
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU AFFERO General Public License as published by
the Free Software Foundation; either version 3 of the License, or
any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU Affero General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
or see http://www.gnu.org/licenses/agpl.txt.
*/
#ifndef MAP_MATCHING_PLUGIN_H
#define MAP_MATCHING_PLUGIN_H
#include "plugin_base.hpp"
#include "../algorithms/object_encoder.hpp"
#include "../util/integer_range.hpp"
#include "../data_structures/search_engine.hpp"
#include "../routing_algorithms/map_matching.hpp"
#include "../util/simple_logger.hpp"
#include "../util/string_util.hpp"
#include <cstdlib>
#include <algorithm>
#include <memory>
#include <string>
#include <vector>
template <class DataFacadeT> class MapMatchingPlugin : public BasePlugin
{
private:
std::shared_ptr<SearchEngine<DataFacadeT>> search_engine_ptr;
public:
MapMatchingPlugin(DataFacadeT *facade) : descriptor_string("match"), facade(facade)
{
search_engine_ptr = std::make_shared<SearchEngine<DataFacadeT>>(facade);
}
virtual ~MapMatchingPlugin() { search_engine_ptr.reset(); }
const std::string GetDescriptor() const final { return descriptor_string; }
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()))
{
if (!facade->IncrementalFindPhantomNodeForCoordinateWithDistance(
route_parameters.coordinates[current_coordinate],
candidate_lists[current_coordinate],
10))
{
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);
if (INVALID_EDGE_WEIGHT == raw_route.shortest_path_length)
{
SimpleLogger().Write(logDEBUG) << "Error occurred, single path not found";
}
return 200;
}
private:
std::string descriptor_string;
DataFacadeT *facade;
};
#endif /* MAP_MATCHING_PLUGIN_H */

317
routing_algorithms/map_matching.hpp Normal file
View File

@ -0,0 +1,317 @@
/*
open source routing machine
Copyright (C) Dennis Luxen, others 2010
This program is free software; you can redistribute it and/or modify
it under the terms of the GNU AFFERO General Public License as published by
the Free Software Foundation; either version 3 of the License, or
any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU Affero General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
or see http://www.gnu.org/licenses/agpl.txt.
*/
#ifndef MAP_MATCHING_H
#define MAP_MATCHING_H
#include "routing_base.hpp"
#include "../data_structures/coordinate_calculation.hpp"
#include "../util/simple_logger.hpp"
#include "../util/container.hpp"
#include <algorithm>
#include <iomanip>
#include <numeric>
namespace Matching
{
typedef std::vector<std::pair<PhantomNode, double>> CandidateList;
typedef std::vector<CandidateList> CandidateLists;
typedef std::pair<PhantomNodes, double> PhantomNodesWithProbability;
}
// implements a hidden markov model map matching algorithm
template <class DataFacadeT> class MapMatching final
: public BasicRoutingInterface<DataFacadeT, MapMatching<DataFacadeT>>
{
using super = BasicRoutingInterface<DataFacadeT, MapMatching<DataFacadeT>>;
using QueryHeap = SearchEngineData::QueryHeap;
SearchEngineData &engine_working_data;
constexpr static const double sigma_z = 4.07;
constexpr double emission_probability(const double distance) const
{
return (1. / (std::sqrt(2. * M_PI) * sigma_z)) *
std::exp(-0.5 * std::pow((distance / sigma_z), 2.));
}
constexpr double log_probability(const double probability) const
{
return std::log2(probability);
}
// TODO: needs to be estimated from the input locations
//constexpr static const double beta = 1.;
// samples/min and beta
// 1 0.49037673
// 2 0.82918373
// 3 1.24364564
// 4 1.67079581
// 5 2.00719298
// 6 2.42513007
// 7 2.81248831
// 8 3.15745473
// 9 3.52645392
// 10 4.09511775
// 11 4.67319795
// 21 12.55107715
// 12 5.41088180
// 13 6.47666590
// 14 6.29010734
// 15 7.80752112
// 16 8.09074504
// 17 8.08550528
// 18 9.09405065
// 19 11.09090603
// 20 11.87752824
// 21 12.55107715
// 22 15.82820829
// 23 17.69496773
// 24 18.07655652
// 25 19.63438911
// 26 25.40832185
// 27 23.76001877
// 28 28.43289797
// 29 32.21683062
// 30 34.56991141
constexpr double transition_probability(const float d_t, const float beta) const
{
return (1. / beta) * std::exp(-d_t / beta);
}
// deprecated
// translates a distance into how likely it is an input
// double DistanceToProbability(const double distance) const
// {
// if (0. > distance)
// {
// return 0.;
// }
// return 1. - 1. / (1. + exp((-distance + 35.) / 6.));
// }
double get_beta(const unsigned state_size,
const Matching::CandidateLists &timestamp_list,
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 s = 0; s < state_size; ++s)
{
d_t_list.push_back(get_distance_difference(coordinate_list[t - 1],
coordinate_list[t],
timestamp_list[t - 1][s].first,
timestamp_list[t][s].first));
median_select_d_t_list.push_back(d_t_list.back());
}
}
std::nth_element(median_select_d_t_list.begin(),
median_select_d_t_list.begin() + median_select_d_t_list.size() / 2,
median_select_d_t_list.end());
const auto median_d_t = median_select_d_t_list[median_select_d_t_list.size() / 2];
return (1. / std::log(2)) * median_d_t;
}
double get_distance_difference(const FixedPointCoordinate &location1,
const FixedPointCoordinate &location2,
const PhantomNode &source_phantom,
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 great_circle_distance =
coordinate_calculation::great_circle_distance(location1, location2);
if (great_circle_distance > network_distance)
{
return great_circle_distance - network_distance;
}
return network_distance - great_circle_distance;
}
EdgeWeight get_network_distance(const PhantomNode &source_phantom,
const PhantomNode &target_phantom) const
{
EdgeWeight upper_bound = INVALID_EDGE_WEIGHT;
NodeID middle_node = SPECIAL_NODEID;
EdgeWeight edge_offset = std::min(0, -source_phantom.GetForwardWeightPlusOffset());
edge_offset = std::min(edge_offset, -source_phantom.GetReverseWeightPlusOffset());
engine_working_data.InitializeOrClearFirstThreadLocalStorage(
super::facade->GetNumberOfNodes());
engine_working_data.InitializeOrClearSecondThreadLocalStorage(
super::facade->GetNumberOfNodes());
QueryHeap &forward_heap = *(engine_working_data.forward_heap_1);
QueryHeap &reverse_heap = *(engine_working_data.reverse_heap_1);
if (source_phantom.forward_node_id != SPECIAL_NODEID)
{
forward_heap.Insert(source_phantom.forward_node_id,
-source_phantom.GetForwardWeightPlusOffset(),
source_phantom.forward_node_id);
}
if (source_phantom.reverse_node_id != SPECIAL_NODEID)
{
forward_heap.Insert(source_phantom.reverse_node_id,
-source_phantom.GetReverseWeightPlusOffset(),
source_phantom.reverse_node_id);
}
if (target_phantom.forward_node_id != SPECIAL_NODEID)
{
reverse_heap.Insert(target_phantom.forward_node_id,
target_phantom.GetForwardWeightPlusOffset(),
target_phantom.forward_node_id);
}
if (target_phantom.reverse_node_id != SPECIAL_NODEID)
{
reverse_heap.Insert(target_phantom.reverse_node_id,
target_phantom.GetReverseWeightPlusOffset(),
target_phantom.reverse_node_id);
}
// search from s and t till new_min/(1+epsilon) > length_of_shortest_path
while (0 < (forward_heap.Size() + reverse_heap.Size()))
{
if (0 < forward_heap.Size())
{
super::RoutingStep(
forward_heap, reverse_heap, &middle_node, &upper_bound, edge_offset, true);
}
if (0 < reverse_heap.Size())
{
super::RoutingStep(
reverse_heap, forward_heap, &middle_node, &upper_bound, edge_offset, false);
}
}
return upper_bound;
}
public:
MapMatching(DataFacadeT *facade, SearchEngineData &engine_working_data)
: super(facade), engine_working_data(engine_working_data)
{
}
void operator()(const unsigned state_size,
const Matching::CandidateLists &timestamp_list,
const std::vector<FixedPointCoordinate> coordinate_list,
InternalRouteResult &raw_route_data) 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)
{
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;
}
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)
{
// how likely is candidate s_prime at time t to be emitted?
const double emission_pr = 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);
// 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])
{
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();
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());
std::deque<std::size_t> reconstructed_indices;
SimpleLogger().Write() << "d";
for (auto i = number_of_timestamps - 1; i > 0; --i)
{
SimpleLogger().Write() << "[" << i << "] parent: " << parent_index ;
reconstructed_indices.push_front(parent_index);
parent_index = parent[parent_index][i];
}
SimpleLogger().Write() << "[0] parent: " << parent_index;
reconstructed_indices.push_front(parent_index);
SimpleLogger().Write() << "e";
for (auto i = 0; 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;
}
SimpleLogger().Write() << "f, done";
}
};
//[1] "Hidden Markov Map Matching Through Noise and Sparseness"; P. Newson and J. Krumm; 2009; ACM GIS
#endif /* MAP_MATCHING_H */

5
server/data_structures/datafacade_base.hpp
View File

@ -105,6 +105,11 @@ template <class EdgeDataT> class BaseDataFacade
IncrementalFindPhantomNodeForCoordinate(const FixedPointCoordinate &input_coordinate,
PhantomNode &resulting_phantom_node) = 0;
virtual bool
IncrementalFindPhantomNodeForCoordinateWithDistance(const FixedPointCoordinate &input_coordinate,
std::vector<std::pair<PhantomNode, double>> &resulting_phantom_node_vector,
const unsigned number_of_results) = 0;
virtual unsigned GetCheckSum() const = 0;
virtual unsigned GetNameIndexFromEdgeID(const unsigned id) const = 0;

15
server/data_structures/internal_datafacade.hpp
View File

@ -398,6 +398,7 @@ template <class EdgeDataT> class InternalDataFacade final : public BaseDataFacad
BOOST_ASSERT(!resulting_phantom_node_vector.empty());
resulting_phantom_node = resulting_phantom_node_vector.front();
}
return result;
}
@ -415,6 +416,20 @@ template <class EdgeDataT> class InternalDataFacade final : public BaseDataFacad
input_coordinate, resulting_phantom_node_vector, number_of_results);
}
bool
IncrementalFindPhantomNodeForCoordinateWithDistance(const FixedPointCoordinate &input_coordinate,
std::vector<std::pair<PhantomNode, double>> &resulting_phantom_node_vector,
const unsigned number_of_results) final
{
if (!m_static_rtree.get())
{
LoadRTree();
}
return m_static_rtree->IncrementalFindPhantomNodeForCoordinateWithDistance(
input_coordinate, resulting_phantom_node_vector, number_of_results);
}
unsigned GetCheckSum() const override final { return m_check_sum; }
unsigned GetNameIndexFromEdgeID(const unsigned id) const override final

15
server/data_structures/shared_datafacade.hpp
View File

@ -386,6 +386,7 @@ template <class EdgeDataT> class SharedDataFacade final : public BaseDataFacade<
BOOST_ASSERT(!resulting_phantom_node_vector.empty());
resulting_phantom_node = resulting_phantom_node_vector.front();
}
return result;
}
@ -403,6 +404,20 @@ template <class EdgeDataT> class SharedDataFacade final : public BaseDataFacade<
input_coordinate, resulting_phantom_node_vector, number_of_results);
}
bool
IncrementalFindPhantomNodeForCoordinateWithDistance(const FixedPointCoordinate &input_coordinate,
std::vector<std::pair<PhantomNode, double>> &resulting_phantom_node_vector,
const unsigned number_of_results) final
{
if (!m_static_rtree.get() || CURRENT_TIMESTAMP != m_static_rtree->first)
{
LoadRTree();
}
return m_static_rtree->second->IncrementalFindPhantomNodeForCoordinateWithDistance(
input_coordinate, resulting_phantom_node_vector, number_of_results);
}
unsigned GetCheckSum() const override final { return m_check_sum; }
unsigned GetNameIndexFromEdgeID(const unsigned id) const override final