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

Preserve heap state in map matching

Browse Source
This commit is contained in:
Patrick Niklaus 2018-04-27 05:36:52 +00:00 committed by Patrick Niklaus
parent 89fabc1b9c
commit b630b4e32a
7 changed files with 185 additions and 98 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

159
include/engine/routing_algorithms/routing_base.hpp
View File

@ -44,50 +44,19 @@ bool needsLoopBackwards(const PhantomNode &source_phantom, const PhantomNode &ta
bool needsLoopForward(const PhantomNodes &phantoms); bool needsLoopForward(const PhantomNodes &phantoms);
bool needsLoopBackwards(const PhantomNodes &phantoms); bool needsLoopBackwards(const PhantomNodes &phantoms);
template <typename Heap> namespace detail
void insertNodesInHeaps(Heap &forward_heap, Heap &reverse_heap, const PhantomNodes &nodes)
{ {
const auto &source = nodes.source_phantom; template <typename Algorithm>
if (source.IsValidForwardSource()) void insertSourceInHeap(typename SearchEngineData<Algorithm>::ManyToManyQueryHeap &heap,
{ const PhantomNode &phantom_node)
forward_heap.Insert(source.forward_segment_id.id,
-source.GetForwardWeightPlusOffset(),
source.forward_segment_id.id);
}
if (source.IsValidReverseSource())
{
forward_heap.Insert(source.reverse_segment_id.id,
-source.GetReverseWeightPlusOffset(),
source.reverse_segment_id.id);
}
const auto &target = nodes.target_phantom;
if (target.IsValidForwardTarget())
{
reverse_heap.Insert(target.forward_segment_id.id,
target.GetForwardWeightPlusOffset(),
target.forward_segment_id.id);
}
if (target.IsValidReverseTarget())
{
reverse_heap.Insert(target.reverse_segment_id.id,
target.GetReverseWeightPlusOffset(),
target.reverse_segment_id.id);
}
}
template <typename ManyToManyQueryHeap>
void insertSourceInHeap(ManyToManyQueryHeap &heap, const PhantomNode &phantom_node)
{ {
if (phantom_node.IsValidForwardSource()) if (phantom_node.IsValidForwardTarget())
{ {
heap.Insert(phantom_node.forward_segment_id.id, heap.Insert(phantom_node.forward_segment_id.id,
-phantom_node.GetForwardWeightPlusOffset(), -phantom_node.GetForwardWeightPlusOffset(),
{phantom_node.forward_segment_id.id, -phantom_node.GetForwardDuration()}); {phantom_node.forward_segment_id.id, -phantom_node.GetForwardDuration()});
} }
if (phantom_node.IsValidReverseSource()) if (phantom_node.IsValidReverseTarget())
{ {
heap.Insert(phantom_node.reverse_segment_id.id, heap.Insert(phantom_node.reverse_segment_id.id,
-phantom_node.GetReverseWeightPlusOffset(), -phantom_node.GetReverseWeightPlusOffset(),
@ -95,8 +64,9 @@ void insertSourceInHeap(ManyToManyQueryHeap &heap, const PhantomNode &phantom_no
} }
} }
template <typename ManyToManyQueryHeap> template <typename Algorithm>
void insertTargetInHeap(ManyToManyQueryHeap &heap, const PhantomNode &phantom_node) void insertTargetInHeap(typename SearchEngineData<Algorithm>::ManyToManyQueryHeap &heap,
const PhantomNode &phantom_node)
{ {
if (phantom_node.IsValidForwardTarget()) if (phantom_node.IsValidForwardTarget())
{ {
@ -112,6 +82,109 @@ void insertTargetInHeap(ManyToManyQueryHeap &heap, const PhantomNode &phantom_no
} }
} }
template <typename Algorithm>
void insertSourceInHeap(typename SearchEngineData<Algorithm>::QueryHeap &heap,
const PhantomNode &phantom_node)
{
if (phantom_node.IsValidForwardSource())
{
heap.Insert(phantom_node.forward_segment_id.id,
-phantom_node.GetForwardWeightPlusOffset(),
phantom_node.forward_segment_id.id);
}
if (phantom_node.IsValidReverseSource())
{
heap.Insert(phantom_node.reverse_segment_id.id,
-phantom_node.GetReverseWeightPlusOffset(),
phantom_node.reverse_segment_id.id);
}
}
template <typename Algorithm>
void insertTargetInHeap(typename SearchEngineData<Algorithm>::QueryHeap &heap,
const PhantomNode &phantom_node)
{
if (phantom_node.IsValidForwardTarget())
{
heap.Insert(phantom_node.forward_segment_id.id,
phantom_node.GetForwardWeightPlusOffset(),
phantom_node.forward_segment_id.id);
}
if (phantom_node.IsValidReverseTarget())
{
heap.Insert(phantom_node.reverse_segment_id.id,
phantom_node.GetReverseWeightPlusOffset(),
phantom_node.reverse_segment_id.id);
}
}
} // namespace detail
inline void insertTargetInHeap(typename SearchEngineData<mld::Algorithm>::ManyToManyQueryHeap &heap,
const PhantomNode &phantom_node)
{
detail::insertTargetInHeap<mld::Algorithm>(heap, phantom_node);
}
inline void insertTargetInHeap(typename SearchEngineData<ch::Algorithm>::ManyToManyQueryHeap &heap,
const PhantomNode &phantom_node)
{
detail::insertTargetInHeap<ch::Algorithm>(heap, phantom_node);
}
inline void insertTargetInHeap(typename SearchEngineData<mld::Algorithm>::QueryHeap &heap,
const PhantomNode &phantom_node)
{
detail::insertTargetInHeap<mld::Algorithm>(heap, phantom_node);
}
inline void insertTargetInHeap(typename SearchEngineData<ch::Algorithm>::QueryHeap &heap,
const PhantomNode &phantom_node)
{
detail::insertTargetInHeap<ch::Algorithm>(heap, phantom_node);
}
inline void insertSourceInHeap(typename SearchEngineData<mld::Algorithm>::ManyToManyQueryHeap &heap,
const PhantomNode &phantom_node)
{
detail::insertSourceInHeap<mld::Algorithm>(heap, phantom_node);
}
inline void insertSourceInHeap(typename SearchEngineData<ch::Algorithm>::ManyToManyQueryHeap &heap,
const PhantomNode &phantom_node)
{
detail::insertSourceInHeap<ch::Algorithm>(heap, phantom_node);
}
inline void insertSourceInHeap(typename SearchEngineData<mld::Algorithm>::QueryHeap &heap,
const PhantomNode &phantom_node)
{
detail::insertSourceInHeap<mld::Algorithm>(heap, phantom_node);
}
inline void insertSourceInHeap(typename SearchEngineData<ch::Algorithm>::QueryHeap &heap,
const PhantomNode &phantom_node)
{
detail::insertSourceInHeap<ch::Algorithm>(heap, phantom_node);
}
template <typename Heap>
void insertNodesInHeaps(Heap &forward_heap, Heap &reverse_heap, const PhantomNodes &nodes)
{
insertSourceInHeap(forward_heap, nodes.source_phantom);
insertTargetInHeap(reverse_heap, nodes.target_phantom);
}
template <typename Algorithm>
void insertSourceInHeap(typename SearchEngineData<Algorithm>::ManyToManyQueryHeap &heap,
const PhantomNode &phantom_node)
{
if (phantom_node.IsValidForwardSource())
{
heap.Insert(phantom_node.forward_segment_id.id,
-phantom_node.GetForwardWeightPlusOffset(),
{phantom_node.forward_segment_id.id, -phantom_node.GetForwardDuration()});
}
if (phantom_node.IsValidReverseSource())
{
heap.Insert(phantom_node.reverse_segment_id.id,
-phantom_node.GetReverseWeightPlusOffset(),
{phantom_node.reverse_segment_id.id, -phantom_node.GetReverseDuration()});
}
}
template <typename FacadeT> template <typename FacadeT>
void annotatePath(const FacadeT &facade, void annotatePath(const FacadeT &facade,
const PhantomNodes &phantom_node_pair, const PhantomNodes &phantom_node_pair,
@ -321,10 +394,10 @@ void annotatePath(const FacadeT &facade,
} }
} }
void adjustPathDistanceToPhantomNodes(const std::vector<NodeID> &path, EdgeDistance adjustPathDistanceToPhantomNodes(const std::vector<NodeID> &path,
const PhantomNode &source_phantom, const PhantomNode &source_phantom,
const PhantomNode &target_phantom, const PhantomNode &target_phantom,
EdgeDistance &distance); const EdgeDistance distance);
template <typename AlgorithmT> template <typename AlgorithmT>
InternalRouteResult extractRoute(const DataFacade<AlgorithmT> &facade, InternalRouteResult extractRoute(const DataFacade<AlgorithmT> &facade,

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

@ -97,7 +97,6 @@ inline LevelID getNodeQueryLevel(const MultiLevelPartition &partition,
const std::vector<std::size_t> &phantom_indices) const std::vector<std::size_t> &phantom_indices)
{ {
auto min_level = [&partition, node](const PhantomNode &phantom_node) { auto min_level = [&partition, node](const PhantomNode &phantom_node) {
const auto &forward_segment = phantom_node.forward_segment_id; const auto &forward_segment = phantom_node.forward_segment_id;
const auto forward_level = const auto forward_level =
forward_segment.enabled ? partition.GetHighestDifferentLevel(node, forward_segment.id) forward_segment.enabled ? partition.GetHighestDifferentLevel(node, forward_segment.id)
@ -391,21 +390,27 @@ UnpackedPath search(SearchEngineData<Algorithm> &engine_working_data,
EdgeWeight weight_upper_bound, EdgeWeight weight_upper_bound,
Args... args) Args... args)
{ {
if (forward_heap.Empty() || reverse_heap.Empty()) if (forward_heap.Empty() && reverse_heap.Empty())
{ {
return std::make_tuple(INVALID_EDGE_WEIGHT, std::vector<NodeID>(), std::vector<EdgeID>()); return std::make_tuple(INVALID_EDGE_WEIGHT, std::vector<NodeID>(), std::vector<EdgeID>());
} }
const auto &partition = facade.GetMultiLevelPartition(); const auto &partition = facade.GetMultiLevelPartition();
BOOST_ASSERT(!forward_heap.Empty() && forward_heap.MinKey() < INVALID_EDGE_WEIGHT); BOOST_ASSERT(forward_heap.Empty() || forward_heap.MinKey() < INVALID_EDGE_WEIGHT);
BOOST_ASSERT(!reverse_heap.Empty() && reverse_heap.MinKey() < INVALID_EDGE_WEIGHT); BOOST_ASSERT(reverse_heap.Empty() || reverse_heap.MinKey() < INVALID_EDGE_WEIGHT);
// run two-Target Dijkstra routing step. // run two-Target Dijkstra routing step.
NodeID middle = SPECIAL_NODEID; NodeID middle = SPECIAL_NODEID;
EdgeWeight weight = weight_upper_bound; EdgeWeight weight = weight_upper_bound;
EdgeWeight forward_heap_min = forward_heap.MinKey();
EdgeWeight reverse_heap_min = reverse_heap.MinKey(); EdgeWeight forward_heap_min = 0;
if (!forward_heap.Empty())
forward_heap_min = forward_heap.MinKey();
EdgeWeight reverse_heap_min = 0;
if (!reverse_heap.Empty())
reverse_heap_min = reverse_heap.MinKey();
while (forward_heap.Size() + reverse_heap.Size() > 0 && while (forward_heap.Size() + reverse_heap.Size() > 0 &&
forward_heap_min + reverse_heap_min < weight) forward_heap_min + reverse_heap_min < weight)
{ {
@ -657,11 +662,7 @@ double getNetworkDistance(SearchEngineData<Algorithm> &engine_working_data,
const PhantomNode &target_phantom, const PhantomNode &target_phantom,
EdgeWeight weight_upper_bound = INVALID_EDGE_WEIGHT) EdgeWeight weight_upper_bound = INVALID_EDGE_WEIGHT)
{ {
forward_heap.Clear();
reverse_heap.Clear();
const PhantomNodes phantom_nodes{source_phantom, target_phantom}; const PhantomNodes phantom_nodes{source_phantom, target_phantom};
insertNodesInHeaps(forward_heap, reverse_heap, phantom_nodes);
EdgeWeight weight = INVALID_EDGE_WEIGHT; EdgeWeight weight = INVALID_EDGE_WEIGHT;
std::vector<NodeID> unpacked_nodes; std::vector<NodeID> unpacked_nodes;
@ -684,16 +685,18 @@ double getNetworkDistance(SearchEngineData<Algorithm> &engine_working_data,
if (!unpacked_nodes.empty()) if (!unpacked_nodes.empty())
{ {
for (auto node_iter = unpacked_nodes.begin(); node_iter != std::prev(unpacked_nodes.end()); node_iter++) distance = std::accumulate(unpacked_nodes.begin(),
{ std::prev(unpacked_nodes.end()),
distance += computeEdgeDistance(facade, *node_iter); EdgeDistance{0},
} [&](const EdgeDistance distance, const auto node_id) {
return distance + computeEdgeDistance(facade, node_id);
});
} }
adjustPathDistanceToPhantomNodes( distance = adjustPathDistanceToPhantomNodes(
unpacked_nodes, phantom_nodes.source_phantom, phantom_nodes.target_phantom, distance); unpacked_nodes, phantom_nodes.source_phantom, phantom_nodes.target_phantom, distance);
return distance / 10.; return distance;
} }
} // namespace mld } // namespace mld

3
src/engine/routing_algorithms/many_to_many_ch.cpp
View File

@ -242,7 +242,8 @@ void calculateDistances(typename SearchEngineData<ch::Algorithm>::ManyToManyQuer
{ {
EdgeDistance annotation = EdgeDistance annotation =
ch::calculateEBGNodeAnnotations(facade, packed_leg.begin(), packed_leg.end()); ch::calculateEBGNodeAnnotations(facade, packed_leg.begin(), packed_leg.end());
adjustPathDistanceToPhantomNodes(packed_leg, source_phantom, target_phantom, annotation); annotation = adjustPathDistanceToPhantomNodes(
packed_leg, source_phantom, target_phantom, annotation);
distances_table[row_index * number_of_targets + column_index] = annotation; distances_table[row_index * number_of_targets + column_index] = annotation;
} }

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

@ -227,6 +227,9 @@ SubMatchingList mapMatching(SearchEngineData<Algorithm> &engine_working_data,
{ {
continue; continue;
} }
forward_heap.Clear();
const auto &source_phantom = prev_unbroken_timestamps_list[s].phantom_node;
insertSourceInHeap(forward_heap, source_phantom);
for (const auto s_prime : util::irange<std::size_t>(0UL, current_viterbi.size())) for (const auto s_prime : util::irange<std::size_t>(0UL, current_viterbi.size()))
{ {
@ -237,14 +240,17 @@ SubMatchingList mapMatching(SearchEngineData<Algorithm> &engine_working_data,
continue; continue;
} }
double network_distance = reverse_heap.Clear();
getNetworkDistance(engine_working_data, const auto &target_phantom = current_timestamps_list[s_prime].phantom_node;
facade, insertTargetInHeap(reverse_heap, target_phantom);
forward_heap,
reverse_heap, double network_distance = getNetworkDistance(engine_working_data,
prev_unbroken_timestamps_list[s].phantom_node, facade,
current_timestamps_list[s_prime].phantom_node, forward_heap,
weight_upper_bound); reverse_heap,
source_phantom,
target_phantom,
weight_upper_bound);
// get distance diff between loc1/2 and locs/s_prime // get distance diff between loc1/2 and locs/s_prime
const auto d_t = std::abs(network_distance - haversine_distance); const auto d_t = std::abs(network_distance - haversine_distance);

15
src/engine/routing_algorithms/routing_base.cpp
View File

@ -33,11 +33,12 @@ bool needsLoopBackwards(const PhantomNodes &phantoms)
return needsLoopBackwards(phantoms.source_phantom, phantoms.target_phantom); return needsLoopBackwards(phantoms.source_phantom, phantoms.target_phantom);
} }
void adjustPathDistanceToPhantomNodes(const std::vector<NodeID> &path, EdgeDistance adjustPathDistanceToPhantomNodes(const std::vector<NodeID> &path,
const PhantomNode &source_phantom, const PhantomNode &source_phantom,
const PhantomNode &target_phantom, const PhantomNode &target_phantom,
EdgeDistance &distance) const EdgeDistance uncorrected_distance)
{ {
EdgeDistance distance = uncorrected_distance;
if (!path.empty()) if (!path.empty())
{ {
@ -97,6 +98,12 @@ void adjustPathDistanceToPhantomNodes(const std::vector<NodeID> &path,
distance = target_phantom.GetReverseDistance() - source_phantom.GetReverseDistance(); distance = target_phantom.GetReverseDistance() - source_phantom.GetReverseDistance();
} }
} }
BOOST_ASSERT_MSG(distance >= 0 || distance > -1.0f,
"Distance correction generated negative number");
// guard against underflow errors caused by rounding
distance = std::max(EdgeDistance{0}, distance);
return distance;
} }
} // namespace routing_algorithms } // namespace routing_algorithms

47
src/engine/routing_algorithms/routing_base_ch.cpp
View File

@ -100,7 +100,7 @@ void search(SearchEngineData<Algorithm> & /*engine_working_data*/,
const PhantomNodes & /*phantom_nodes*/, const PhantomNodes & /*phantom_nodes*/,
const EdgeWeight weight_upper_bound) const EdgeWeight weight_upper_bound)
{ {
if (forward_heap.Empty() || reverse_heap.Empty()) if (forward_heap.Empty() && reverse_heap.Empty())
{ {
weight = INVALID_EDGE_WEIGHT; weight = INVALID_EDGE_WEIGHT;
return; return;
@ -110,10 +110,14 @@ void search(SearchEngineData<Algorithm> & /*engine_working_data*/,
weight = weight_upper_bound; weight = weight_upper_bound;
// get offset to account for offsets on phantom nodes on compressed edges // get offset to account for offsets on phantom nodes on compressed edges
const auto min_edge_offset = std::min(0, forward_heap.MinKey()); EdgeWeight min_edge_offset = 0;
BOOST_ASSERT(min_edge_offset <= 0); if (forward_heap.Size() > 0)
{
min_edge_offset = std::min(min_edge_offset, forward_heap.MinKey());
BOOST_ASSERT(min_edge_offset <= 0);
}
// we only every insert negative offsets for nodes in the forward heap // we only every insert negative offsets for nodes in the forward heap
BOOST_ASSERT(reverse_heap.MinKey() >= 0); BOOST_ASSERT(reverse_heap.Empty() || reverse_heap.MinKey() >= 0);
// run two-Target Dijkstra routing step. // run two-Target Dijkstra routing step.
while (0 < (forward_heap.Size() + reverse_heap.Size())) while (0 < (forward_heap.Size() + reverse_heap.Size()))
@ -176,11 +180,6 @@ double getNetworkDistance(SearchEngineData<Algorithm> &engine_working_data,
const PhantomNode &target_phantom, const PhantomNode &target_phantom,
EdgeWeight weight_upper_bound) EdgeWeight weight_upper_bound)
{ {
forward_heap.Clear();
reverse_heap.Clear();
insertNodesInHeaps(forward_heap, reverse_heap, {source_phantom, target_phantom});
EdgeWeight weight = INVALID_EDGE_WEIGHT; EdgeWeight weight = INVALID_EDGE_WEIGHT;
std::vector<NodeID> packed_path; std::vector<NodeID> packed_path;
search(engine_working_data, search(engine_working_data,
@ -199,8 +198,6 @@ double getNetworkDistance(SearchEngineData<Algorithm> &engine_working_data,
return std::numeric_limits<double>::max(); return std::numeric_limits<double>::max();
} }
BOOST_ASSERT(nodes_number > 0);
EdgeDistance distance = 0; EdgeDistance distance = 0;
std::vector<NodeID> unpacked_nodes; std::vector<NodeID> unpacked_nodes;
@ -208,24 +205,24 @@ double getNetworkDistance(SearchEngineData<Algorithm> &engine_working_data,
if (!packed_path.empty()) if (!packed_path.empty())
{ {
unpacked_nodes.push_back(packed_path.front()); unpacked_nodes.push_back(packed_path.front());
unpackPath(facade, unpackPath(
packed_path.begin(), facade, packed_path.begin(), packed_path.end(), [&](const auto &edge, const auto &) {
packed_path.end(), BOOST_ASSERT(edge.first == unpacked_nodes.back());
[&](std::pair<NodeID, NodeID> &edge, const auto &) { unpacked_nodes.push_back(edge.second);
BOOST_ASSERT(edge.first == unpacked_nodes.back()); });
unpacked_nodes.push_back(edge.second);
});
for (auto node_iter = unpacked_nodes.begin(); node_iter != std::prev(unpacked_nodes.end()); distance = std::accumulate(unpacked_nodes.begin(),
node_iter++) std::prev(unpacked_nodes.end()),
{ EdgeDistance{0},
distance += computeEdgeDistance(facade, *node_iter); [&](const EdgeDistance distance, const auto node_id) {
} return distance + computeEdgeDistance(facade, node_id);
});
} }
adjustPathDistanceToPhantomNodes(unpacked_nodes, source_phantom, target_phantom, distance); distance =
adjustPathDistanceToPhantomNodes(unpacked_nodes, source_phantom, target_phantom, distance);
return distance / 10.; return distance;
} }
} // namespace ch } // namespace ch

2
src/util/coordinate_calculation.cpp
View File

@ -25,7 +25,7 @@ namespace
// earth radius varies between 6,356.750-6,378.135 km (3,949.901-3,963.189mi) // earth radius varies between 6,356.750-6,378.135 km (3,949.901-3,963.189mi)
// The IUGG value for the equatorial radius is 6378.137 km (3963.19 miles) // The IUGG value for the equatorial radius is 6378.137 km (3963.19 miles)
const constexpr long double EARTH_RADIUS = 6372797.560856; const constexpr double EARTH_RADIUS = 6372797.560856;
class CheapRulerContainer class CheapRulerContainer
{ {