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

wip

Browse Source
This commit is contained in:
Siarhei Fedartsou 2024-07-28 18:55:57 +02:00
parent 2b38c936d5
commit 4a34c86544
6 changed files with 228 additions and 67 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
include/engine/routing_algorithms/routing_base_ch.hpp
View File

@ -464,7 +464,7 @@ inline std::vector<double>
getNetworkDistances(SearchEngineData<Algorithm> &,
const DataFacade<ch::Algorithm> &,
SearchEngineData<Algorithm>::QueryHeap &,
SearchEngineData<Algorithm>::QueryHeap &,
const std::vector<std::unique_ptr<typename SearchEngineData<Algorithm>::QueryHeap>> &,
const PhantomNode &,
const std::vector<PhantomNode> &,
EdgeWeight /*duration_upper_bound*/ = INVALID_EDGE_WEIGHT)

107
include/engine/routing_algorithms/routing_base_mld.hpp
View File

@ -38,10 +38,13 @@ inline LevelID getNodeQueryLevel(const MultiLevelPartition &partition,
return INVALID_LEVEL_ID;
};
return std::min(std::min(level(source.forward_segment_id, target.forward_segment_id),
auto res = std::min(std::min(level(source.forward_segment_id, target.forward_segment_id),
level(source.forward_segment_id, target.reverse_segment_id)),
std::min(level(source.reverse_segment_id, target.forward_segment_id),
level(source.reverse_segment_id, target.reverse_segment_id)));
// std::cerr << "OLD!!! " << (int)res << std::endl;
return res;
}
template <typename MultiLevelPartition>
@ -92,6 +95,7 @@ inline LevelID getNodeQueryLevel(const MultiLevelPartition &partition,
getNodeQueryLevel(partition, node, source, target));
}));
});
// std::cerr << "NEW " << (int)min_level << std::endl;
return min_level;
}
@ -140,7 +144,10 @@ inline LevelID getNodeQueryLevel(const MultiLevelPartition &partition,
highest_different_level(phantom_node.reverse_segment_id));
return std::min(current_level, highest_level);
});
return node_level;
// std::cerr << "NEW!!! " << (int)node_level << std::endl;
return node_level;
}
// Unrestricted search with a single phantom node and a vector of phantom nodes:
@ -300,7 +307,6 @@ void relaxOutgoingEdges(const DataFacade<Algorithm> &facade,
const auto &metric = facade.GetCellMetric();
const auto level = getNodeQueryLevel(partition, heapNode.node, args...);
static constexpr auto IS_MAP_MATCHING =
std::is_same_v<typename SearchEngineData<mld::Algorithm>::MapMatchingQueryHeap, Heap>;
@ -457,6 +463,12 @@ void routingStep(const DataFacade<Algorithm> &facade,
BOOST_ASSERT(!facade.ExcludeNode(heapNode.node));
if (DIRECTION == FORWARD_DIRECTION) {
// std::cerr << "FORWARDO " << heapNode.node << std::endl;
} else {
//std::cerr << "REVERSEO " << heapNode.node << std::endl;
}
// Upper bound for the path source -> target with
// weight(source -> node) = weight weight(to -> target) ≤ reverse_weight
// is weight + reverse_weight
@ -644,6 +656,7 @@ searchDistance(SearchEngineData<Algorithm> &,
auto [middle, _] = *searchResult;
// std::cerr << "old " << middle << std::endl;
auto distance = forward_heap.GetData(middle).distance + reverse_heap.GetData(middle).distance;
return distance;
@ -796,14 +809,15 @@ auto routingStep2(const DataFacade<Algorithm> &facade, Heap &forward_heap, const
return heapNode;
}
template <typename Algorithm, typename Heap, typename... Args>
template <typename Algorithm, typename Heap>
std::vector<std::optional<std::pair<NodeID, EdgeWeight>>>
runSearch2(const DataFacade<Algorithm> &facade,
Heap &forward_heap,
const std::vector<std::unique_ptr<Heap>> &reverse_heap,
size_t candidatesCount,
const std::vector<NodeID> &force_step_nodes,
EdgeWeight weight_upper_bound,
const Args &...args)
const PhantomEndpointCandidates& candidates)
{
// if (forward_heap.Empty() || reverse_heap.Empty())
// {
@ -816,23 +830,23 @@ runSearch2(const DataFacade<Algorithm> &facade,
std::vector<NodeID> middles;
std::vector<EdgeWeight> weights;
middles.resize(reverse_heap.size(), SPECIAL_NODEID);
weights.resize(reverse_heap.size(), weight_upper_bound);
middles.resize(candidatesCount, SPECIAL_NODEID);
weights.resize(candidatesCount, weight_upper_bound);
// run two-Target Dijkstra routing step.
EdgeWeight forward_heap_min = forward_heap.MinKey();
std::vector<EdgeWeight> reverse_heap_mins;
for (const auto &heap : reverse_heap)
for (size_t i = 0; i < candidatesCount; ++i)
{
reverse_heap_mins.push_back(heap->MinKey());
reverse_heap_mins.push_back(reverse_heap[i]->MinKey());
}
auto shouldContinue = [&]()
{
bool cont = false;
for (size_t i = 0; i < reverse_heap.size(); ++i)
for (size_t i = 0; i < candidatesCount; ++i)
{
if (reverse_heap_mins[i] < weights[i])
if ((forward_heap_min + reverse_heap_mins[i]) < weights[i])
{
cont = true;
break;
@ -851,9 +865,11 @@ runSearch2(const DataFacade<Algorithm> &facade,
{
if (!forward_heap.Empty())
{
auto heapNode = routingStep2<FORWARD_DIRECTION>(facade, forward_heap, args...);
const auto heapNode = forward_heap.DeleteMinGetHeapNode();
// std::cerr << "FORWARDN " << heapNode.node << std::endl;
//auto heapNode = routingStep2<FORWARD_DIRECTION>(facade, forward_heap, args...);
for (size_t i = 0; i < reverse_heap.size(); ++i)
for (size_t i = 0; i < candidatesCount; ++i)
{
auto &rh = reverse_heap[i];
const auto reverseHeapNode = rh->GetHeapNodeIfWasInserted(heapNode.node);
@ -867,18 +883,27 @@ runSearch2(const DataFacade<Algorithm> &facade,
{
middles[i] = heapNode.node;
weights[i] = path_weight;
// auto distance =
// forward_heap.GetData(middles[i]).distance + reverse_heap[i]->GetData(middles[i]).distance;
// std::cerr << "RFOUNDN " << i <<" " << distance << std::endl;
}
}
}
relaxOutgoingEdges<FORWARD_DIRECTION>(facade, forward_heap, heapNode, candidates);
if (!forward_heap.Empty())
forward_heap_min = forward_heap.MinKey();
}
for (size_t i = 0; i < reverse_heap.size(); ++i)
for (size_t i = 0; i < candidatesCount; ++i)
{
if (!reverse_heap[i]->Empty())
if (!reverse_heap[i]->Empty() && (forward_heap_min + reverse_heap_mins[i]) < weights[i])
{
auto heapNode = routingStep2<REVERSE_DIRECTION>(facade, *reverse_heap[i], args...);
const auto heapNode = reverse_heap[i]->DeleteMinGetHeapNode();
//std::cerr << "REVERSEN " << i << " " << heapNode.node << std::endl;
const auto reverseHeapNode = forward_heap.GetHeapNodeIfWasInserted(heapNode.node);
if (reverseHeapNode)
{
@ -888,10 +913,18 @@ runSearch2(const DataFacade<Algorithm> &facade,
if (!shouldForceStep(force_step_nodes, heapNode, *reverseHeapNode) &&
(path_weight >= EdgeWeight{0}) && (path_weight < weights[i]))
{
middles[i] = heapNode.node;
weights[i] = path_weight;
// auto distance =
// forward_heap.GetData(middles[i]).distance + reverse_heap[i]->GetData(middles[i]).distance;
// std::cerr << "FFOUNDN " << i << " " << distance << std::endl;
}
}
relaxOutgoingEdges<REVERSE_DIRECTION>(facade, *reverse_heap[i], heapNode, candidates);
if (!reverse_heap[i]->Empty())
reverse_heap_mins[i] = reverse_heap[i]->MinKey();
}
@ -899,7 +932,7 @@ runSearch2(const DataFacade<Algorithm> &facade,
};
std::vector<std::optional<std::pair<NodeID, EdgeWeight>>> results;
for (size_t i = 0; i < reverse_heap.size(); ++i)
for (size_t i = 0; i < candidatesCount; ++i)
{
if (weights[i] >= weight_upper_bound || SPECIAL_NODEID == middles[i])
{
@ -945,19 +978,20 @@ runSearch2(const DataFacade<Algorithm> &facade,
// return {{middle, weight}};
}
template <typename Algorithm, typename... Args>
template <typename Algorithm>
std::vector<EdgeDistance> searchDistance2(
SearchEngineData<Algorithm> &,
const DataFacade<Algorithm> &facade,
typename SearchEngineData<Algorithm>::MapMatchingQueryHeap &forward_heap,
const std::vector<std::unique_ptr<typename SearchEngineData<Algorithm>::MapMatchingQueryHeap>>
&reverse_heaps,
size_t candidatesCount,
const std::vector<NodeID> &force_step_nodes,
EdgeWeight weight_upper_bound,
const Args &...args)
const PhantomEndpointCandidates& candidates)
{
auto searchResults = runSearch2(
facade, forward_heap, reverse_heaps, force_step_nodes, weight_upper_bound, args...);
facade, forward_heap, reverse_heaps, candidatesCount, force_step_nodes, weight_upper_bound, candidates);
std::vector<EdgeDistance> res;
for (size_t i = 0; i < searchResults.size(); ++i)
{
@ -968,6 +1002,9 @@ std::vector<EdgeDistance> searchDistance2(
else
{
auto [middle, _] = *searchResults[i];
//std::cerr << "new " << i << " " << middle << std::endl;
auto distance =
forward_heap.GetData(middle).distance + reverse_heaps[i]->GetData(middle).distance;
res.push_back(distance);
@ -980,21 +1017,24 @@ std::vector<double>
getNetworkDistances(SearchEngineData<Algorithm> &engine_working_data,
const DataFacade<Algorithm> &facade,
typename SearchEngineData<Algorithm>::MapMatchingQueryHeap &forward_heap,
typename SearchEngineData<Algorithm>::MapMatchingQueryHeap &,
const std::vector<std::unique_ptr<typename SearchEngineData<Algorithm>::MapMatchingQueryHeap>> &reverse_heaps,
const PhantomNode &source_phantom,
const std::vector<PhantomNode> &target_phantoms,
EdgeWeight weight_upper_bound = INVALID_EDGE_WEIGHT)
{
using Heap = typename SearchEngineData<Algorithm>::MapMatchingQueryHeap;
forward_heap.Clear();
std::vector<std::unique_ptr<Heap>> reverse_heaps;
const auto nodes_number = facade.GetNumberOfNodes();
const auto border_nodes_number = facade.GetMaxBorderNodeID() + 1;
for (const auto &target_phantom : target_phantoms)
for (const auto &heap : reverse_heaps)
{
(void)target_phantom;
reverse_heaps.emplace_back(std::make_unique<Heap>(nodes_number, border_nodes_number));
heap->Clear();
}
// std::vector<std::unique_ptr<Heap>> reverse_heaps;
// const auto nodes_number = facade.GetNumberOfNodes();
// const auto border_nodes_number = facade.GetMaxBorderNodeID() + 1;
// for (const auto &target_phantom : target_phantoms)
// {
// (void)target_phantom;
// reverse_heaps.emplace_back(std::make_unique<Heap>(nodes_number, border_nodes_number));
// }
if (source_phantom.IsValidForwardSource())
{
@ -1041,17 +1081,16 @@ getNetworkDistances(SearchEngineData<Algorithm> &engine_working_data,
// {
// endpoints.push_back(target_phantom);
// }
PhantomNodeCandidates phantom_candidates;
phantom_candidates.push_back(source_phantom);
for (const auto &target_phantom : target_phantoms)
{
phantom_candidates.push_back(target_phantom);
}
std::vector<PhantomNode> source_phantomes;
source_phantomes.push_back(source_phantom);
PhantomEndpointCandidates phantom_candidates{source_phantomes, target_phantoms};
auto distances2 = searchDistance2(engine_working_data,
facade,
forward_heap,
reverse_heaps,
target_phantoms.size(),
{},
weight_upper_bound,
phantom_candidates);

5
include/engine/search_engine_data.hpp
View File

@ -56,6 +56,7 @@ template <> struct SearchEngineData<routing_algorithms::ch::Algorithm>
static thread_local ManyToManyHeapPtr many_to_many_heap;
static thread_local SearchEngineHeapPtr map_matching_forward_heap_1;
static thread_local SearchEngineHeapPtr map_matching_reverse_heap_1;
static thread_local std::vector<SearchEngineHeapPtr> map_matching_reverse_heaps;
void InitializeOrClearMapMatchingThreadLocalStorage(unsigned number_of_nodes);
@ -133,13 +134,15 @@ template <> struct SearchEngineData<routing_algorithms::mld::Algorithm>
static thread_local SearchEngineHeapPtr reverse_heap_1;
static thread_local MapMatchingHeapPtr map_matching_forward_heap_1;
static thread_local MapMatchingHeapPtr map_matching_reverse_heap_1;
static thread_local std::vector<MapMatchingHeapPtr> map_matching_reverse_heaps;
static thread_local ManyToManyHeapPtr many_to_many_heap;
void InitializeOrClearFirstThreadLocalStorage(unsigned number_of_nodes,
unsigned number_of_boundary_nodes);
void InitializeOrClearMapMatchingThreadLocalStorage(unsigned number_of_nodes,
unsigned number_of_boundary_nodes);
unsigned number_of_boundary_nodes,
size_t max_candidates);
void InitializeOrClearManyToManyThreadLocalStorage(unsigned number_of_nodes,
unsigned number_of_boundary_nodes);

6
src/benchmarks/bench.cpp
View File

@ -395,9 +395,9 @@ void runMatchBenchmark(const OSRM &osrm, const GPSTraces &gpsTraces, int iterati
std::optional<size_t> radius = std::nullopt;
};
std::vector<Benchmark> benchmarks = {{"500 matches, default radius"},
{"500 matches, radius=10", 10},
{"500 matches, radius=20", 20}};
std::vector<Benchmark> benchmarks = {{"500 matches, default radius", 10},
/*{"500 matches, radius=10", 10},
{"500 matches, radius=20", 20}*/};
runBenchmarks(benchmarks,
iterations,

162
src/engine/routing_algorithms/map_matching.cpp
View File

@ -45,7 +45,8 @@ unsigned getMedianSampleTime(const std::vector<unsigned> &timestamps)
template <typename Algorithm>
inline void initializeHeap(SearchEngineData<Algorithm> &engine_working_data,
const DataFacade<Algorithm> &facade)
const DataFacade<Algorithm> &facade,
size_t)
{
const auto nodes_number = facade.GetNumberOfNodes();
@ -54,14 +55,86 @@ inline void initializeHeap(SearchEngineData<Algorithm> &engine_working_data,
template <>
inline void initializeHeap<mld::Algorithm>(SearchEngineData<mld::Algorithm> &engine_working_data,
const DataFacade<mld::Algorithm> &facade)
const DataFacade<mld::Algorithm> &facade,
size_t max_candidates)
{
const auto nodes_number = facade.GetNumberOfNodes();
const auto border_nodes_number = facade.GetMaxBorderNodeID() + 1;
engine_working_data.InitializeOrClearMapMatchingThreadLocalStorage(nodes_number,
border_nodes_number);
border_nodes_number,
max_candidates);
}
#include <iostream>
#include <fstream>
template <typename T>
void saveVectorToFile(const std::vector<T>& data, const std::string& filename) {
std::ofstream outFile(filename, std::ios::binary);
if (!outFile) {
std::cerr << "Error opening file for writing: " << filename << std::endl;
return;
}
size_t size = data.size();
outFile.write(reinterpret_cast<const char*>(&size), sizeof(size));
outFile.write(reinterpret_cast<const char*>(data.data()), size * sizeof(T));
outFile.close();
if (!outFile.good()) {
std::cerr << "Error occurred at writing time!" << std::endl;
}
}
template <typename T>
bool loadVectorFromFile(std::vector<T>& data, const std::string& filename) {
std::ifstream inFile(filename, std::ios::binary);
if (!inFile) {
std::cerr << "Error opening file for reading: " << filename << std::endl;
return false;
}
size_t size;
inFile.read(reinterpret_cast<char*>(&size), sizeof(size));
data.resize(size);
inFile.read(reinterpret_cast<char*>(data.data()), size * sizeof(T));
inFile.close();
if (!inFile.good()) {
std::cerr << "Error occurred at reading time!" << std::endl;
return false;
}
return true;
}
template <typename T>
void saveStructToFile(const T& data, const std::string& filename) {
std::ofstream outFile(filename, std::ios::binary);
if (!outFile) {
std::cerr << "Error opening file for writing: " << filename << std::endl;
return;
}
outFile.write(reinterpret_cast<const char*>(&data), sizeof(T));
outFile.close();
if (!outFile.good()) {
std::cerr << "Error occurred at writing time!" << std::endl;
}
}
template <typename T>
bool loadStructFromFile(T& data, const std::string& filename) {
std::ifstream inFile(filename, std::ios::binary);
if (!inFile) {
std::cerr << "Error opening file for reading: " << filename << std::endl;
return false;
}
inFile.read(reinterpret_cast<char*>(&data), sizeof(T));
inFile.close();
if (!inFile.good()) {
std::cerr << "Error occurred at reading time!" << std::endl;
return false;
}
return true;
}
} // namespace
template <typename Algorithm>
@ -144,9 +217,16 @@ SubMatchingList mapMatching(SearchEngineData<Algorithm> &engine_working_data,
return sub_matchings;
}
initializeHeap(engine_working_data, facade);
size_t max_candidates = 0;
for (const auto &candidates : candidates_list)
{
max_candidates = std::max(max_candidates, candidates.size());
}
initializeHeap(engine_working_data, facade, max_candidates);
auto &forward_heap = *engine_working_data.map_matching_forward_heap_1;
auto &reverse_heap = *engine_working_data.map_matching_reverse_heap_1;
const auto &reverse_heaps = engine_working_data.map_matching_reverse_heaps;
std::size_t breakage_begin = map_matching::INVALID_STATE;
std::vector<std::size_t> split_points;
@ -224,8 +304,20 @@ SubMatchingList mapMatching(SearchEngineData<Algorithm> &engine_working_data,
{
continue;
}
std::vector<PhantomNode> target_phantom_nodes;
// PhantomNode source;
// loadStructFromFile<PhantomNode>(source, "source.bin");
std::vector<PhantomNode> target_phantom_nodes;
// loadVectorFromFile(target_phantom_nodes, "target.bin");
// target_phantom_nodes.erase(target_phantom_nodes.begin());
// target_phantom_nodes.erase(target_phantom_nodes.begin());
// target_phantom_nodes.erase(target_phantom_nodes.begin());
// target_phantom_nodes.erase(target_phantom_nodes.begin());
// target_phantom_nodes.pop_back();
// target_phantom_nodes.pop_back();
// target_phantom_nodes.erase(target_phantom_nodes.begin() + 1);
// target_phantom_nodes.push_back(target);
for (const auto s_prime : util::irange<std::size_t>(0UL, current_viterbi.size()))
{
const double emission_pr = emission_log_probabilities[t][s_prime];
@ -237,16 +329,22 @@ SubMatchingList mapMatching(SearchEngineData<Algorithm> &engine_working_data,
target_phantom_nodes.push_back(current_timestamps_list[s_prime].phantom_node);
}
auto new_distances =
// TIMER_START(NEW_DIST);
#if 1
(void)reverse_heap;
auto distances =
getNetworkDistances(engine_working_data,
facade,
forward_heap,
reverse_heap,
reverse_heaps,
prev_unbroken_timestamps_list[s].phantom_node,
target_phantom_nodes,
weight_upper_bound);
std::vector<double> old_distances;
// TIMER_STOP(NEW_DIST);
#else
// TIMER_START(OLD_DIST);
(void)reverse_heaps;
std::vector<double> distances;
for (const auto &pn : target_phantom_nodes)
{
@ -258,18 +356,26 @@ SubMatchingList mapMatching(SearchEngineData<Algorithm> &engine_working_data,
prev_unbroken_timestamps_list[s].phantom_node,
pn,
weight_upper_bound);
old_distances.push_back(network_distance);
distances.push_back(network_distance);
}
#endif
// TIMER_STOP(OLD_DIST);
for (size_t i = 0; i < old_distances.size(); ++i)
{
if (std::abs(old_distances[i] - new_distances[i]) > 0.01)
{
std::cerr << "OOPS " << old_distances[i] << " " << new_distances[i]
<< std::endl;
}
}
// std::cerr << "Old: " << TIMER_MSEC(OLD_DIST) << " New: " << TIMER_MSEC(NEW_DIST)
// << std::endl;
// for (size_t i = 0; i < old_distances.size(); ++i)
// {
// if (std::abs(old_distances[i] - new_distances[i]) > 0.01)
// {
// // saveStructToFile(prev_unbroken_timestamps_list[s].phantom_node, "source.bin");
// // saveVectorToFile(target_phantom_nodes, "target.bin");
// // std::cerr << "OOPS " << old_distances[i] << " " << new_distances[i]
// // << std::endl;
// // std::exit(1);
// }
// }
size_t distance_index = 0;
for (const auto s_prime : util::irange<std::size_t>(0UL, current_viterbi.size()))
{
const double emission_pr = emission_log_probabilities[t][s_prime];
@ -279,14 +385,16 @@ SubMatchingList mapMatching(SearchEngineData<Algorithm> &engine_working_data,
continue;
}
double network_distance =
getNetworkDistance(engine_working_data,
facade,
forward_heap,
reverse_heap,
prev_unbroken_timestamps_list[s].phantom_node,
current_timestamps_list[s_prime].phantom_node,
weight_upper_bound);
double network_distance = distances[distance_index];
++distance_index;
// double network_distance =
// getNetworkDistance(engine_working_data,
// facade,
// forward_heap,
// reverse_heap,
// prev_unbroken_timestamps_list[s].phantom_node,
// current_timestamps_list[s_prime].phantom_node,
// weight_upper_bound);
// get distance diff between loc1/2 and locs/s_prime
const auto d_t = std::abs(network_distance - haversine_distance);

13
src/engine/search_engine_data.cpp
View File

@ -15,6 +15,7 @@ thread_local SearchEngineData<CH>::SearchEngineHeapPtr
SearchEngineData<CH>::map_matching_forward_heap_1;
thread_local SearchEngineData<CH>::SearchEngineHeapPtr
SearchEngineData<CH>::map_matching_reverse_heap_1;
thread_local std::vector<typename SearchEngineData<CH>::SearchEngineHeapPtr> SearchEngineData<CH>::map_matching_reverse_heaps;
thread_local SearchEngineData<CH>::ManyToManyHeapPtr SearchEngineData<CH>::many_to_many_heap;
@ -123,9 +124,10 @@ thread_local SearchEngineData<MLD>::MapMatchingHeapPtr
thread_local SearchEngineData<MLD>::MapMatchingHeapPtr
SearchEngineData<MLD>::map_matching_reverse_heap_1;
thread_local SearchEngineData<MLD>::ManyToManyHeapPtr SearchEngineData<MLD>::many_to_many_heap;
thread_local std::vector<typename SearchEngineData<MLD>::MapMatchingHeapPtr> SearchEngineData<MLD>::map_matching_reverse_heaps;
void SearchEngineData<MLD>::InitializeOrClearMapMatchingThreadLocalStorage(
unsigned number_of_nodes, unsigned number_of_boundary_nodes)
unsigned number_of_nodes, unsigned number_of_boundary_nodes, size_t max_candidates)
{
if (map_matching_forward_heap_1.get())
{
@ -146,6 +148,15 @@ void SearchEngineData<MLD>::InitializeOrClearMapMatchingThreadLocalStorage(
map_matching_reverse_heap_1.reset(
new MapMatchingQueryHeap(number_of_nodes, number_of_boundary_nodes));
}
if (max_candidates > map_matching_reverse_heaps.size()) {
size_t to_add = max_candidates - map_matching_reverse_heaps.size();
for (unsigned i = 0; i < to_add; ++i)
{
map_matching_reverse_heaps.emplace_back(new MapMatchingQueryHeap(number_of_nodes, number_of_boundary_nodes));
}
}
}
void SearchEngineData<MLD>::InitializeOrClearFirstThreadLocalStorage(