somehow works
This commit is contained in:
Siarhei Fedartsou
parent
b577558980
commit
2b38c936d5
@ -460,16 +460,18 @@ void search(SearchEngineData<Algorithm> &engine_working_data,
|
||||
duration_upper_bound);
|
||||
}
|
||||
|
||||
inline std::vector<double> getNetworkDistances(SearchEngineData<Algorithm> &,
|
||||
const DataFacade<ch::Algorithm> &,
|
||||
SearchEngineData<Algorithm>::QueryHeap &,
|
||||
SearchEngineData<Algorithm>::QueryHeap &,
|
||||
const PhantomNode &,
|
||||
const std::vector<PhantomNode> &,
|
||||
EdgeWeight /*duration_upper_bound*/ = INVALID_EDGE_WEIGHT) {
|
||||
inline std::vector<double>
|
||||
getNetworkDistances(SearchEngineData<Algorithm> &,
|
||||
const DataFacade<ch::Algorithm> &,
|
||||
SearchEngineData<Algorithm>::QueryHeap &,
|
||||
SearchEngineData<Algorithm>::QueryHeap &,
|
||||
const PhantomNode &,
|
||||
const std::vector<PhantomNode> &,
|
||||
EdgeWeight /*duration_upper_bound*/ = INVALID_EDGE_WEIGHT)
|
||||
{
|
||||
std::vector<double> distances;
|
||||
return distances;
|
||||
}
|
||||
}
|
||||
|
||||
// Requires the heaps for be empty
|
||||
// If heaps should be adjusted to be initialized outside of this function,
|
||||
|
||||
@ -705,8 +705,6 @@ void unpackPath(const FacadeT &facade,
|
||||
annotatePath(facade, route_endpoints, unpacked_nodes, unpacked_edges, unpacked_path);
|
||||
}
|
||||
|
||||
|
||||
|
||||
template <typename Algorithm>
|
||||
double getNetworkDistance(SearchEngineData<Algorithm> &engine_working_data,
|
||||
const DataFacade<Algorithm> &facade,
|
||||
@ -765,14 +763,228 @@ double getNetworkDistance(SearchEngineData<Algorithm> &engine_working_data,
|
||||
return from_alias<double>(distance);
|
||||
}
|
||||
|
||||
template <bool DIRECTION, typename Algorithm, typename Heap, typename... Args>
|
||||
auto routingStep2(const DataFacade<Algorithm> &facade, Heap &forward_heap, const Args &...args)
|
||||
{
|
||||
const auto heapNode = forward_heap.DeleteMinGetHeapNode();
|
||||
// const auto weight = heapNode.weight;
|
||||
|
||||
BOOST_ASSERT(!facade.ExcludeNode(heapNode.node));
|
||||
|
||||
// // Upper bound for the path source -> target with
|
||||
// // weight(source -> node) = weight weight(to -> target) ≤ reverse_weight
|
||||
// // is weight + reverse_weight
|
||||
// // More tighter upper bound requires additional condition reverse_heap.WasRemoved(to)
|
||||
// // with weight(to -> target) = reverse_weight and all weights ≥ 0
|
||||
// const auto reverseHeapNode = reverse_heap.GetHeapNodeIfWasInserted(heapNode.node);
|
||||
// if (reverseHeapNode)
|
||||
// {
|
||||
// auto reverse_weight = reverseHeapNode->weight;
|
||||
// auto path_weight = weight + reverse_weight;
|
||||
|
||||
// if (!shouldForceStep(force_step_nodes, heapNode, *reverseHeapNode) &&
|
||||
// (path_weight >= EdgeWeight{0}) && (path_weight < path_upper_bound))
|
||||
// {
|
||||
// middle_node = heapNode.node;
|
||||
// path_upper_bound = path_weight;
|
||||
// }
|
||||
// }
|
||||
|
||||
// Relax outgoing edges from node
|
||||
relaxOutgoingEdges<DIRECTION>(facade, forward_heap, heapNode, args...);
|
||||
|
||||
return heapNode;
|
||||
}
|
||||
|
||||
template <typename Algorithm, typename Heap, typename... Args>
|
||||
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,
|
||||
const std::vector<NodeID> &force_step_nodes,
|
||||
EdgeWeight weight_upper_bound,
|
||||
const Args &...args)
|
||||
{
|
||||
// if (forward_heap.Empty() || reverse_heap.Empty())
|
||||
// {
|
||||
// return {};
|
||||
// }
|
||||
|
||||
// BOOST_ASSERT(!forward_heap.Empty() && forward_heap.MinKey() < INVALID_EDGE_WEIGHT);
|
||||
// BOOST_ASSERT(!reverse_heap.Empty() && reverse_heap.MinKey() < INVALID_EDGE_WEIGHT);
|
||||
|
||||
std::vector<NodeID> middles;
|
||||
std::vector<EdgeWeight> weights;
|
||||
|
||||
middles.resize(reverse_heap.size(), SPECIAL_NODEID);
|
||||
weights.resize(reverse_heap.size(), 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)
|
||||
{
|
||||
reverse_heap_mins.push_back(heap->MinKey());
|
||||
}
|
||||
|
||||
auto shouldContinue = [&]()
|
||||
{
|
||||
bool cont = false;
|
||||
for (size_t i = 0; i < reverse_heap.size(); ++i)
|
||||
{
|
||||
if (reverse_heap_mins[i] < weights[i])
|
||||
{
|
||||
cont = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
return cont;
|
||||
};
|
||||
|
||||
while ((forward_heap.Size() + std::accumulate(reverse_heap.begin(),
|
||||
reverse_heap.end(),
|
||||
0,
|
||||
[](auto sum, const auto &heap)
|
||||
{ return sum + heap->Size(); }) >
|
||||
0) &&
|
||||
shouldContinue())
|
||||
{
|
||||
if (!forward_heap.Empty())
|
||||
{
|
||||
auto heapNode = routingStep2<FORWARD_DIRECTION>(facade, forward_heap, args...);
|
||||
|
||||
for (size_t i = 0; i < reverse_heap.size(); ++i)
|
||||
{
|
||||
auto &rh = reverse_heap[i];
|
||||
const auto reverseHeapNode = rh->GetHeapNodeIfWasInserted(heapNode.node);
|
||||
if (reverseHeapNode)
|
||||
{
|
||||
auto reverse_weight = reverseHeapNode->weight;
|
||||
auto path_weight = heapNode.weight + reverse_weight;
|
||||
|
||||
if (!shouldForceStep(force_step_nodes, heapNode, *reverseHeapNode) &&
|
||||
(path_weight >= EdgeWeight{0}) && (path_weight < weights[i]))
|
||||
{
|
||||
middles[i] = heapNode.node;
|
||||
weights[i] = path_weight;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (!forward_heap.Empty())
|
||||
forward_heap_min = forward_heap.MinKey();
|
||||
}
|
||||
for (size_t i = 0; i < reverse_heap.size(); ++i)
|
||||
{
|
||||
if (!reverse_heap[i]->Empty())
|
||||
{
|
||||
auto heapNode = routingStep2<REVERSE_DIRECTION>(facade, *reverse_heap[i], args...);
|
||||
const auto reverseHeapNode = forward_heap.GetHeapNodeIfWasInserted(heapNode.node);
|
||||
if (reverseHeapNode)
|
||||
{
|
||||
auto reverse_weight = reverseHeapNode->weight;
|
||||
auto path_weight = heapNode.weight + reverse_weight;
|
||||
|
||||
if (!shouldForceStep(force_step_nodes, heapNode, *reverseHeapNode) &&
|
||||
(path_weight >= EdgeWeight{0}) && (path_weight < weights[i]))
|
||||
{
|
||||
middles[i] = heapNode.node;
|
||||
weights[i] = path_weight;
|
||||
}
|
||||
}
|
||||
if (!reverse_heap[i]->Empty())
|
||||
reverse_heap_mins[i] = reverse_heap[i]->MinKey();
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
std::vector<std::optional<std::pair<NodeID, EdgeWeight>>> results;
|
||||
for (size_t i = 0; i < reverse_heap.size(); ++i)
|
||||
{
|
||||
if (weights[i] >= weight_upper_bound || SPECIAL_NODEID == middles[i])
|
||||
{
|
||||
results.push_back({});
|
||||
}
|
||||
else
|
||||
{
|
||||
results.push_back({{middles[i], weights[i]}});
|
||||
}
|
||||
}
|
||||
return results;
|
||||
|
||||
// // run two-Target Dijkstra routing step.
|
||||
// NodeID middle = SPECIAL_NODEID;
|
||||
// EdgeWeight weight = weight_upper_bound;
|
||||
// EdgeWeight forward_heap_min = forward_heap.MinKey();
|
||||
// EdgeWeight reverse_heap_min = reverse_heap.MinKey();
|
||||
// while (forward_heap.Size() + reverse_heap.Size() > 0 &&
|
||||
// forward_heap_min + reverse_heap_min < weight)
|
||||
// {
|
||||
// if (!forward_heap.Empty())
|
||||
// {
|
||||
// routingStep<FORWARD_DIRECTION>(
|
||||
// facade, forward_heap, reverse_heap, middle, weight, force_step_nodes, args...);
|
||||
// if (!forward_heap.Empty())
|
||||
// forward_heap_min = forward_heap.MinKey();
|
||||
// }
|
||||
// if (!reverse_heap.Empty())
|
||||
// {
|
||||
// routingStep<REVERSE_DIRECTION>(
|
||||
// facade, reverse_heap, forward_heap, middle, weight, force_step_nodes, args...);
|
||||
// if (!reverse_heap.Empty())
|
||||
// reverse_heap_min = reverse_heap.MinKey();
|
||||
// }
|
||||
// };
|
||||
|
||||
// // No path found for both target nodes?
|
||||
// if (weight >= weight_upper_bound || SPECIAL_NODEID == middle)
|
||||
// {
|
||||
// return {};
|
||||
// }
|
||||
|
||||
// return {{middle, weight}};
|
||||
}
|
||||
|
||||
template <typename Algorithm, typename... Args>
|
||||
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,
|
||||
const std::vector<NodeID> &force_step_nodes,
|
||||
EdgeWeight weight_upper_bound,
|
||||
const Args &...args)
|
||||
{
|
||||
auto searchResults = runSearch2(
|
||||
facade, forward_heap, reverse_heaps, force_step_nodes, weight_upper_bound, args...);
|
||||
std::vector<EdgeDistance> res;
|
||||
for (size_t i = 0; i < searchResults.size(); ++i)
|
||||
{
|
||||
if (!searchResults[i])
|
||||
{
|
||||
res.push_back(INVALID_EDGE_DISTANCE);
|
||||
}
|
||||
else
|
||||
{
|
||||
auto [middle, _] = *searchResults[i];
|
||||
auto distance =
|
||||
forward_heap.GetData(middle).distance + reverse_heaps[i]->GetData(middle).distance;
|
||||
res.push_back(distance);
|
||||
}
|
||||
}
|
||||
return res;
|
||||
}
|
||||
template <typename Algorithm>
|
||||
std::vector<double> getNetworkDistances(SearchEngineData<Algorithm> &engine_working_data,
|
||||
const DataFacade<Algorithm> &facade,
|
||||
typename SearchEngineData<Algorithm>::MapMatchingQueryHeap &forward_heap,
|
||||
typename SearchEngineData<Algorithm>::MapMatchingQueryHeap &reverse_heap,
|
||||
const PhantomNode &source_phantom,
|
||||
const std::vector<PhantomNode> &target_phantoms,
|
||||
EdgeWeight duration_upper_bound = INVALID_EDGE_WEIGHT) {
|
||||
std::vector<double>
|
||||
getNetworkDistances(SearchEngineData<Algorithm> &engine_working_data,
|
||||
const DataFacade<Algorithm> &facade,
|
||||
typename SearchEngineData<Algorithm>::MapMatchingQueryHeap &forward_heap,
|
||||
typename SearchEngineData<Algorithm>::MapMatchingQueryHeap &,
|
||||
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;
|
||||
@ -802,9 +1014,10 @@ std::vector<double> getNetworkDistances(SearchEngineData<Algorithm> &engine_work
|
||||
EdgeDistance{0} - source_phantom.GetReverseDistance()});
|
||||
}
|
||||
|
||||
for (size_t i = 0; i < target_phantoms.size(); ++i) {
|
||||
auto& reverse_heap = *reverse_heaps[i];
|
||||
const auto& target_phantom = target_phantoms[i];
|
||||
for (size_t i = 0; i < target_phantoms.size(); ++i)
|
||||
{
|
||||
auto &reverse_heap = *reverse_heaps[i];
|
||||
const auto &target_phantom = target_phantoms[i];
|
||||
if (target_phantom.IsValidForwardTarget())
|
||||
{
|
||||
reverse_heap.Insert(
|
||||
@ -822,21 +1035,68 @@ std::vector<double> getNetworkDistances(SearchEngineData<Algorithm> &engine_work
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
std::vector<double> distances;
|
||||
// PhantomEndpoints endpoints{};
|
||||
// endpoints.push_back(source_phantom);
|
||||
// for (const auto &target_phantom : target_phantoms)
|
||||
// {
|
||||
// endpoints.push_back(target_phantom);
|
||||
// }
|
||||
PhantomNodeCandidates phantom_candidates;
|
||||
phantom_candidates.push_back(source_phantom);
|
||||
for (const auto &target_phantom : target_phantoms)
|
||||
{
|
||||
auto distance = getNetworkDistance(engine_working_data,
|
||||
facade,
|
||||
forward_heap,
|
||||
reverse_heap,
|
||||
source_phantom,
|
||||
target_phantom,
|
||||
duration_upper_bound);
|
||||
distances.push_back(distance);
|
||||
phantom_candidates.push_back(target_phantom);
|
||||
}
|
||||
|
||||
auto distances2 = searchDistance2(engine_working_data,
|
||||
facade,
|
||||
forward_heap,
|
||||
reverse_heaps,
|
||||
{},
|
||||
weight_upper_bound,
|
||||
phantom_candidates);
|
||||
std::vector<double> distances;
|
||||
for (auto d : distances2)
|
||||
{
|
||||
if (d == INVALID_EDGE_DISTANCE)
|
||||
{
|
||||
distances.push_back(std::numeric_limits<double>::max());
|
||||
}
|
||||
else
|
||||
{
|
||||
distances.push_back(from_alias<double>(d));
|
||||
}
|
||||
}
|
||||
return distances;
|
||||
|
||||
// for (const auto &target_phantom : target_phantoms)
|
||||
// {
|
||||
// // forward_heap.Clear();
|
||||
// // auto& reverse_heap = *reverse_heaps[0];
|
||||
// // reverse_heap.Clear();
|
||||
// // const PhantomEndpoints endpoints{source_phantom, target_phantom};
|
||||
|
||||
// auto distance = searchDistance2(
|
||||
// engine_working_data, facade, forward_heap, reverse_heap, {}, weight_upper_bound,
|
||||
// endpoints);
|
||||
|
||||
// // if (distance == INVALID_EDGE_DISTANCE)
|
||||
// // {
|
||||
// // distances.push_back(std::numeric_limits<double>::max());
|
||||
// // } else {
|
||||
// // distances.push_back(from_alias<double>(distance));
|
||||
// // }
|
||||
// // return from_alias<double>(distance);
|
||||
// auto distance = getNetworkDistance(engine_working_data,
|
||||
// facade,
|
||||
// forward_heap,
|
||||
// *reverse_heaps[0],
|
||||
// source_phantom,
|
||||
// target_phantom,
|
||||
// weight_upper_bound);
|
||||
// distances.push_back(distance);
|
||||
// }
|
||||
// return distances;
|
||||
}
|
||||
|
||||
} // namespace osrm::engine::routing_algorithms::mld
|
||||
|
||||
@ -237,19 +237,20 @@ SubMatchingList mapMatching(SearchEngineData<Algorithm> &engine_working_data,
|
||||
target_phantom_nodes.push_back(current_timestamps_list[s_prime].phantom_node);
|
||||
}
|
||||
|
||||
auto new_distances = getNetworkDistances(engine_working_data,
|
||||
facade,
|
||||
forward_heap,
|
||||
reverse_heap,
|
||||
prev_unbroken_timestamps_list[s].phantom_node,
|
||||
target_phantom_nodes,
|
||||
weight_upper_bound);
|
||||
auto new_distances =
|
||||
getNetworkDistances(engine_working_data,
|
||||
facade,
|
||||
forward_heap,
|
||||
reverse_heap,
|
||||
prev_unbroken_timestamps_list[s].phantom_node,
|
||||
target_phantom_nodes,
|
||||
weight_upper_bound);
|
||||
|
||||
std::vector<double> old_distances;
|
||||
|
||||
for (const auto& pn: target_phantom_nodes)
|
||||
for (const auto &pn : target_phantom_nodes)
|
||||
{
|
||||
double network_distance =
|
||||
double network_distance =
|
||||
getNetworkDistance(engine_working_data,
|
||||
facade,
|
||||
forward_heap,
|
||||
@ -257,14 +258,18 @@ SubMatchingList mapMatching(SearchEngineData<Algorithm> &engine_working_data,
|
||||
prev_unbroken_timestamps_list[s].phantom_node,
|
||||
pn,
|
||||
weight_upper_bound);
|
||||
old_distances.push_back(network_distance);
|
||||
old_distances.push_back(network_distance);
|
||||
}
|
||||
|
||||
if (old_distances != new_distances) {
|
||||
std::cerr << "OOPS " << std::endl;
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
for (const auto s_prime : util::irange<std::size_t>(0UL, current_viterbi.size()))
|
||||
{
|
||||
const double emission_pr = emission_log_probabilities[t][s_prime];
|
||||
|
||||
Loading…
Reference in New Issue
Block a user