Revert "Improve speed of Map Matching" (#5196)

* Revert "Update changelog" This reverts commit 9b779c704f. * Revert "Fix formating" This reverts commit 5bd7d04fe3. * Revert "Fix bug in computation of distance offset for phantom node" This reverts commit 0f78f7b2cc. * Revert "Adjust text cases for flightly different matching due to rounding" This reverts commit 8473be69d2. * Revert "Round network distance to deci-meter to retain previous behavior" This reverts commit c0124f7d77. * Revert "Preserve heap state in map matching" This reverts commit b630b4e32a. * Revert "Use distance functions from many to many" This reverts commit 89fabc1b9c. * Revert "Use FCC algorithm for map matching distance calculation" This reverts commit a649a8a5cf.
2018-09-06 12:05:28 -04:00
parent 5476f6ab27
commit 5597415f28
11 changed files with 214 additions and 289 deletions
@@ -44,19 +44,50 @@ bool needsLoopBackwards(const PhantomNode &source_phantom, const PhantomNode &ta
 bool needsLoopForward(const PhantomNodes &phantoms);
 bool needsLoopBackwards(const PhantomNodes &phantoms);

-namespace detail
+template <typename Heap>
+void insertNodesInHeaps(Heap &forward_heap, Heap &reverse_heap, const PhantomNodes &nodes)
 {
-template <typename Algorithm>
-void insertSourceInHeap(typename SearchEngineData<Algorithm>::ManyToManyQueryHeap &heap,
-                        const PhantomNode &phantom_node)
+    const auto &source = nodes.source_phantom;
+    if (source.IsValidForwardSource())
+    {
+        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.IsValidForwardTarget())
+    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.IsValidReverseTarget())
+    if (phantom_node.IsValidReverseSource())
    {
        heap.Insert(phantom_node.reverse_segment_id.id,
                    -phantom_node.GetReverseWeightPlusOffset(),
@@ -64,9 +95,8 @@ void insertSourceInHeap(typename SearchEngineData<Algorithm>::ManyToManyQueryHea
    }
 }

-template <typename Algorithm>
-void insertTargetInHeap(typename SearchEngineData<Algorithm>::ManyToManyQueryHeap &heap,
-                        const PhantomNode &phantom_node)
+template <typename ManyToManyQueryHeap>
+void insertTargetInHeap(ManyToManyQueryHeap &heap, const PhantomNode &phantom_node)
 {
    if (phantom_node.IsValidForwardTarget())
    {
@@ -82,109 +112,6 @@ void insertTargetInHeap(typename SearchEngineData<Algorithm>::ManyToManyQueryHea
    }
 }

-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>
 void annotatePath(const FacadeT &facade,
                  const PhantomNodes &phantom_node_pair,
@@ -394,10 +321,58 @@ void annotatePath(const FacadeT &facade,
    }
 }

-EdgeDistance adjustPathDistanceToPhantomNodes(const std::vector<NodeID> &path,
-                                              const PhantomNode &source_phantom,
-                                              const PhantomNode &target_phantom,
-                                              const EdgeDistance distance);
+template <typename Algorithm>
+double getPathDistance(const DataFacade<Algorithm> &facade,
+                       const std::vector<PathData> unpacked_path,
+                       const PhantomNode &source_phantom,
+                       const PhantomNode &target_phantom)
+{
+    using util::coordinate_calculation::detail::DEGREE_TO_RAD;
+    using util::coordinate_calculation::detail::EARTH_RADIUS;
+
+    double distance = 0;
+    double prev_lat =
+        static_cast<double>(util::toFloating(source_phantom.location.lat)) * DEGREE_TO_RAD;
+    double prev_lon =
+        static_cast<double>(util::toFloating(source_phantom.location.lon)) * DEGREE_TO_RAD;
+    double prev_cos = std::cos(prev_lat);
+    for (const auto &p : unpacked_path)
+    {
+        const auto current_coordinate = facade.GetCoordinateOfNode(p.turn_via_node);
+
+        const double current_lat =
+            static_cast<double>(util::toFloating(current_coordinate.lat)) * DEGREE_TO_RAD;
+        const double current_lon =
+            static_cast<double>(util::toFloating(current_coordinate.lon)) * DEGREE_TO_RAD;
+        const double current_cos = std::cos(current_lat);
+
+        const double sin_dlon = std::sin((prev_lon - current_lon) / 2.0);
+        const double sin_dlat = std::sin((prev_lat - current_lat) / 2.0);
+
+        const double aharv = sin_dlat * sin_dlat + prev_cos * current_cos * sin_dlon * sin_dlon;
+        const double charv = 2. * std::atan2(std::sqrt(aharv), std::sqrt(1.0 - aharv));
+        distance += EARTH_RADIUS * charv;
+
+        prev_lat = current_lat;
+        prev_lon = current_lon;
+        prev_cos = current_cos;
+    }
+
+    const double current_lat =
+        static_cast<double>(util::toFloating(target_phantom.location.lat)) * DEGREE_TO_RAD;
+    const double current_lon =
+        static_cast<double>(util::toFloating(target_phantom.location.lon)) * DEGREE_TO_RAD;
+    const double current_cos = std::cos(current_lat);
+
+    const double sin_dlon = std::sin((prev_lon - current_lon) / 2.0);
+    const double sin_dlat = std::sin((prev_lat - current_lat) / 2.0);
+
+    const double aharv = sin_dlat * sin_dlat + prev_cos * current_cos * sin_dlon * sin_dlon;
+    const double charv = 2. * std::atan2(std::sqrt(aharv), std::sqrt(1.0 - aharv));
+    distance += EARTH_RADIUS * charv;
+
+    return distance;
+}

 template <typename AlgorithmT>
 InternalRouteResult extractRoute(const DataFacade<AlgorithmT> &facade,
@@ -97,6 +97,7 @@ inline LevelID getNodeQueryLevel(const MultiLevelPartition &partition,
                                 const std::vector<std::size_t> &phantom_indices)
 {
    auto min_level = [&partition, node](const PhantomNode &phantom_node) {
+
        const auto &forward_segment = phantom_node.forward_segment_id;
        const auto forward_level =
            forward_segment.enabled ? partition.GetHighestDifferentLevel(node, forward_segment.id)
@@ -119,7 +120,7 @@ inline LevelID getNodeQueryLevel(const MultiLevelPartition &partition,
    }
    return result;
 }
-} // namespace
+}

 // Heaps only record for each node its predecessor ("parent") on the shortest path.
 // For re-constructing the actual path we need to trace back all parent "pointers".
@@ -390,27 +391,21 @@ UnpackedPath search(SearchEngineData<Algorithm> &engine_working_data,
                    EdgeWeight weight_upper_bound,
                    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>());
    }

    const auto &partition = facade.GetMultiLevelPartition();

-    BOOST_ASSERT(forward_heap.Empty() || forward_heap.MinKey() < INVALID_EDGE_WEIGHT);
-    BOOST_ASSERT(reverse_heap.Empty() || reverse_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);

    // run two-Target Dijkstra routing step.
    NodeID middle = SPECIAL_NODEID;
    EdgeWeight weight = weight_upper_bound;
-
-    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();
-
+    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)
    {
@@ -662,7 +657,11 @@ double getNetworkDistance(SearchEngineData<Algorithm> &engine_working_data,
                          const PhantomNode &target_phantom,
                          EdgeWeight weight_upper_bound = INVALID_EDGE_WEIGHT)
 {
+    forward_heap.Clear();
+    reverse_heap.Clear();
+
    const PhantomNodes phantom_nodes{source_phantom, target_phantom};
+    insertNodesInHeaps(forward_heap, reverse_heap, phantom_nodes);

    EdgeWeight weight = INVALID_EDGE_WEIGHT;
    std::vector<NodeID> unpacked_nodes;
@@ -681,22 +680,11 @@ double getNetworkDistance(SearchEngineData<Algorithm> &engine_working_data,
        return std::numeric_limits<double>::max();
    }

-    EdgeDistance distance = 0;
+    std::vector<PathData> unpacked_path;

-    if (!unpacked_nodes.empty())
-    {
-        distance = std::accumulate(unpacked_nodes.begin(),
-                                   std::prev(unpacked_nodes.end()),
-                                   EdgeDistance{0},
-                                   [&](const EdgeDistance distance, const auto node_id) {
-                                       return distance + computeEdgeDistance(facade, node_id);
-                                   });
-    }
+    annotatePath(facade, phantom_nodes, unpacked_nodes, unpacked_edges, unpacked_path);

-    distance = adjustPathDistanceToPhantomNodes(
-        unpacked_nodes, phantom_nodes.source_phantom, phantom_nodes.target_phantom, distance);
-
-    return distance;
+    return getPathDistance(facade, unpacked_path, source_phantom, target_phantom);
 }

 } // namespace mld